室外帶電作業(yè)機(jī)器人的融合定位方法研究*

徐偉 王勇軍*

(1 深圳億嘉和科技研發(fā)有限公司 智能清潔事業(yè)部,廣東 深圳 518055;2 桂林航天工業(yè)學(xué)院 無人機(jī)遙測重點(diǎn)實(shí)驗(yàn)室,廣西 桂林 541004)

在高壓輸電線剝線、接線等帶電操作中,通常依靠人工完成,但存在效率低下、勞動(dòng)強(qiáng)度大和高壓安全不足等問題[1-4]。為了解決這些問題,工業(yè)界研制了帶電作業(yè)機(jī)器人[2,5-6]。這些室外帶電作業(yè)機(jī)器人多采用激光點(diǎn)云技術(shù)對(duì)電線等作業(yè)對(duì)象進(jìn)行建模,并計(jì)算出最佳的目標(biāo)作業(yè)位姿。通過定位系統(tǒng)實(shí)時(shí)反饋精準(zhǔn)定位信息,從而引導(dǎo)機(jī)器人達(dá)到指定位置。室外帶電作業(yè)機(jī)器人的作業(yè)場景通常是半高空?qǐng)鼍?且周圍環(huán)境比較復(fù)雜。在這種工作環(huán)境下,準(zhǔn)確定位是帶電作業(yè)機(jī)器人順利完成作業(yè)任務(wù)的基礎(chǔ)[7]。

諸多學(xué)者對(duì)室外帶電作業(yè)機(jī)器人半高空?qǐng)鼍跋碌木珳?zhǔn)定位技術(shù)開展了研究。文獻(xiàn)[8]使用單目視覺定位方法,文獻(xiàn)[9]使用激光定位方法,文獻(xiàn)[10]使用激光雷達(dá)建立場景語義來獲取定位,文獻(xiàn)[11]采用的是雙目立體視覺的定位方法。但是,半高空?qǐng)鼍跋碌碾娋€等關(guān)鍵目標(biāo)點(diǎn)云特征稀疏,視覺成像易受強(qiáng)光光照干擾,因而該場景下的點(diǎn)云匹配和視覺應(yīng)用定位方法受限較多。借鑒全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)技術(shù)可實(shí)現(xiàn)一種不依賴環(huán)境特征的絕對(duì)定位方法,來解決環(huán)境受限場景下帶電作業(yè)機(jī)器人的定位問題[12-18]。但是該技術(shù)多數(shù)都以GPS數(shù)據(jù)為主要基準(zhǔn),其單點(diǎn)定位精度較差,無法滿足帶電作業(yè)機(jī)器人的厘米級(jí)定位精度要求。為此,可在衛(wèi)星導(dǎo)航基礎(chǔ)上,進(jìn)一步采用載波相位差分(Real-Time Kinematic,RTK)技術(shù),可使得定位精度能達(dá)到厘米級(jí)[19]。但RTK定位性能通常易受環(huán)境遮擋的影響,且需要考慮并解決工程上的電磁干擾問題。文獻(xiàn)[20-21]使用RTK與激光雷達(dá)進(jìn)行融合定位,可在一定程度上解決干擾問題,但在工程上增加了全局建圖的工序。

結(jié)合以上分析,設(shè)計(jì)了一種基于雙天線RTK/激光雷達(dá)/IMU的誤差濾波器,采用以雙天線RTK與慣性測量單元(Inertial Measurement Unit,IMU)的組合導(dǎo)航技術(shù)[22]為主,激光點(diǎn)云地圖匹配技術(shù)為輔的融合定位方法,充分發(fā)揮RTK、激光雷達(dá)和IMU三種傳感器的優(yōu)勢互補(bǔ)特性,不僅有效解決場景受限時(shí)的定位問題,而且大大提升了該場景下的定位穩(wěn)定性。同時(shí),構(gòu)建了一種適應(yīng)半高空?qǐng)鼍暗臋C(jī)器人運(yùn)動(dòng)約束模型,可有效解決機(jī)器人本體電磁干擾導(dǎo)致航向估計(jì)誤差大甚至發(fā)散問題,大大提升了抗干擾能力。

