地下電纜溝智能巡檢機(jī)器人導(dǎo)航避障技術(shù)研究

肖慧慧，樊紹勝，何 瑛，胡曉東

（1. 復(fù)雜環(huán)境特種機(jī)器人控制技術(shù)與裝備湖南省工程研究中心，湖南 湘潭 411104；2. 湖南理工職業(yè)技術(shù)學(xué)院，湖南 湘潭 411104；3. 長(zhǎng)沙理工大學(xué) 電氣與信息工程學(xué)院，湖南 長(zhǎng)沙 410014；4. 電力機(jī)器人湖南省重點(diǎn)實(shí)驗(yàn)室，湖南 長(zhǎng)沙 410014；5. 中車株洲電力機(jī)車研究所有限公司，湖南 株洲 412001）

0 引言

隨著我國(guó)經(jīng)濟(jì)的增長(zhǎng)以及人民生活水平的不斷改善，工業(yè)及城市居民用電量持續(xù)增加，電力系統(tǒng)供配電電網(wǎng)處于不斷增容擴(kuò)建狀態(tài)。電纜溝是電力系統(tǒng)必不可少的基礎(chǔ)設(shè)施。地下電纜溝一般比較窄小，對(duì)其中電纜的檢測(cè)目前還多以人工為主。檢修人員攜帶紅外測(cè)溫儀、手電筒和相機(jī)等工具在溝道中行走巡檢很不方便，且溝內(nèi)的空氣質(zhì)量差，可能存在有毒、有害氣體［1］，這不僅給電力運(yùn)維人員定期巡檢帶來了很大困難，而且巡檢過程中很難及時(shí)發(fā)現(xiàn)電纜火災(zāi)或運(yùn)行過程中的安全隱患。對(duì)此，一種智能巡檢機(jī)器人被研制出來并在不斷被改進(jìn)，用于代替電力巡檢人員高效、安全地完成地下電纜巡檢工作［2］。近年來，電力系統(tǒng)巡檢機(jī)器人技術(shù)經(jīng)過快速發(fā)展，在市場(chǎng)上已有功能較為完善的產(chǎn)品，主要集中在地面設(shè)備巡檢的應(yīng)用；而針對(duì)地下電纜溝巡檢的機(jī)器人研究較少，尤其是在復(fù)雜的地下電纜溝環(huán)境下，巡檢機(jī)器人的導(dǎo)航精度、故障檢測(cè)的可靠性以及環(huán)境適應(yīng)性仍需提高。

地下電纜溝空間狹窄，環(huán)境特殊復(fù)雜，操作人員手動(dòng)控制巡檢機(jī)器人在電纜溝中行走時(shí)較易碰撞到兩側(cè)的墻壁或者是電纜的橫擔(dān)，導(dǎo)致機(jī)器人損壞。導(dǎo)航定位是地下電纜巡檢機(jī)器人能夠?qū)崿F(xiàn)自主運(yùn)行的關(guān)鍵技術(shù)之一，其主要是通過探測(cè)機(jī)器人的四周環(huán)境進(jìn)行定位，識(shí)別檢測(cè)出行駛路徑，再沿著路徑自主行駛，以完成相應(yīng)的巡檢任務(wù)。本文以雙驅(qū)動(dòng)履帶式電纜溝智能巡檢機(jī)器人為載體，采用基于改進(jìn)的RBPF（Rao‐Blackwellized particle filter）‐SLAM（simultaneous localization and mapping）算法對(duì)電纜溝智能機(jī)器人進(jìn)行定位并同時(shí)構(gòu)建地圖，在此基礎(chǔ)上設(shè)計(jì)自主導(dǎo)航避障控制策略，實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航定位，使機(jī)器人在巡檢時(shí)不僅能避免碰撞，還能進(jìn)一步提高巡檢效率［3］。

1 電纜溝智能巡檢機(jī)器人系統(tǒng)簡(jiǎn)介

