徐偉 王勇軍*

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

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

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

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

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

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

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

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

RTK主要由基站和移動站組成,基站和移動站通過電臺或網(wǎng)絡(luò)傳輸差分?jǐn)?shù)據(jù),實現(xiàn)對移動站位置的精確計算。通常將基站架高并靜止放置于室外空曠環(huán)境,移動站固連于機器人本體并隨機器人運動。因此,其位姿用兩種不同的坐標(biāo)系描述p點在地心地固坐標(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]。

激光雷達用于測量和獲取目標(biāo)物體的距離、位置和形狀等信息,它通過發(fā)射激光束并接受激光束的反射信號來實現(xiàn)測量。對電線等作業(yè)對象建模,同時對環(huán)境進行激光掃描建圖[25],并生成三維點云地圖。根據(jù)激光點云地圖數(shù)據(jù),并結(jié)合點云精匹配算法(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觀測修正估計。三者的關(guān)系用式(1)表示, ⊕ 表示向量運算,如加減法、四元數(shù)乘法。

xt=x⊕δx

(1)

1.1.1 預(yù)測方程

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

(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)和擾動向量的雅可比矩陣分別為Fx和Fi,擾動向量的協(xié)方差矩陣為Q,建立誤差預(yù)測方程(4)。

(4)

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

1.2 雙天線RTK觀測器

1.2.1 位置觀測方程

(5)

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

(6)

位置觀測雅可比矩陣為:

(7)

圖3 RTK位置觀測模型

1.2.2 速度觀測方程

對式(5)進行微分,建立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軸指向機頭方向,φ關(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)

同時選擇一個閾值λ使得

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

(20)

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

r2=λ(HP-HT+R)

(21)

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

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

2 基于運動約束模型的抗干擾處理

2.1 電磁干擾描述

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

2.2 基于IMU的運動約束

室外帶電作業(yè)機器人主要靠機械臂操作,且本體通常處于靜態(tài)執(zhí)行建模、接線、剝線等作業(yè)任務(wù),因而本體不會有高頻振動,對IMU非常友好。因此,可利用IMU實時檢測機器人運動狀態(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è)定的經(jīng)驗值,應(yīng)用時要根據(jù)使用的IMU精度進行參數(shù)調(diào)整。δωb=[δωbx,δωby,δωbz]是陀螺儀零偏偏置,在處于穩(wěn)態(tài)時可用式(25)去更新。

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

(25)

2.3 濾波處理

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

3 實驗驗證

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

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

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

圖6 室外半高空場景點云地圖及定位軌跡

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

實驗測試流程如圖7所示。

圖7 實驗測試流程圖

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

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

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

圖9 航向濾波前后對比曲線

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

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

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

4 結(jié) 論

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

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

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