在本方法中,RTK和IMU進(jìn)行數(shù)據(jù)融合,考慮到單天線RTK無法靜止定向,因此采用了雙天線RTK方案,并推導(dǎo)出了雙天線RTK位置、速度和航向觀測模型。RTK和激光雷達(dá)亦進(jìn)行數(shù)據(jù)融合,RTK在空曠場景表現(xiàn)優(yōu)異,而激光雷達(dá)在遮擋場景表現(xiàn)優(yōu)異,兩者在室外復(fù)雜場景互補(bǔ)融合,同時(shí)考慮到半高空?qǐng)鼍耙钥諘鐬橹?因而以RTK為主要定位傳感器。最后,考慮到高壓線路作業(yè)過程中,大電流和高電壓產(chǎn)生的強(qiáng)烈電磁場可能會(huì)干擾GPS和RTK的工作,導(dǎo)致定位失效或誤差增大,還引入了抗干擾處理方法。為了解決干擾問題,本文引入了機(jī)器人優(yōu)化運(yùn)動(dòng)約束模型。通過對(duì)機(jī)器人的運(yùn)動(dòng)模式進(jìn)行深入分析,并結(jié)合IMU的高頻數(shù)據(jù),實(shí)時(shí)調(diào)整和優(yōu)化模型參數(shù),從而有效抵消電磁干擾的影響,保證了定位的準(zhǔn)確性和穩(wěn)定性。

1 雙天線RTK/激光雷達(dá)/IMU組合系統(tǒng)

如圖1所示的雙天線RTK/激光雷達(dá)/IMU組合系統(tǒng),測量IMU坐標(biāo)系相對(duì)于東北天坐標(biāo)系[15]的三維位姿變換。要求IMU、激光雷達(dá)與雙天線嚴(yán)格剛性安裝,且RTK航向觀測方程隨IMU安裝位置和姿態(tài)不同而不同,設(shè)置RTK主天線指向副天線方向與IMU傳感器y軸正向一致。誤差濾波器以IMU誤差傳播建立預(yù)測方程、以雙天線RTK位置、速度和航向建立RTK觀測方程、以激光點(diǎn)云地圖匹配位姿建立激光觀測方程。注意的是,當(dāng)RTK觀測質(zhì)量不佳時(shí),才會(huì)融合激光位姿。

IMU通常包括三軸加速度計(jì)和三軸陀螺儀,分別測量物體的線性加速度和角速度。捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System,SINS)使用加速度和角速度信息并進(jìn)行積分,估計(jì)位置、速度和姿態(tài)[15,23],并根據(jù)IMU精度推導(dǎo)誤差協(xié)方差[24]。

RTK主要由基站和移動(dòng)站組成,基站和移動(dòng)站通過電臺(tái)或網(wǎng)絡(luò)傳輸差分?jǐn)?shù)據(jù),實(shí)現(xiàn)對(duì)移動(dòng)站位置的精確計(jì)算。通常將基站架高并靜止放置于室外空曠環(huán)境,移動(dòng)站固連于機(jī)器人本體并隨機(jī)器人運(yùn)動(dòng)。因此,其位姿用兩種不同的坐標(biāo)系描述p點(diǎn)在地心地固坐標(biāo)系(Earth-Centered Earth-Fixed,ECEF)中的位置,即大地坐標(biāo)系和直角坐標(biāo)系[14],如圖2所示。RTK位置測量采用緯度,經(jīng)度,高度(φ,λ,h)大地坐標(biāo)系表示,RTK航向測量為主天線指向副天線方向與真北的夾角。為方便數(shù)學(xué)處理,將經(jīng)緯高度轉(zhuǎn)換到東北天坐標(biāo)系[15]。

激光雷達(dá)用于測量和獲取目標(biāo)物體的距離、位置和形狀等信息,它通過發(fā)射激光束并接受激光束的反射信號(hào)來實(shí)現(xiàn)測量。對(duì)電線等作業(yè)對(duì)象建模,同時(shí)對(duì)環(huán)境進(jìn)行激光掃描建圖[25],并生成三維點(diǎn)云地圖。根據(jù)激光點(diǎn)云地圖數(shù)據(jù),并結(jié)合點(diǎn)云精匹配算法(Iterative Closest Point,ICP)可求解位姿和置信度[26]。

1.1 IMU誤差濾波器