地下電纜溝內(nèi)空間比較狹窄，可能存在磚頭、施工時(shí)遺留的線纜、動(dòng)物尸體等雜物，電纜溝巡檢機(jī)器人在進(jìn)行巡檢時(shí)需要具備以下能力：在電纜溝內(nèi)直線行走且能靈活轉(zhuǎn)向，具備足夠的爬坡及跨越障礙的能力。為此，本文的智能巡檢機(jī)器人車體采取雙電機(jī)履帶驅(qū)動(dòng)車體結(jié)構(gòu)，其由機(jī)械底盤、電源模塊、控制電路單元、驅(qū)動(dòng)模塊、電機(jī)、通信模塊、圖像采集模塊以及各類傳感器模塊等組成，實(shí)物如圖1所示。車體攜帶旋轉(zhuǎn)云臺(tái)和紅外、高清雙視頻攝像機(jī)，用于監(jiān)測(cè)電纜溝內(nèi)的溫度分布、電纜及電纜接頭部分的運(yùn)行狀態(tài)；搭載陀螺儀、激光雷達(dá)等，用于感知運(yùn)動(dòng)姿態(tài)，實(shí)現(xiàn)對(duì)車體當(dāng)前行進(jìn)方向等信息的獲取。

圖1 雙驅(qū)動(dòng)履帶式機(jī)器人實(shí)物Fig. 1 Dual‐drive‐crawler robot material object

智能巡檢機(jī)器人在地下電纜溝執(zhí)行巡檢任務(wù)時(shí)，依托本體所搭載的圖像采集模塊、傳感器模塊獲取數(shù)據(jù)［4］，并通過相應(yīng)的導(dǎo)航定位算法、運(yùn)動(dòng)控制算法、多傳感器融合狀態(tài)評(píng)估算法以及電纜溝三維建模算法等來對(duì)數(shù)據(jù)進(jìn)行分析、計(jì)算［5］，從而獲得對(duì)機(jī)器人本體控制以及對(duì)電纜溝內(nèi)環(huán)境的感知，完成自主導(dǎo)航避障、電纜運(yùn)行狀態(tài)檢測(cè)、電纜溝內(nèi)有害氣體狀態(tài)及溫濕度狀態(tài)檢測(cè)等任務(wù)。

巡檢機(jī)器人車體基本參數(shù)如下：長(zhǎng)寬高分別為500 mm、300 mm、120 mm，底盤自重8.45 kg，最大負(fù)重30 kg。根據(jù)車體運(yùn)動(dòng)方程求解得知機(jī)器人總驅(qū)動(dòng)能力要求為58.5 W，因此選用功率不低于30 W 的驅(qū)動(dòng)電機(jī)。雙電機(jī)通過差速控制對(duì)車體的轉(zhuǎn)向和掉頭等運(yùn)動(dòng)姿態(tài)進(jìn)行控制。

2 電纜溝智能巡檢機(jī)器人導(dǎo)航定位

本文從適用于地下電纜溝內(nèi)部環(huán)境的角度來設(shè)計(jì)智能巡檢機(jī)器人的導(dǎo)航定位方案。將機(jī)器人看成是一個(gè)整體來建立運(yùn)動(dòng)模型。為實(shí)現(xiàn)自主導(dǎo)航避障，提高導(dǎo)航定位精度，對(duì)RBPF‐SLAM算法進(jìn)行了改進(jìn)，使機(jī)器人在移動(dòng)時(shí)不僅能描繪出完整的地圖，而且在構(gòu)建地圖的同時(shí)又能在地圖中找到機(jī)器人本身的位置。

2.1 機(jī)器人運(yùn)動(dòng)模型

