換流站巡檢四足機器人建圖定位系統優化

王旋, 侯賀偉, 富銀芳, 胡雪亮, 張時, 張虞旭駒, 周燕鑫, 姚恒燕

(1.國網浙江省電力有限公司超高壓分公司, 杭州 311100; 2.杭州云深處科技有限公司, 杭州 310030)

由于人工智能技術研究的突破,智能巡檢機器人在一般變電站環境中已經得到較為廣泛的應用[1-2]。這些應用加快了變電站智能化無人值守進程,解放了人力資源[3]。然而,對于超高壓換流站場景,存在環境復雜惡劣、設備類型多、數量大等特點,需要跨越樓梯以及狹窄區域場景,巡檢的設備復雜煩瑣、檢查點密集。傳統人工巡檢模式巡檢效率低、運維成本高,無法達到巡檢工作基本要求[4]。常規的巡檢機器人一方面受限于運動方式,另一方面在多樓層樓梯場景以及狹窄區域場景建圖定位較為困難,很難部署在換流站或復雜地形環境中[5]。因此研究一種智能機器人在換流站復雜環境中的建圖定位技術對于換流站無人巡檢發展具有重要意義。

機器人在換流站中所處的環境往往是未知、難以預測的,要實現機器人在換流站中的自主導航巡檢,關鍵是要實現機器人在換流站中精確的建圖定位[6]。其中建圖的常用方式是采用實時定位與地圖構建(simultaneous localization and mapping,SLAM)技術,SLAM技術能夠實現移動機器人在未知環境中的自我運動估計,同時建立周圍環境地圖[7-8]。現有的SLAM算法依據主導傳感器大致可以分為視覺SLAM與激光SLAM兩大類,激光SLAM建圖直觀、精度高、不受光照變化影響、不存在尺度漂移現象,同時無需預先布置場景,可融合多傳感器生成便于導航的環境地圖[9-10]。以激光SLAM技術構建變電站點云圖已有一些研究進展,其中,尹政等[11]分析對比了目前幾種主流激光SLAM算法,以Cartographer SLAM作為研究基礎,通過高精度校準方法標定兩臺2D激光儀的位姿關系,并對兩組數據進行優化融合以獲得高精度的三維數據,從而實現達到一定建圖精度的新系統。魯錦濤等[12]使用改進的點云匹配算法解決了變電站中相似環境誤匹配的問題,提高了地圖的可靠性以及可視性。然而上述研究中都沒有涉及換流站中多樓層環境的點云圖建立以及完全解決狹窄通道相似誤匹配問題。在定位方面,巡檢機器人定位方式大多采用有軌方式或全球定位系統[13],有軌方式成本較高,全球定位系統定位方式不能在室內以及有遮擋的區域進行定位,因此需要一種更為有效的定位方式,王鵬程等[14]通過Hough變換算法和最小二乘法算法將雷達點云數據擬合成直線,建立環境地圖模型,應用擴展卡爾曼濾波算法結合點云數據和位姿數據進行實時定位和預測走向,但單獨依靠激光點云匹配得到位姿不夠魯棒,需要融合其他傳感器來提高定位的穩定性。

現通過分析換流站巡檢環境的特點,融合激光雷達、深度相機、慣性測量單元(inertial measurement unit, IMU)以及足式里程計重點解決四足機器人在換流站中多層樓梯以及狹窄通道場景的建圖定位問題。對激光雷達和深度相機進行優勢互補,融合激光雷達2～85 m范圍內和深度相機2 m范圍內較高精度點云,提取邊緣特征、地面面特征以及非地面面特征,改進基于特征的點云匹配算法,使用緊耦合的迭代卡爾曼濾波器(iterated extended Kalman filter, IEKF)來融合點云特征點和IMU觀測,解決樓梯間相似場景誤匹配以及狹窄通道特征匹配失效的問題,優化建圖算法回環檢測部分,解決大場景建圖回環問題。通過UKF融合足式里程計與融合點云和三維環境地圖NDT匹配位姿得到四足機器人高頻穩定的全局位姿。

1 四足機器人模型及巡檢系統

采用四足機器人作為巡檢載體,它能夠跨越樓梯以及穿越換流站狹窄通道區域,其模型圖如圖1所示,四足機器人上搭配3個RealSense D435i相機、1個16線三維激光雷達、1個高精度IMU傳感器和12個電機驅動器。

圖1 四足機器人模型

四足機器人的巡檢系統框架如圖2所示,傳感器數據包括3個深度相機數據、1個16線激光雷達點云數據、IMU數據以及足式里程計數據,足式里程計數據由運動控制端融合IMU以及編碼器數據得到。同步單元將各個傳感器數據進行時間同步,預處理之后再輸入建圖以及定位模塊中。可以看出,建圖和定位模塊是四足機器人巡檢系統中非常重要的兩個模塊。