濾波狀態(tài)xt=[pt,vt,qt,abt,wbt]由預(yù)測狀態(tài)x=[p,v,q,ab,wb]和誤差狀態(tài)δx=[δp,δv,δq,δab,δwb]組成。其中,p,v,q,ab,wb分別為三維位置、速度、姿態(tài)四元數(shù)、加速度零偏、角速度零偏。x通過三維角速度和加速度積分,δx通過建立IMU誤差方程并結(jié)合雙天線RTK觀測修正估計(jì)。三者的關(guān)系用式(1)表示, ⊕ 表示向量運(yùn)算,如加減法、四元數(shù)乘法。

xt=x⊕δx

(1)

1.1.1 預(yù)測方程

預(yù)測狀態(tài)x,用式(2)對(duì)三維角速度ωm和加速度am進(jìn)行積分。

(2)

1.1.2 誤差方程

建立誤差方程式(3)。

δp←δp+δvΔt

δv←δv+(-(R(am-ab))δθ-Rδab)Δt+vi

δθ←δθ-RδωbΔt+θi

δab←δab+ai

δωb←δωb+ωi

(3)

其中,vi,θi,ai,ωi分別為速度,角度,加速度偏置,角速度偏置高斯白噪聲。

設(shè)誤差狀態(tài)和擾動(dòng)向量的雅可比矩陣分別為Fx和Fi,擾動(dòng)向量的協(xié)方差矩陣為Q,建立誤差預(yù)測方程(4)。

(4)

為了讓濾波器快速收斂,通常取較大初始協(xié)方差P0[15]。

1.2 雙天線RTK觀測器

1.2.1 位置觀測方程

(5)

其關(guān)于δθ的雅可比矩陣為:

(6)

位置觀測雅可比矩陣為:

(7)

圖3 RTK位置觀測模型

1.2.2 速度觀測方程

對(duì)式(5)進(jìn)行微分,建立RTK速度觀測方程式(8)。

(8)

其關(guān)于δθ的雅可比矩陣為:

(9)

其關(guān)于δωb的雅可比矩陣為:

(10)

速度觀測雅可比矩陣為:

(11)

1.2.3 航向觀測方程

因旋轉(zhuǎn)矩陣R提取的航向φ′∈[-π,π],而雙天線RTK測量的真北方向角φ∈[0,2π),將其歸一化到區(qū)間[-π,π],如圖4所示。

圖4 雙天線RTK航向歸一化

考慮IMU的y軸指向機(jī)頭方向,φ關(guān)于旋轉(zhuǎn)四元數(shù)q=[q0,q1,q2,q3]T的方程為:

(12)

(13)

其中,φ關(guān)于q的雅可比為:

(14)

q關(guān)于δθ的雅可比為:

(15)

航向觀測雅可比矩陣為:

(16)

1.3 激光位姿觀測器

(17)

其中T為4×4矩陣,包含了三維旋轉(zhuǎn)矩陣R和平移向量p。則激光位置觀測方程為:

(18)

位置觀測雅可比矩陣同式(7)。

1.4 有害數(shù)據(jù)剔除與濾波

E{rrT}=HP-HT+R

(19)

同時(shí)選擇一個(gè)閾值λ使得

Prob{r2>λ(HP-HT+R)}=μ

(20)

此處μ∈(0,1)通常較小。當(dāng)式(21)被滿足時(shí),

r2=λ(HP-HT+R)

(21)

測量值是無效的且相應(yīng)的觀測修正更新可以被忽略[27]。

該作業(yè)機(jī)器人的位置、速度和航向觀測過程各自進(jìn)行更新,且各自進(jìn)行有害數(shù)據(jù)檢測和剔除。需要注意的是,通常RTK可以反饋定位和定向狀態(tài),根據(jù)其狀態(tài)亦可判斷測量值是否有效。

2 基于運(yùn)動(dòng)約束模型的抗干擾處理

2.1 電磁干擾描述

帶電作業(yè)機(jī)器人電子設(shè)備復(fù)雜,難免會(huì)產(chǎn)生高頻雜波等電磁干擾,影響RTK接收衛(wèi)星信號(hào)并進(jìn)行定位解算,特別是RTK副天線對(duì)干擾信號(hào)尤為敏感,若處理不好,將大大降低RTK解算性能?？赏ㄟ^架高天線等遠(yuǎn)離干擾源的方法可改善電磁干擾問題,但這樣會(huì)增大產(chǎn)品體積,應(yīng)用空間受限。