在建立電纜溝機(jī)器人運(yùn)動(dòng)模型時(shí)，為簡(jiǎn)化電纜溝機(jī)器人結(jié)構(gòu)，將機(jī)器人看作一個(gè)整體，根據(jù)RPY（Roll翻滾角，Pitch俯仰角，Yaw偏航角）變換方法建立地下電纜溝智能巡檢機(jī)器人大地坐標(biāo)系和車體坐標(biāo)系之間的聯(lián)系，如圖2所示。OXYZ為大地坐標(biāo)系，主要用來描述電纜溝智能巡檢機(jī)器人整體所在位置信息，機(jī)器人的運(yùn)動(dòng)起點(diǎn)為大地坐標(biāo)系的原點(diǎn)；OrXrYrZr為車體坐標(biāo)系，其隨著機(jī)器人的移動(dòng)而移動(dòng)，機(jī)器人當(dāng)前時(shí)刻所處位置的幾何中心點(diǎn)為車體坐標(biāo)系的原點(diǎn)。車體坐標(biāo)系 與大地坐標(biāo)的旋轉(zhuǎn)矩陣關(guān)系如式（1）所示［6］。

圖2 機(jī)器人坐標(biāo)系Fig. 2 Robot coordinate system

式中：α——偏航角；β——俯仰角；γ——翻滾角。

機(jī)器人在地下電纜溝巡檢運(yùn)動(dòng)時(shí)近似為理想狀態(tài)的水平面運(yùn)動(dòng)，機(jī)器人的俯仰角和翻滾角可以忽略不計(jì)，只需要考慮前進(jìn)時(shí)的偏航角，則機(jī)器人的位姿信息可簡(jiǎn)化為用(x，y，α)來表示［7］。

在圖2 的二維平面XOY中，車體坐標(biāo)系相對(duì)大地坐標(biāo)系繞Z軸旋轉(zhuǎn)了α角度，規(guī)定逆時(shí)針方向?yàn)檎?，?jiǎn)化后的旋轉(zhuǎn)矩陣如下：

則大地坐標(biāo)系和車體坐標(biāo)系的轉(zhuǎn)換關(guān)系為

機(jī)器人在運(yùn)動(dòng)過程中既有平移運(yùn)動(dòng)，也存在旋轉(zhuǎn)運(yùn)動(dòng)。為建立相對(duì)精確的機(jī)器人運(yùn)動(dòng)學(xué)模型，假設(shè)t到（t+1）時(shí)間段內(nèi)機(jī)器人在進(jìn)行如圖3 所示的弧線移動(dòng)［8］，在t時(shí)刻，機(jī)器人的位姿為。在大地坐標(biāo)系下，移動(dòng)后機(jī)器人的位姿增量為

圖3 機(jī)器人弧線運(yùn)動(dòng)圖Fig. 3 Arc motion of robot

式中：vt——機(jī)器人的平移線速度；ωt——機(jī)器人的旋轉(zhuǎn)角速度。

根據(jù)式（4），可以得到大地坐標(biāo)系下的機(jī)器人運(yùn)動(dòng)數(shù)學(xué)模型［9］：

2.2 基于改進(jìn)RBPF-SLAM算法的定位與地圖構(gòu)建

2.2.1 同時(shí)進(jìn)行定位與地圖構(gòu)建

在陌生的地下電纜溝環(huán)境中，智能巡檢機(jī)器人通過在車載傳感設(shè)備對(duì)周圍環(huán)境進(jìn)行觀測(cè)，采集路標(biāo)的觀測(cè)值數(shù)據(jù)并構(gòu)建局部環(huán)境地圖［10］。隨著智能巡檢機(jī)器人位姿的不斷變化，傳感器采集的觀測(cè)信息也不斷地更新并對(duì)機(jī)器人的位姿進(jìn)行校正，以實(shí)現(xiàn)機(jī)器人在環(huán)境地圖中的自定位；同時(shí)，根據(jù)不同時(shí)刻構(gòu)建的局部環(huán)境地圖對(duì)全局地圖進(jìn)行更新［11］。圖4示出巡檢機(jī)器人的同時(shí)定位與地圖構(gòu)建圖。圖中，xt為t時(shí)刻地下電纜溝智能巡檢機(jī)器人的位姿向量xt=[xt yt βt]T，x0為機(jī)器人初始位姿，則x0：t=[x0x1…xt]；ut為地下電纜溝智能巡檢機(jī)器人從（t‐1）時(shí)刻到t時(shí)刻之間的運(yùn)動(dòng)控制量，則u1：t=(u1，???，ut)；mi為在全局地圖中第i個(gè)路標(biāo)位姿向量，該路標(biāo)假定為固定不變的路標(biāo)，位姿信息不隨時(shí)間變化，環(huán)境地圖可由mi=(m0，m1，???，mn)表示；zit為機(jī)器人在t時(shí)刻掃描環(huán)境信息時(shí)獲取的第i個(gè)路標(biāo)的位姿觀測(cè)值，全部路標(biāo)的位姿觀測(cè)值可表示為z0：t=(z0，z1，???，zt)。

