室外帶電作業機器人的融合定位方法研究*

徐偉 王勇軍*

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

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

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

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

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

1 雙天線RTK/激光雷達/IMU組合系統

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

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

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

激光雷達用于測量和獲取目標物體的距離、位置和形狀等信息,它通過發射激光束并接受激光束的反射信號來實現測量。對電線等作業對象建模,同時對環境進行激光掃描建圖[25],并生成三維點云地圖。根據激光點云地圖數據,并結合點云精匹配算法(Iterative Closest Point,ICP)可求解位姿和置信度[26]。

1.1 IMU誤差濾波器

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

xt=x⊕δx

(1)

1.1.1 預測方程

預測狀態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分別為速度,角度,加速度偏置,角速度偏置高斯白噪聲。

設誤差狀態和擾動向量的雅可比矩陣分別為Fx和Fi,擾動向量的協方差矩陣為Q,建立誤差預測方程(4)。

(4)

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

1.2 雙天線RTK觀測器

1.2.1 位置觀測方程

(5)

其關于δθ的雅可比矩陣為:

(6)

位置觀測雅可比矩陣為:

(7)

圖3 RTK位置觀測模型

1.2.2 速度觀測方程

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

(8)

其關于δθ的雅可比矩陣為:

(9)

其關于δωb的雅可比矩陣為:

(10)

速度觀測雅可比矩陣為:

(11)

1.2.3 航向觀測方程

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

圖4 雙天線RTK航向歸一化

考慮IMU的y軸指向機頭方向,φ關于旋轉四元數q=[q0,q1,q2,q3]T的方程為:

(12)

(13)

其中,φ關于q的雅可比為:

(14)

q關于δθ的雅可比為:

(15)

航向觀測雅可比矩陣為:

(16)

1.3 激光位姿觀測器

(17)

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

(18)

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

1.4 有害數據剔除與濾波

E{rrT}=HP-HT+R

(19)

同時選擇一個閾值λ使得

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

(20)

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

r2=λ(HP-HT+R)

(21)

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

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

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

2.1 電磁干擾描述

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

2.2 基于IMU的運動約束

室外帶電作業機器人主要靠機械臂操作,且本體通常處于靜態執行建模、接線、剝線等作業任務,因而本體不會有高頻振動,對IMU非常友好。因此,可利用IMU實時檢測機器人運動狀態[28]。

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

(22)

(23)

|ωmi-δωbi|<0.2

(24)

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

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

(25)

2.3 濾波處理

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

3 實驗驗證

圖5所示的是自行研制的雙天線RTK/IMU組合導航系統。

圖5 雙天線RTK/IMU組合導航系統

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

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

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

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

圖7 實驗測試流程圖

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

圖8 轉換后的激光點云與地圖重合情況

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

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

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

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

表1 重復定位精度測試結果

4 結 論

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

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

需要注意的是,雙天線RTK與IMU的桿臂值、激光雷達與IMU的靜態位姿,均直接采用結構初值,應用時存在旋轉外參誤差,下一步可結合該誤差的補償研究進而提高系統的定位定向精度。