2.2 基于IMU的運(yùn)動(dòng)約束

室外帶電作業(yè)機(jī)器人主要靠機(jī)械臂操作,且本體通常處于靜態(tài)執(zhí)行建模、接線、剝線等作業(yè)任務(wù),因而本體不會(huì)有高頻振動(dòng),對(duì)IMU非常友好。因此,可利用IMU實(shí)時(shí)檢測機(jī)器人運(yùn)動(dòng)狀態(tài)[28]。

設(shè)三維加速度am=[amx,amy,amz],三維陀螺儀ωm=[ωmx,ωmy,ωmz],若滿足式 (22)、(23)、(24),則認(rèn)為其處于穩(wěn)態(tài)。

(22)

(23)

|ωmi-δωbi|<0.2

(24)

其中,i=x,y,z,0.1、0.01、0.2是根據(jù)實(shí)驗(yàn)設(shè)定的經(jīng)驗(yàn)值,應(yīng)用時(shí)要根據(jù)使用的IMU精度進(jìn)行參數(shù)調(diào)整。δωb=[δωbx,δωby,δωbz]是陀螺儀零偏偏置,在處于穩(wěn)態(tài)時(shí)可用式(25)去更新。

δωb←δωb+0.01(ωm-δωb)

(25)

2.3 濾波處理

應(yīng)用時(shí)架高雙天線就可忽略電磁干擾對(duì)定位天線的影響,僅需考慮如何剔除航向有害數(shù)據(jù)。若式(21)生效,則可剔除當(dāng)前RTK航向測量值。此時(shí)若檢測到機(jī)器人處于靜態(tài),則使用當(dāng)前時(shí)刻濾波器航向估計(jì)值作為后續(xù)觀測量,直到穩(wěn)態(tài)退出或定向狀態(tài)恢復(fù)。

3 實(shí)驗(yàn)驗(yàn)證

圖5所示的是自行研制的雙天線RTK/IMU組合導(dǎo)航系統(tǒng)。

圖5 雙天線RTK/IMU組合導(dǎo)航系統(tǒng)

將組合導(dǎo)航和3D激光雷達(dá)(32線,測量精度:±1 cm,FOV:360°×90°,分辨率:水平0.2°、垂直2.81°)分別固定安裝于室外帶電作業(yè)機(jī)器人,雙天線架高0.3米,主副天線間隔1米,分別在半高空無遮擋和半遮擋場景進(jìn)行作業(yè)。圖6展示了半高空無遮擋和半遮擋場景下的點(diǎn)云地圖及其定位軌跡,并在這兩種場景下測試定位和定向精度。

圖6 室外半高空?qǐng)鼍包c(diǎn)云地圖及定位軌跡

整個(gè)實(shí)驗(yàn)可分六個(gè)步驟:1)將自研RTK/IMU組合導(dǎo)航設(shè)備固定安裝在帶電作業(yè)機(jī)器人。2)利用功分器讓兩者共用雙天線衛(wèi)星信號(hào)。3)使用自研基站發(fā)送差分電文。4)分別在半高空無遮擋和半遮擋兩種場景均執(zhí)行下述步驟。5)3支激光筆分別以不同的位姿固定安裝在機(jī)器人本體上,3條激光束打到地面標(biāo)簽紙上。6)在三維空間中任意選取A1,A2,…An點(diǎn),并在地面上貼上3個(gè)標(biāo)簽紙。6)將機(jī)器人從三維空間中任意位置操控到A1,A2,…An點(diǎn),每個(gè)點(diǎn)反復(fù)操作10次及以上,并記錄結(jié)果。

實(shí)驗(yàn)測試流程如圖7所示。

圖7 實(shí)驗(yàn)測試流程圖

若機(jī)器人處于靜態(tài)狀態(tài),激光雷達(dá)對(duì)電線等作業(yè)對(duì)象按照1 m/s速度進(jìn)行動(dòng)態(tài)掃描,使用LOAM系列[27]建圖算法就可生成點(diǎn)云地圖,如圖8所示。與此同時(shí),通過濾波器計(jì)算的位姿將激光點(diǎn)云轉(zhuǎn)換到地圖坐標(biāo)系,可確定轉(zhuǎn)換后的點(diǎn)云與地圖的匹配重合度。不難發(fā)現(xiàn),圖8所示的匹配重合度較好,這也從側(cè)面能反映出濾波器能提供精準(zhǔn)定位和定向,證明了本文方法的有效性。

