基于慣性導航的管道探測蛇形機器人定位算法

王曉錚,王亞慧,張成林

(1.北京建筑大學,機器人仿生與功能研究北京市重點實驗室,北京 100044;2.北京建筑大學電氣與信息工程學院,北京 102616)

0 引言

管道作為燃氣運輸?shù)闹饕绞?具有安全性高、運輸方便等優(yōu)點。然而隨著全球燃氣規(guī)模不斷擴大,地下管線日益增多,異常復雜,難以進行日常巡檢和維護,特別是管道內(nèi)部的探測[1]。為了預防管道事故,天津大學開發(fā)了一種高精度數(shù)據(jù)采集和監(jiān)控系統(tǒng),目前已應用于管線泄漏檢查[2];鄧蕊等[3]、邢利輝等[4]設計了一種管道探測蛇形機器人,將檢測到的管道內(nèi)部環(huán)境信息上傳至上位機,以便工作人員確定是否進行管道維護或開挖。因此對于管道內(nèi)機器人的定位是至關(guān)重要的。

目前應用于管道無損檢測中的定位技術(shù)有探地雷達法、電磁法和磁強計法等,然而這些方法的定位精度往往會受到外部環(huán)境影響。基于慣性器件的定位導航技術(shù)是一種完全自主式定位,具有短時間內(nèi)定位精度高且不受環(huán)境影響的優(yōu)點[5]。張成林等[6]為檢測燃氣管道內(nèi)部設計了一種基于離散卡爾曼濾波算法的高精度捷聯(lián)慣性定位系統(tǒng),其定位誤差最大不超過4.7%。然而慣性器件的定位誤差會隨著時間的推移不斷累積,因而在實際應用中大多采用與其他傳感器相結(jié)合的組合定位導航方式對慣導數(shù)據(jù)進行修正[7]。劉鴻宇[8]以微慣性測量單元與里程輪相結(jié)合完成地下管道機器人定位,有效提高了管線定位精度,但在工作過程中管道內(nèi)壁往往存在焊縫或積存雜物等致使里程輪出現(xiàn)打滑甚至脫離與管道接觸,從而影響測速導致定位誤差增加。向泓銘[9]為進行管道內(nèi)部機器人的精準定位,融合視覺與慣性定位,解決了慣導誤差積累問題。但是融合視覺定位依賴于信標建立,對于長直管道等無法建立信標的環(huán)境,難以利用周圍環(huán)境來判斷位置信息。此外,一些研究人員通過濾波算法來減小慣性導航的定位誤差。P.Zhang[10]減少了由慣導系統(tǒng)和里程計安裝位置以及比例因子引起的誤差。Y.Huang[11]提出了一種用于里程計輔助慣性導航系統(tǒng)的卡爾曼濾波方法。實驗結(jié)果表明,所提出的方法可以更好地估計從當前坐標系到初始坐標系的姿態(tài)矩陣。

為保證定位精度,減小誤差積累,并解決上述文獻中存在的實際問題,本文提出了一種基于擴展卡爾曼濾波算法的雙慣導+地表編碼器里程計融合定位算法。系統(tǒng)以兩套慣導的距離為約束,結(jié)合地表編碼器里程計測得蛇形機器人位移增量信息,再以擴展卡爾曼濾波算法建立管道探測蛇形機器人定位系統(tǒng)模型,以補償單慣導定位隨時間產(chǎn)生的累積誤差,從而提高了管道探測蛇形機器人的導航定位精度,達到高精度導航定位的目的。

1 系統(tǒng)定位原理

系統(tǒng)硬件單元連接如圖1所示。燃氣管道探測蛇形機器人以行波運動在管道內(nèi)部行走,慣導系統(tǒng)安裝在“蛇身”,通過有線電纜將慣導解算信息傳輸?shù)缴衔粰C監(jiān)控中心,另外在信號傳輸線上加裝一根尼龍繩并行。編碼器里程計由地表上位于管道閘井入口處的繞線盤和旋轉(zhuǎn)編碼器組成,將尼龍繩纏繞于繞線盤上,隨著蛇形機器人在管道內(nèi)部的移動帶動繞線盤轉(zhuǎn)動,利用安裝在繞線盤上的旋轉(zhuǎn)編碼器即可獲得蛇形機器人在管道內(nèi)部運行速度及位移增量。

根據(jù)兩套慣導系統(tǒng)輸出的姿態(tài)角解算出蛇形機器人從載體坐標系到參考坐標系的方向余弦矩陣。同時通過地表上編碼器里程計解算出蛇形機器人在載體坐標系下的速度和位移增量,再利用方向余弦矩陣將其轉(zhuǎn)換到參考坐標系下,并累計到兩套慣導系統(tǒng)的初始位置上,獲得雙慣導系統(tǒng)在參考坐標系下的位置。將慣導系統(tǒng)初始位置作為狀態(tài)量,編碼器測得機器人位移增量作為控制量,慣導間的距離作為量測量引入擴展卡爾曼濾波器,經(jīng)過擴展卡爾曼濾波獲得兩套慣導系統(tǒng)的位置,從而輸出地下管道內(nèi)部蛇形機器人位姿信息。融合算法定位原理如圖2所示。