圖4 同時(shí)定位與地圖構(gòu)建圖Fig. 4 Simultaneous positioning and map construction

2.2.2 改進(jìn)RBPF‐SLAM算法

常規(guī)RBPF‐SLAM 算法通過獲取機(jī)器人所搭載的外部傳感器信息數(shù)據(jù)和里程計(jì)數(shù)據(jù)，預(yù)估機(jī)器人的運(yùn)動(dòng)軌跡與環(huán)境地圖聯(lián)合的后驗(yàn)概率密度；然后基于Rao‐Blackwellised 粒子濾波器，估計(jì)機(jī)器人位置姿態(tài)，構(gòu)建相應(yīng)的柵格地圖。算法步驟如下：采樣→權(quán)重計(jì)算→重采樣→地圖更新。在計(jì)算過程中，一般選取里程計(jì)運(yùn)動(dòng)模型作為提議分布。這樣雖然計(jì)算簡(jiǎn)單，但運(yùn)算速度比較慢，在迭代過程中，很多權(quán)重小的粒子會(huì)被舍棄，權(quán)值嚴(yán)重退化。因此，常規(guī)RBPF‐SLAM 算法在進(jìn)行定位與地圖構(gòu)建時(shí)精度較低［12］。

在采樣的過程中，需要根據(jù)提議分布對(duì)前一時(shí)刻的粒子進(jìn)行采樣，且提議分布的精度直接影響采樣的效果。本文通過將環(huán)境感知傳感器（即激光雷達(dá)）的最新測(cè)量數(shù)據(jù)融入提議分布，同時(shí)在重采樣過程中引入自適應(yīng)重要性采樣算法，對(duì)RBPF‐SLAM 算法進(jìn)行改進(jìn)。融入了激光雷達(dá)數(shù)據(jù)的優(yōu)化提議分布如下：

在自適應(yīng)重要性重采樣時(shí)，對(duì)粒子權(quán)重進(jìn)行歸一化處理，即重采樣時(shí)需要獲取權(quán)值差不多的粒子。通過式（8）即可獲得歸一化粒子的重要性權(quán)值［2］。

采用改進(jìn)后的RBPF‐SLAM 算法，電纜溝智能巡檢機(jī)器人的定位與構(gòu)圖精度都得到了提高。圖5 示出由RBPF‐SLAM 算法構(gòu)建的柵格地圖，其中圖5（a）為真實(shí)工作環(huán)境，圖5（b）為通過改進(jìn)RBPF‐SLAM 算法構(gòu)建的柵格地圖。可以看出，采用本算法構(gòu)建的地圖邊緣清晰，沒有重疊現(xiàn)象，較好地反映了真實(shí)的運(yùn)行環(huán)境。

圖5 RBPF‐SLAM 算法構(gòu)建的柵格地圖Fig. 5 Grid map constructed by RBPF‐SLAM algorithm

3 機(jī)器人控制系統(tǒng)

在電纜溝中，智能巡檢機(jī)器人為了完成相關(guān)巡檢任務(wù)，不僅需要具備自主導(dǎo)航定位的功能，而且要求在行進(jìn)過程中，能夠?qū)?dǎo)航線路中出現(xiàn)的各種障礙物進(jìn)行規(guī)避，即具備主動(dòng)避障的功能［13］。電纜溝智能巡檢機(jī)器人控制系統(tǒng)結(jié)構(gòu)如圖6所示。

圖6 機(jī)器人控制系統(tǒng)結(jié)構(gòu)Fig.6 Structure of robot control system