圖2 四足機器人巡檢框架

2 傳感器標定及數據預處理

激光雷達、相機以及IMU之間外參和時間戳的標定是四足機器人實施建圖與定位模塊的首要條件。采用3D棋盤格對激光雷達和相機的外參進行標定[15],設相機到激光雷達的外參表達式為

PL=TPC

(1)

式(1)中:PL為激光雷達L坐標齊次坐標轉換表示;T為相機到激光雷達的坐標變換矩陣;PC為相機C的坐標。可以得出3個相機到激光雷達的外參矩陣分別為TLC1、TLC2、TLC3,其中,TLC1為四足機器人左前相機到激光雷達的坐標變換矩陣,TLC2為四足機器人右前相機到激光雷達的坐標變換矩陣,TLC3為四足機器人后面相機到激光雷達的坐標變換矩陣。

標定完激光雷達與相機的外參后,還需要標定激光雷達與IMU的外參。采用基于軌跡重合的標定方法對激光雷達與IMU進行標定,對激光雷達在一段時間內進行較高精度的點云匹配,得到該時間段內高精度里程計信息,利用慣導預積分的方式獲取同一段時間內位姿約束信息。其原理如圖3所示。

L0為激光雷達初始坐標系;Lidar(t)和Lidar(t+1)分別為激光雷達在t時刻和t+1時刻在L0中的位置;B0為IMU初始坐標系;IMU(t)和IMU(t+1)分別為IMU在t時刻和t+1時刻在B0中的位置;C為兩個傳感器所形成的軌跡;I、J、K為直角坐標系基向量

圖3中虛線條分別表示激光雷達和IMU通過點云匹配和預積分得到的里程計信息,對兩條軌跡進行重合的思想優化求解IMU到激光雷達的變換矩陣,首先構建優化方程為

(2)

由于激光雷達、深度相機與IMU獲取數據的頻率不一樣,深度相機與激光雷達采集數據的頻率為10 Hz,而IMU采集數據的頻率為200 Hz,因此還需要進行激光雷達、深度相機與IMU的時間同步。首先通過時間同步單元對激光雷達、深度相機與IMU的時間戳基準進行同步,然后獲取激光雷達當前幀前后時刻的IMU數據,通過插值的方法計算出一個激光雷達當前幀時刻的等效值,后續每一幀激光雷達數據都以此方法得到IMU同一時刻的等效信息,從而實現激光雷達、深度相機與IMU的時間軟同步。

3 多傳感器融合SLAM建圖及實現

通過融合激光雷達、深度相機以及IMU傳感器構建四足機器人的SLAM建圖框架,如圖4所示,整個框架主要包括傳感器數據接收及預處理、特征提取[16]、點云里程計、回環檢測及全局位姿優化[17]和建圖模塊。其中傳感器數據接收及預處理模塊分為深度相機與激光雷達的數據獲取、激光點云去畸變以及激光點云與相機點云融合部分,激光里程計模塊分為融合點云匹配、位姿求解以及IEKF[18]數據融合部分。

圖4 四足機器人多傳感器融合SLAM建圖框架

多傳感器融合框架具體步驟如下。

3.1 傳感器數據接收及預處理

圖5 激光雷達與深度相機點云融合

3.2 特征提取

從融合點云Pk中提取邊緣特征和面特征,其中面特征又包括地面面特征和非地面面特征。通過評估局部區域內點的平滑度來提取邊緣特征和面特征,計算平滑度的公式如式(3)所示。設i是Pk中的一個點,i∈Pk,設S是同一次掃描中激光掃描儀返回的i的連續點的集合,則有

(3)

為解決狹窄區域以及樓梯場景相似環境下點云建圖回環不上的問題,構建BoW詞袋向量用于后續回環檢測。首先在3個深度相機獲取的RGB圖像中提取加速分段試驗(features from accelerated segment test,FAST)特征點,并計算特征點的ORB描述符,最終根據ORB描述符計算該幀圖像的BoW向量。為了減少計算量和需維護的內存大小,僅計算關鍵幀的BoW向量用于建立關鍵幀數據庫。非關鍵幀僅提取特征點及計算ORB描述符,用于計算當前追蹤的特征點數量。

3.3 點云里程計

3.4 回環檢測及全局位姿優化

為降低回環優化時的計算成本以及系統維護的內存,僅對關鍵幀進行回環檢測。以成功匹配的特征點數量小于某一閾值或當前幀生成的新特征點數量大于某一閾值或當前幀與上一關鍵幀之間幀數大于某一閾值為條件構建關鍵幀數據庫。在關鍵幀數據庫中通過離線訓練的BoW詞典搜索候選回環匹配幀,通過時間和幾何一致性檢查找到最佳匹配。檢測到回環后,啟動全局位姿圖對所有關鍵幀位姿進行優化。全局位姿圖如圖6所示。