圖2 融合定位算法原理

2 雙慣導+編碼器里程計融合定位算法模型

2.1 坐標系建立

建立管內(nèi)蛇形機器人本體的數(shù)學定位模型[12],并建立坐標系,如圖3所示。參考坐標系為東-北-天坐標系(O-XnYnZn),Xn指向北向,Yn指向東向,Zn垂直于Xn軸和Yn軸指向天向。載體坐標系(O-XbYbZb)以蛇形機器人機身幾何中心為坐標原點O,其中Xb為機器人的軸向方向,其與管道的軸向方向一致,Yb垂直于管道軸向,Zb為垂直方向。k為慣導系統(tǒng)采樣時間。

圖3 管道內(nèi)蛇形探測機器人的數(shù)學模型及運動軌跡

圖4 蛇形機器人本體簡化模型

假設{Ob}繞負z軸旋轉(zhuǎn)φ度角,坐標系定義為{rXb′,rYb′,rZb′},表示如下:

(1)

得到其變換矩陣如下:

(2)

同理,圍繞X旋轉(zhuǎn)θ角,圍繞Y軸旋轉(zhuǎn)γ角得到的變換矩陣如下:

(3)

(4)

(5)

(6)

將慣導系統(tǒng)的角度信息輸入到式(6)中,便可以描述蛇形機器人姿態(tài)信息。

2.2 擴展卡爾曼濾波模型

(7)

(8)

(9)

則慣導系統(tǒng)在參考坐標系下的參考位置如下:

(10)

選取兩套慣導系統(tǒng)在參考坐標系下的位置坐標為狀態(tài)量X(k),X(k)表達式如下:

X(k)=[N1(k)E1(k)U1(k)N2(k)E2(k)U2(k)]

(11)

設計擴展卡爾曼濾波器狀態(tài)方程如下:

X(k+1)=Φ(k+1|||k)X(k)+D(k)+W(k)

(12)

式中:X(k)為系統(tǒng)在k時刻的狀態(tài),X(k)∈Rn;W(k)為k時刻系統(tǒng)過程白噪聲;Φ(k+1|||k)為k時刻到k+1時刻的狀態(tài)轉(zhuǎn)移矩陣,其表達式如下:

Φ(k+1|||k)=diag[1 1 1 1 1 1]

(13)

D(k)為k時刻控制矩陣,其表達式如下:

(14)

使兩套慣導系統(tǒng)固定在機身上,選取2個慣導之間的距離r12為卡爾曼濾波器觀測量Z(k):

Z(k)=[r12]

(15)

r12表達式如下:

(16)

假設k時刻的觀測噪聲為V(k),離散非線性系統(tǒng)觀測方程可以表示為

Z(k)=h[k,X(k)]+V(k)

(17)

式中Z(k)為對應狀態(tài)的觀測信號,Z(k)∈Rm。

Z(k)=H(k)X(k)+V(k)

(18)

其中量測矩陣H(k)為h(*)的雅可比矩陣,忽略高階余項可得其表達式為

(19)

根據(jù)上述建立的狀態(tài)方程和量測方程,對線性化后的模型建立擴展卡爾曼濾波遞推方程如文獻[14]所示,即可根據(jù)機器人前一時刻位置信息得到當前時刻位置信息,從而獲得機器人的定位信息。

3 方法驗證

3.1 仿真實驗

為驗證融合方案的定位效果,本節(jié)采用MATLAB軟件對機器人的運動軌跡進行仿真。仿真參數(shù)設置:仿真總時長為5 000 s,周期1 s,設置編碼器測得機器人運行速度為0.1 m/s,設置蛇形機器人上兩套慣導的坐標:慣導1坐標為(0,0,0),慣導2坐標為(0.5,0,0)。兩套慣導系統(tǒng)的姿態(tài)角誤差均設置為由0.5°的常值誤差、頻率1 Hz、振幅0.5°的周期性誤差和方差為0.25的白噪聲組成的簡化模型。慣導1姿態(tài)角誤差漂移方向為正,慣導2為負。

圖5、圖6為蛇形機器人航向角和俯仰角均為0°時,雙慣導+編碼器里程計融合定位算法處理前后單慣導的定位軌跡與理想定位軌跡的對比。從圖5、圖6可以看出,隨著運行距離增加,單慣導定位誤差逐漸增大,具有明顯的誤差累計現(xiàn)象,而融合定位算法的定位軌跡,隨著運行時間的增加其誤差累積現(xiàn)象不明顯,定位軌跡與理想定位軌跡重合度高,其定位誤差相較于單慣導定位誤差明顯減小。

(a)東北平面定位軌跡

(a)東北平面定位軌跡

單慣導定位誤差與融合定位算法的定位誤差對比如表1、表2所示,可看出融合定位方法的定位誤差均小于單慣導定位方法。