圖8 轉(zhuǎn)換后的激光點(diǎn)云與地圖重合情況

實(shí)際應(yīng)用過程中因存在各種干擾,如果僅采用RTK數(shù)據(jù)來解算機(jī)器人定向信息,會(huì)發(fā)現(xiàn)RTK航向測量值波動(dòng)較大,解算的航向角會(huì)逐漸發(fā)散,如圖9所示原始RTK航向值。

圖9 航向?yàn)V波前后對(duì)比曲線

為提高定向精度,解決航向發(fā)散問題,結(jié)合機(jī)器人“運(yùn)動(dòng)約束模型”所設(shè)計(jì)的濾波器可修正RTK解算數(shù)據(jù),獲得平穩(wěn)光滑的航向?yàn)V波結(jié)果(圖9航向?yàn)V波值)。這也表明,優(yōu)化的運(yùn)動(dòng)約束模型能提升RTK/IMU組合濾波的精度和穩(wěn)定性,該方法可提升濾波器的抗干擾能力。

圖6(b)所示的半高空半遮擋場景是應(yīng)用中出現(xiàn)最多的場景,存在較多的遮擋物,RTK定位狀態(tài)存在“假固定”現(xiàn)象,且本體的電磁干擾也會(huì)導(dǎo)致RTK定向狀態(tài)出現(xiàn)浮動(dòng)解。在該環(huán)境下進(jìn)行零位靜態(tài)重復(fù)定位和定向精度測試,10次實(shí)驗(yàn)結(jié)果如表1所示。結(jié)果表明,RTK組合導(dǎo)航融合了激光位姿以及運(yùn)動(dòng)學(xué)約束模型,能夠?yàn)槭彝鈳щ娮鳂I(yè)機(jī)器人提供高于3cm的定位精度和高于0.3°的定向精度,滿足機(jī)器人作業(yè)要求。

表1 重復(fù)定位精度測試結(jié)果

4 結(jié) 論

本文采用室外帶電作業(yè)機(jī)器人為載體,以解決帶電作業(yè)機(jī)器人定位問題以及干擾情況下引起的航向誤差大甚至發(fā)散問題為目標(biāo),設(shè)計(jì)了雙天線RTK/激光雷達(dá)/IMU組合濾波器,充分利用雙天線RTK、激光雷達(dá)、IMU三種傳感器的優(yōu)勢互補(bǔ)特性,解決了應(yīng)用場景下精準(zhǔn)定位問題;同時(shí),還建立了基于室外帶電作業(yè)機(jī)器人的運(yùn)動(dòng)約束模型,充分發(fā)揮IMU在該場景下的優(yōu)勢,利用IMU數(shù)據(jù)進(jìn)行穩(wěn)態(tài)檢測,解決了航向誤差發(fā)散的問題。經(jīng)過實(shí)驗(yàn)驗(yàn)證,基于雙天線RTK組合導(dǎo)航,融合激光點(diǎn)云地圖匹配技術(shù),引入運(yùn)動(dòng)學(xué)約束模型,不僅能夠提供精準(zhǔn)定位和定向信息,而且大大提升定位穩(wěn)定性和抗干擾能力。

基于雙天線RTK/IMU的激光點(diǎn)云數(shù)據(jù)融合濾波定位方法,不僅提高了帶電作業(yè)機(jī)器人在復(fù)雜環(huán)境下的定位精度,也降低了因環(huán)境變化和電磁干擾引起的定位失效風(fēng)險(xiǎn),為帶電作業(yè)機(jī)器人的廣泛應(yīng)用打下了堅(jiān)實(shí)的基礎(chǔ)。該方法已經(jīng)在公司承接的多個(gè)任務(wù)場景下進(jìn)行了長時(shí)間的作業(yè)測試,結(jié)果符合預(yù)期,具備較高的商業(yè)推廣價(jià)值。

需要注意的是,雙天線RTK與IMU的桿臂值、激光雷達(dá)與IMU的靜態(tài)位姿,均直接采用結(jié)構(gòu)初值,應(yīng)用時(shí)存在旋轉(zhuǎn)外參誤差,下一步可結(jié)合該誤差的補(bǔ)償研究進(jìn)而提高系統(tǒng)的定位定向精度。