L1為激光雷達t1時刻的點云里程計數據,以此類推,Lk為tk時刻的點云里程計數據

3.5 建圖

多傳感器融合SLAM建圖方法融合了三個深度相機的點云來補足激光雷達點云,對融合點云進行地面分割,并提取三類特征點,通過提取RGB圖像特征點的ORB描述符計算關鍵幀圖像的BoW向量用于回環檢測以及全局的位姿優化。

4 多傳感器融合定位及實現

通過多傳感器融合SLAM建圖方法建立換流站的全局地圖之后,還需要實時獲取四足機器人在換流站中的精準位姿。與實時建圖中的定位不同的是,四足機器人在定位導航過程中點云幀間的運動有可能非常劇烈,如在上下樓梯或快速奔跑執行巡檢任務時,掃描特征匹配可能會由于大的位移對于估計四足機器人的運動位姿有延遲,導致錯誤判斷四足機器人在點云圖中的位姿。因此采用UKF[21]將NDT掃描匹配與運動控制端提供的足式里程計進行融合,以足式里程計預測位姿作為NDT匹配的初始值,加快NDT匹配時間,并采用足式里程計進行有效插值輸出高頻的四足機器人位姿。四足機器人多傳感器融合的定位框架如圖7所示。

圖7 多傳感器融合定位框架

四足機器人的多傳感器融合定位框架主要分為NDT點云匹配得到運動位姿以及UKF數據融合。

首先定義待估計的四足機器人狀態為

(4)

(5)

式(5)中:Δpt為四足機器人在t-1到t時間段內補償里程計位置偏置的位移;Δqt為四足機器人在t-1到t時間段內補償里程計方向偏置的旋轉。

式(5)中利用UKF預測的機器人位姿作為NDT匹配的初始值,再執行NDT將觀測到的點云與全局地圖進行匹配,最后通過得到的NDT掃描匹配之后的機器人位姿對機器人狀態進行校正。定位觀測方程為

zt=[pt,qt]T

(6)

在每幀足式里程計數據傳入之后都對四足機器人位姿做一次預測更新,有了足式里程計的預測作為NDT匹配的初值,可以大大減短NDT點云匹配的時間,降低算法計算資源的消耗,以點云的匹配位姿作為估計真值對四足機器人的位姿進行校正,最終通過多傳感器融合定位框架可以理論實現四足機器人在換流站環境中200 Hz高頻穩定的實時位姿輸出。

5 實驗與分析

為驗證四足機器人“絕影X20”在換流站中實際建圖定位效果,以圖8所示的四足機器人平臺開展了5個實驗。對四足機器人建圖算法在換流站中整體的建圖能力包括狹窄通道場景進行測試,與LIO-SAM算法[22]進行對比,驗證所提出的建圖算法整體建圖能力;對四足機器人在多樓層場景進行建圖測試;對四足機器人定位算法以足式里程計為預測、IMU預測以及無預測時位移以及旋轉的偏差進行測試;對四足機器人在以建圖定位算法基礎上的上下樓梯成功率進行測試;對四足機器人在換流站場景的重復定位精度進行測試。

圖8 四足機器人

首先測試四足機器人建圖算法在換流站場景的整體建圖能力,分別控制四足機器人運行所提出的SLAM方法與LIO-SAM算法走完整個換流站,所述SLAM算法所建點云圖如圖9所示,可以看出,點云圖沒有出現重影以及漂移情況。圖10為LIO-SAM建立的換流站點云圖,可以看到四足機器人在進入一個狹窄通道的時候,點云出現了圖10左邊重影的情況。

圖9 SLAM算法所建點云圖

圖10 LIO-SAM算法所建點云圖

建圖算法融合了3個相機點云并且結合地面檢測提取了地面特征點以及非地面特征點進行匹配,對于抑制建圖Z軸方向的漂移具有重要作用,觀測四足機器人在換流站中的建圖軌跡與LIO-SAM算法的建圖軌跡進行對比,提出的建圖算法的軌跡在Z軸方向幾乎沒有漂移,而LIO-SAM算法具有2.132 m的漂移。因此提出的建圖算法對于換流站環境整體點云地圖建立更為準確,尤其是具有狹窄通道的場景。

驗證四足機器人在換流站中跨越多樓層室內樓梯場景的建圖能力,應用提出的建圖算法進行測試,如圖11所示,可以較好地建立樓梯間的點云圖,和只采用激光雷達與IMU傳感器的LIO-SAM算法進行對比,LIO-SAM算法在樓梯間點云匹配失效,出現了點云位姿漂移的現象,如圖12所示。