系統(tǒng)采用多CPU協(xié)同工作的架構(gòu)，以提高系統(tǒng)的數(shù)據(jù)處理能力。機(jī)器人的核心處理模塊采用的是英偉達(dá)的核心主控板，其能夠提供強(qiáng)大的圖像處理能力。搭載有專為機(jī)器人控制而設(shè)計(jì)的ROS操作系統(tǒng)，以提供完成自主導(dǎo)航算法的系統(tǒng)平臺(tái)。巡檢機(jī)器人安裝有激光雷達(dá)，其直接接入到英偉達(dá)主控板上，為機(jī)器人提供定位信息。紅外成像儀、高清攝像頭以及深度相機(jī)等視覺傳感器也被直接接入英偉達(dá)主控板，由其對(duì)所采集的圖像進(jìn)行處理，實(shí)現(xiàn)對(duì)紅外、高清圖像的信息融合以及對(duì)深度相機(jī)所反饋的深度信息的重組，以構(gòu)建電纜溝隧道的三維立體模型，用于準(zhǔn)確判別特定電纜走向，為機(jī)器人在電纜溝內(nèi)的運(yùn)動(dòng)控制提供基礎(chǔ)［14］。

系統(tǒng)嵌入式子處理器模塊用于實(shí)現(xiàn)對(duì)機(jī)器人的運(yùn)動(dòng)控制，其搭建了陀螺儀，以獲得機(jī)器人的位姿信息；通過對(duì)機(jī)器人驅(qū)動(dòng)電機(jī)的控制，實(shí)現(xiàn)了對(duì)機(jī)器人前進(jìn)、后退、轉(zhuǎn)彎以及掉頭等運(yùn)動(dòng)姿態(tài)的控制。另外，通過控制搭載紅外、高清攝像頭的云臺(tái)，實(shí)現(xiàn)了機(jī)器人對(duì)電纜溝隧道多方位、立體的視覺觀測(cè)［15］。

4 實(shí)驗(yàn)結(jié)果分析

為驗(yàn)證機(jī)器人性能是否滿足項(xiàng)目設(shè)計(jì)要求，將電纜溝智能巡檢機(jī)器人放入某電力公司電纜溝現(xiàn)場(chǎng)進(jìn)行導(dǎo)航避障、越障等方面性能測(cè)試，并人為設(shè)置了磚塊、PVC管、撬棍等障礙物。通過1.5 h的反復(fù)測(cè)試，得到機(jī)器人的導(dǎo)航定位誤差小于20 mm、最大越障高度達(dá)90 mm的實(shí)驗(yàn)結(jié)果，表明其滿足電纜溝巡檢要求。

4.1 巡檢機(jī)器人導(dǎo)航避障能力測(cè)試

電纜溝內(nèi)部有一些較高的障礙物，機(jī)器人無法直接翻越通過。這種情況下，就需要巡檢機(jī)器人具備足夠的避障能力，能夠通過路徑規(guī)劃，自主繞開障礙物通行。

圖7示出巡檢機(jī)器人導(dǎo)航避障能力測(cè)試實(shí)景。實(shí)驗(yàn)過程中，在電纜溝現(xiàn)場(chǎng)布置了豎立的磚塊，以此來模擬較高的障礙物，通過操作終端給機(jī)器人下發(fā)巡檢的線路點(diǎn)，由機(jī)器人自主進(jìn)行導(dǎo)航行進(jìn)。機(jī)器人在行進(jìn)過程中，能夠有效地發(fā)現(xiàn)障礙物并對(duì)路徑進(jìn)行及時(shí)的優(yōu)化調(diào)整。電纜溝內(nèi)部空間狹小，機(jī)器人避障路線沒有過多的空間。在實(shí)驗(yàn)中，機(jī)器人仍然能對(duì)路線進(jìn)行非常精準(zhǔn)的規(guī)劃，在繞開障礙物時(shí)，與障礙物之間保持了非常小的距離，從而以最小的空間代價(jià)完成了避障作業(yè)。結(jié)果表明，電纜溝智能巡檢機(jī)器人的自主導(dǎo)航避障算法在保證避障成功的同時(shí)，因?yàn)樽陨眢w積小、不占周邊空間、充分壓縮了對(duì)周邊空間的占用率，保障了在狹小電纜溝空間內(nèi)的通過性。