表1 慣導1及融合定位算法的定位誤差比較 m

表2 慣導2及融合定位算法的定位誤差比較 m

為測試慣導安裝距離與仿真航向角對定位結(jié)果的影響,采用球概率誤差(SPE)[15]評估仿真實驗定位精度,仿真結(jié)果如圖7和圖8所示。

圖7 航向角對定位算法的精度影響

圖8 慣導安裝距離對定位算法的精度影響

球概率誤差(SPE)計算式如下:

SPE=0.51(σE+σN+σU)

(20)

式中:σE為東向定位誤差均方根;σN為北向定位誤差均方根,σU為天向定位誤差均方根。

圖7為蛇形機器人俯仰角為0°時航向角對定位精度的影響。圖7中,隨著航向角的變化,單慣導的球概率誤差為1.36～1.71 m,而融合定位算法的球概率誤差為0.05～0.11 m。圖8為蛇形機器人俯仰角、航向角均為0°時慣導之間的安裝距離對定位精度的影響。可以看出,單慣導的定位球概率誤差約為1.35 m,而融合定位算法的定位球概率誤差約為0.11 m。實驗結(jié)果證明,基于雙慣導+編碼器里程計的融合定位算法能夠在機器人不同運行條件下以及慣導不同的安裝位置下均減小定位誤差。

3.2 實驗驗證

實驗平臺搭建如下:管道探測蛇形機器人樣機、JY901慣性導航模塊、繞線盤、旋轉(zhuǎn)編碼器、5 V直流穩(wěn)壓電源、裝有上位機系統(tǒng)的筆記本電腦以及通訊線纜若干等。慣導1安裝在蛇形機器人機身原點,其定位信息可以準確代表機器人位置信息,慣導2坐標為(0.1,0,0)。實驗地點為學校內(nèi)部,總長度261 m,機器人按順時針方向前進。行駛路線如圖9所示。

圖9 蛇形機器人行駛路線規(guī)劃圖

在實驗過程中蛇形機器人以行波、勻速的方式向前行進。兩套慣導模塊實時同步采集蛇形機器人運行時產(chǎn)生的加速度、角速度等信息。將蛇形機器人尾部的安全牽引尼龍繩纏繞于繞線盤上,通過旋轉(zhuǎn)編碼器獲得蛇形機器人運行速度及位移增量,并通過有線電纜將慣導系統(tǒng)和編碼器采集到的信息上傳至上位機終端進行解算。經(jīng)過單慣導算法和融合定位算法各自進行解算,將解算得到的數(shù)據(jù)進行擬合可以分別得出其定位軌跡,如圖10所示。

圖10 蛇形機器人坐標系下軌跡圖

由圖10可以看出,雙慣導融合算法解算的軌跡較單慣導定位算法導航偏差更小,解算的軌跡更加貼合實際的行駛軌跡,且雙慣導融合方案解算的定位軌跡尾端與真實的蛇形機器人定位軌跡更加契合。2種算法定位誤差如表3所示。

表3 2種定位算法的誤差范圍 m

從表3可以看出,該融合算法定位誤差波動范圍較小,其北向定位誤差在-1.47～2.12 m之間,而單慣導定位算法北向定位誤差在-3.65～11.82 m之間。雙慣導定位算法東向定位誤差在-2.04～2.33 m之間,而單慣導定位算法東向定位誤差在-10.51～15.27 m之間。從東北兩向的定位誤差結(jié)果分析可知,雙慣導+編碼器里程計融合定位較單慣導定位準確率高,更加適用于蛇形機器人在地下燃氣管道內(nèi)部定位。

4 結(jié)論

為保證管道探測蛇形機器人定位精度,提出了基于擴展卡爾曼濾波的雙慣導+編碼器里程計融合算法。論文主要工作內(nèi)容如下:

(1)為獲取地下管道內(nèi)部蛇形機器人精準定位信息,提出了一種融合算法。以慣導系統(tǒng)的位置為狀態(tài)量,融合機器人運動過程中位移增量作為狀態(tài)方程控制量進行誤差補償,將兩套慣導之間的距離作為觀測量,建立擴展卡爾曼濾波方程。改善了單慣導隨時間產(chǎn)生的誤差累積現(xiàn)象。

(2)為測得管道內(nèi)部機器人單位時間內(nèi)位移增量,使用繞線盤+旋轉(zhuǎn)編碼器構(gòu)成的編碼器里程計,有效避免了傳統(tǒng)輪式里程計產(chǎn)生的打滑誤差。

(3)為了驗證算法的可靠性,利用MATLAB進行仿真,并搭建實驗平臺進行實驗驗證。實驗結(jié)果證明雙慣導+編碼器里程計融合算法相較于單慣導定位精度更高,可以有效降低單慣導長時間工作導致的累積誤差,且在機器人運行條件和慣導間安裝距離變化時均有效,能夠為蛇形機器人在地下燃氣管道內(nèi)部檢測時提供可靠的定位信息,為實際工程應用奠定了基礎。