圖11 多樓層樓梯點云圖

圖12 LIO-SAM建立樓梯間點云圖

為了評估足式里程計對于四足機器人定位姿態預測的效果,假設NDT估計的姿態大部分是正確的,將足式里程計預測的四足機器人姿態與LNDT估計的姿態進行比較,計算出足式里程計預測的四足機器人姿態錯誤量,將IMU預測的姿態與NDT估計的姿態進行比較,計算出IMU預測的姿態錯誤量,在沒有位姿預測的情況下,將先前的匹配結果作為初始猜測計算錯誤量。分別測試四足機器人在上樓梯的過程以及快速奔跑的過程中足式里程計對于定位姿態預測的效果,分別如圖13和圖14所示,可以看出,不管是四足機器人在上樓梯還是以3 m/s快速奔跑的情況下,足式里程計預測的姿態都更為精準。當幀間位移變大,在沒有位姿預測或預測誤差較差的情況下,NDT匹配需要更長的時間,大約超過56 ms,因為幀間的大位移使NDT需要更多的迭代才能收斂到局部最優解。通過足式里程計預測,由于良好的初始猜測,每幀匹配時間約為42 ms。結果表明,基于足式里程計的姿態預測使四足機器人姿態估計對上下樓梯劇烈波動以及快速運動具有魯棒性和快速收斂性。

圖13 上樓梯情景下預測姿態錯誤量

圖14 快速奔跑下預測姿態錯誤量

四足機器人穩定精準地上下樓梯是換流站巡檢中的一個難點,需要定位框架提供精準的四足機器人位姿估計,構建換流站中樓梯的地形圖,給四足機器人提供可踩區域和不可踩區域信息,使四足機器人可以提前設置好落腳點。在紹興一個換流站中選取一處6層階梯使用手柄遙控四足機器人和使用實時位姿構建地形圖輔助四足機器人進行上下樓梯成功率的對比試驗,每層階梯高為18 cm,寬為21 cm,長為140 cm,如圖15所示,分別以手柄遙控的方式和以地形圖輔助的方式,統計100次上下樓梯的成功率,其中手柄遙控以及地形圖輔助上樓梯時機器狗前進的速度均為0.5 m/s。分析數據得出,以手柄遙控方式的平均成功率為46.4%,以實時位姿構建地形圖輔助四足機器人上下樓梯方式的成功率為98%。后一種方式比前一種方式上下樓梯的成功率提高了51.6%。因此,所述的定位方法對于提高四足機器人上下樓梯成功率具有重要作用。

圖15 四足機器人爬樓梯

四足機器人在換流站中的重復定位精度是實現無人自主巡檢的重要保障,只有當重復定位精度較高才能保證裝載在四足機器人上的云臺能夠準確識別各個表計的讀數以及一些異常點。通過標定換流站中某一點的坐標,使四足機器人重復到達這一點,重復12次,計算每一次到達坐標與標定坐標的直線誤差絕對值,從而驗證出四足機器人建圖定位算法的重復定位精度,圖16是截取的其中一次實驗,可以看出四足機器人在換流站中的重復定位精度最大誤差小于6 cm。

圖16 重復定位精度測試

經過反復5輪不同點的測試,四足機器人的重復定位誤差均小于6 cm,可以滿足換流站的巡檢需求。

6 結論

對換流站中四足機器人的建圖定位系統進行了細致的優化。通過融合深度相機點云作為點云輸入,彌補以飛行時間為原理的三維激光雷達在2 m范圍內點云具有較低精度的缺陷,對融合點云進行地面分割,提取邊緣特征、非地面面特征以及地面面特征,優化基于特征的點云匹配算法,使用緊耦合的IEKF濾波器來融合點云特征點和慣導觀測,使四足機器人在狹窄環境以及樓梯通道環境具有較佳的建圖效果,優化回環檢測使建圖算法對于換流站具有整體精準的三維建圖能力。通過UKF濾波器融合足式里程計預測四足機器人姿態作為NDT匹配初值,加快NDT點云匹配時間,通過實時匹配融合點云與三維環境地圖得到全局位姿進行姿態更新,使四足機器人在換流站中具備穩定精準的重復定位精度。最終,經過換流站帶有狹窄通道的整體建圖以及多樓層建圖與LIO-SAM算法進行對比,驗證了提出的建圖算法對于狹窄通道場景以及多樓層場景建圖的有效性,通過里程計預測對比實驗、四足機器人上下樓梯實驗以及重復定位精度實驗驗證了所提出的定位算法可以縮短NDT匹配的時間至42 ms以及達到6 cm以內的重復定位精度。