換流站巡檢四足機(jī)器人建圖定位系統(tǒng)優(yōu)化

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

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

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

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

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

1 四足機(jī)器人模型及巡檢系統(tǒng)

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

圖1 四足機(jī)器人模型

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

圖2 四足機(jī)器人巡檢框架

2 傳感器標(biāo)定及數(shù)據(jù)預(yù)處理

激光雷達(dá)、相機(jī)以及IMU之間外參和時(shí)間戳的標(biāo)定是四足機(jī)器人實(shí)施建圖與定位模塊的首要條件。采用3D棋盤格對(duì)激光雷達(dá)和相機(jī)的外參進(jìn)行標(biāo)定[15],設(shè)相機(jī)到激光雷達(dá)的外參表達(dá)式為

PL=TPC

(1)

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

標(biāo)定完激光雷達(dá)與相機(jī)的外參后,還需要標(biāo)定激光雷達(dá)與IMU的外參。采用基于軌跡重合的標(biāo)定方法對(duì)激光雷達(dá)與IMU進(jìn)行標(biāo)定,對(duì)激光雷達(dá)在一段時(shí)間內(nèi)進(jìn)行較高精度的點(diǎn)云匹配,得到該時(shí)間段內(nèi)高精度里程計(jì)信息,利用慣導(dǎo)預(yù)積分的方式獲取同一段時(shí)間內(nèi)位姿約束信息。其原理如圖3所示。

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

圖3中虛線條分別表示激光雷達(dá)和IMU通過點(diǎn)云匹配和預(yù)積分得到的里程計(jì)信息,對(duì)兩條軌跡進(jìn)行重合的思想優(yōu)化求解IMU到激光雷達(dá)的變換矩陣,首先構(gòu)建優(yōu)化方程為

(2)

由于激光雷達(dá)、深度相機(jī)與IMU獲取數(shù)據(jù)的頻率不一樣,深度相機(jī)與激光雷達(dá)采集數(shù)據(jù)的頻率為10 Hz,而IMU采集數(shù)據(jù)的頻率為200 Hz,因此還需要進(jìn)行激光雷達(dá)、深度相機(jī)與IMU的時(shí)間同步。首先通過時(shí)間同步單元對(duì)激光雷達(dá)、深度相機(jī)與IMU的時(shí)間戳基準(zhǔn)進(jìn)行同步,然后獲取激光雷達(dá)當(dāng)前幀前后時(shí)刻的IMU數(shù)據(jù),通過插值的方法計(jì)算出一個(gè)激光雷達(dá)當(dāng)前幀時(shí)刻的等效值,后續(xù)每一幀激光雷達(dá)數(shù)據(jù)都以此方法得到IMU同一時(shí)刻的等效信息,從而實(shí)現(xiàn)激光雷達(dá)、深度相機(jī)與IMU的時(shí)間軟同步。

3 多傳感器融合SLAM建圖及實(shí)現(xiàn)

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

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

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

3.1 傳感器數(shù)據(jù)接收及預(yù)處理

圖5 激光雷達(dá)與深度相機(jī)點(diǎn)云融合

3.2 特征提取

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

(3)

為解決狹窄區(qū)域以及樓梯場(chǎng)景相似環(huán)境下點(diǎn)云建圖回環(huán)不上的問題,構(gòu)建BoW詞袋向量用于后續(xù)回環(huán)檢測(cè)。首先在3個(gè)深度相機(jī)獲取的RGB圖像中提取加速分段試驗(yàn)(features from accelerated segment test,FAST)特征點(diǎn),并計(jì)算特征點(diǎn)的ORB描述符,最終根據(jù)ORB描述符計(jì)算該幀圖像的BoW向量。為了減少計(jì)算量和需維護(hù)的內(nèi)存大小,僅計(jì)算關(guān)鍵幀的BoW向量用于建立關(guān)鍵幀數(shù)據(jù)庫。非關(guān)鍵幀僅提取特征點(diǎn)及計(jì)算ORB描述符,用于計(jì)算當(dāng)前追蹤的特征點(diǎn)數(shù)量。

3.3 點(diǎn)云里程計(jì)

3.4 回環(huán)檢測(cè)及全局位姿優(yōu)化

為降低回環(huán)優(yōu)化時(shí)的計(jì)算成本以及系統(tǒng)維護(hù)的內(nèi)存,僅對(duì)關(guān)鍵幀進(jìn)行回環(huán)檢測(cè)。以成功匹配的特征點(diǎn)數(shù)量小于某一閾值或當(dāng)前幀生成的新特征點(diǎn)數(shù)量大于某一閾值或當(dāng)前幀與上一關(guān)鍵幀之間幀數(shù)大于某一閾值為條件構(gòu)建關(guān)鍵幀數(shù)據(jù)庫。在關(guān)鍵幀數(shù)據(jù)庫中通過離線訓(xùn)練的BoW詞典搜索候選回環(huán)匹配幀,通過時(shí)間和幾何一致性檢查找到最佳匹配。檢測(cè)到回環(huán)后,啟動(dòng)全局位姿圖對(duì)所有關(guān)鍵幀位姿進(jìn)行優(yōu)化。全局位姿圖如圖6所示。