圖7 機(jī)器人導(dǎo)航避障實(shí)驗(yàn)Fig.7 Robot navigation obstacle avoidance test

4.2 巡檢機(jī)器人越障能力測(cè)試

電纜溝內(nèi)部地面環(huán)境復(fù)雜，通常存在大量的泥沙、磚塊、線纜甚至動(dòng)物尸體等障礙物。巡檢機(jī)器人必須具備足夠的通過能力，才能在如此復(fù)雜的地面環(huán)境下執(zhí)行巡檢任務(wù)。圖8示出巡檢機(jī)器人越障實(shí)驗(yàn)實(shí)景。

圖8 機(jī)器人越障實(shí)驗(yàn)Fig.8 Robot obstacle climbing test

現(xiàn)場(chǎng)在電纜溝內(nèi)模擬了各種類型的障礙，以檢驗(yàn)巡檢機(jī)器人的通過能力。實(shí)驗(yàn)障礙對(duì)象包括有電纜溝地面埋設(shè)的PVC 管、橫向擺放的2 塊磚塊以及斜向放置的直徑約30 mm 的撬棍等。實(shí)驗(yàn)過程中，巡檢機(jī)器人能夠輕易地跨過PVC 管；面對(duì)磚塊時(shí)，通過調(diào)快行進(jìn)速度，巡檢機(jī)器人能輕易地跨過磚塊，實(shí)現(xiàn)越障；而在經(jīng)過撬棍時(shí)，通過調(diào)整機(jī)器人的行進(jìn)角度，最終也成功地跨越了撬棍障礙，實(shí)現(xiàn)了越障。測(cè)試結(jié)果表明，巡檢機(jī)器人具備足夠的越障能力，能夠應(yīng)對(duì)電纜溝內(nèi)部大部分障礙情況；即使面對(duì)金屬撬棍這類特殊障礙物，也能夠通過調(diào)整行進(jìn)角度的方法實(shí)現(xiàn)越障，說明該巡檢機(jī)器人有能力完全應(yīng)對(duì)電纜溝內(nèi)的地形環(huán)境。

5 結(jié)語

本文通過建立電纜溝智能巡檢機(jī)器人運(yùn)動(dòng)模型，采用改進(jìn)的RBPF‐SLAM算法對(duì)電纜溝智能巡檢機(jī)器人同時(shí)進(jìn)行定位與地圖構(gòu)建，設(shè)計(jì)了機(jī)器人控制系統(tǒng)，實(shí)現(xiàn)了電纜溝智能巡檢機(jī)器人在陰暗狹窄、內(nèi)部復(fù)雜環(huán)境下的自主導(dǎo)航定位。實(shí)驗(yàn)結(jié)果表明，地下電纜溝智能巡檢機(jī)器人利用改進(jìn)的RBPF‐SLAM算法構(gòu)建環(huán)境地圖是可行的；對(duì)于電纜溝內(nèi)的地形環(huán)境，機(jī)器人通過多CPU 協(xié)同進(jìn)行控制，具備足夠的越障、避障通行能力，滿足地下電纜溝內(nèi)巡檢任務(wù)需求。

受時(shí)間方面的限制，在狹小的電纜溝內(nèi)，當(dāng)輸電線纜較多或環(huán)境較為極端時(shí)，該機(jī)器人的通過性還有待進(jìn)一步驗(yàn)證。后續(xù)將進(jìn)一步優(yōu)化機(jī)器人結(jié)構(gòu)，提高機(jī)器人在不同電纜溝情況下運(yùn)行的適應(yīng)性，推動(dòng)其向?qū)嵱没a(chǎn)品化的階段邁進(jìn)，為電力系統(tǒng)的穩(wěn)定可靠運(yùn)行提供有力支撐。