L1為激光雷達(dá)t1時(shí)刻的點(diǎn)云里程計(jì)數(shù)據(jù),以此類推,Lk為tk時(shí)刻的點(diǎn)云里程計(jì)數(shù)據(jù)

3.5 建圖

多傳感器融合SLAM建圖方法融合了三個(gè)深度相機(jī)的點(diǎn)云來補(bǔ)足激光雷達(dá)點(diǎn)云,對(duì)融合點(diǎn)云進(jìn)行地面分割,并提取三類特征點(diǎn),通過提取RGB圖像特征點(diǎn)的ORB描述符計(jì)算關(guān)鍵幀圖像的BoW向量用于回環(huán)檢測(cè)以及全局的位姿優(yōu)化。

4 多傳感器融合定位及實(shí)現(xiàn)

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

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

四足機(jī)器人的多傳感器融合定位框架主要分為NDT點(diǎn)云匹配得到運(yùn)動(dòng)位姿以及UKF數(shù)據(jù)融合。

首先定義待估計(jì)的四足機(jī)器人狀態(tài)為

(4)

(5)

式(5)中:Δpt為四足機(jī)器人在t-1到t時(shí)間段內(nèi)補(bǔ)償里程計(jì)位置偏置的位移;Δqt為四足機(jī)器人在t-1到t時(shí)間段內(nèi)補(bǔ)償里程計(jì)方向偏置的旋轉(zhuǎn)。

式(5)中利用UKF預(yù)測(cè)的機(jī)器人位姿作為NDT匹配的初始值,再執(zhí)行NDT將觀測(cè)到的點(diǎn)云與全局地圖進(jìn)行匹配,最后通過得到的NDT掃描匹配之后的機(jī)器人位姿對(duì)機(jī)器人狀態(tài)進(jìn)行校正。定位觀測(cè)方程為

zt=[pt,qt]T

(6)

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

5 實(shí)驗(yàn)與分析

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

圖8 四足機(jī)器人

首先測(cè)試四足機(jī)器人建圖算法在換流站場(chǎng)景的整體建圖能力,分別控制四足機(jī)器人運(yùn)行所提出的SLAM方法與LIO-SAM算法走完整個(gè)換流站,所述SLAM算法所建點(diǎn)云圖如圖9所示,可以看出,點(diǎn)云圖沒有出現(xiàn)重影以及漂移情況。圖10為L(zhǎng)IO-SAM建立的換流站點(diǎn)云圖,可以看到四足機(jī)器人在進(jìn)入一個(gè)狹窄通道的時(shí)候,點(diǎn)云出現(xiàn)了圖10左邊重影的情況。

圖9 SLAM算法所建點(diǎn)云圖

圖10 LIO-SAM算法所建點(diǎn)云圖

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

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

圖11 多樓層樓梯點(diǎn)云圖

圖12 LIO-SAM建立樓梯間點(diǎn)云圖

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

圖13 上樓梯情景下預(yù)測(cè)姿態(tài)錯(cuò)誤量

圖14 快速奔跑下預(yù)測(cè)姿態(tài)錯(cuò)誤量

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

圖15 四足機(jī)器人爬樓梯

四足機(jī)器人在換流站中的重復(fù)定位精度是實(shí)現(xiàn)無人自主巡檢的重要保障,只有當(dāng)重復(fù)定位精度較高才能保證裝載在四足機(jī)器人上的云臺(tái)能夠準(zhǔn)確識(shí)別各個(gè)表計(jì)的讀數(shù)以及一些異常點(diǎn)。通過標(biāo)定換流站中某一點(diǎn)的坐標(biāo),使四足機(jī)器人重復(fù)到達(dá)這一點(diǎn),重復(fù)12次,計(jì)算每一次到達(dá)坐標(biāo)與標(biāo)定坐標(biāo)的直線誤差絕對(duì)值,從而驗(yàn)證出四足機(jī)器人建圖定位算法的重復(fù)定位精度,圖16是截取的其中一次實(shí)驗(yàn),可以看出四足機(jī)器人在換流站中的重復(fù)定位精度最大誤差小于6 cm。

圖16 重復(fù)定位精度測(cè)試

經(jīng)過反復(fù)5輪不同點(diǎn)的測(cè)試,四足機(jī)器人的重復(fù)定位誤差均小于6 cm,可以滿足換流站的巡檢需求。

6 結(jié)論

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