王曉錚,王亞慧,張成林

(1.北京建筑大學(xué),機(jī)器人仿生與功能研究北京市重點(diǎn)實(shí)驗(yàn)室,北京 100044;2.北京建筑大學(xué)電氣與信息工程學(xué)院,北京 102616)

0 引言

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

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

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

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

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

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

圖2 融合定位算法原理

2 雙慣導(dǎo)+編碼器里程計(jì)融合定位算法模型

2.1 坐標(biāo)系建立

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

圖3 管道內(nèi)蛇形探測機(jī)器人的數(shù)學(xué)模型及運(yùn)動(dòng)軌跡

圖4 蛇形機(jī)器人本體簡化模型

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

(1)

得到其變換矩陣如下:

(2)

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

(3)

(4)

(5)

(6)

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

2.2 擴(kuò)展卡爾曼濾波模型

(7)

(8)

(9)

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

(10)

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

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

(11)

設(shè)計(jì)擴(kuò)展卡爾曼濾波器狀態(tài)方程如下:

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

(12)

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

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

(13)

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

(14)

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

Z(k)=[r12]

(15)

r12表達(dá)式如下:

(16)

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

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

(17)

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

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

(18)

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

(19)

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

3 方法驗(yàn)證

3.1 仿真實(shí)驗(yàn)

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

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

(a)東北平面定位軌跡

(a)東北平面定位軌跡

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

表1 慣導(dǎo)1及融合定位算法的定位誤差比較 m

表2 慣導(dǎo)2及融合定位算法的定位誤差比較 m

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

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

圖8 慣導(dǎo)安裝距離對定位算法的精度影響

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

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

(20)

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

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

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

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

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

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

圖10 蛇形機(jī)器人坐標(biāo)系下軌跡圖

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

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

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

4 結(jié)論

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

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

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

(3)為了驗(yàn)證算法的可靠性,利用MATLAB進(jìn)行仿真,并搭建實(shí)驗(yàn)平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)結(jié)果證明雙慣導(dǎo)+編碼器里程計(jì)融合算法相較于單慣導(dǎo)定位精度更高,可以有效降低單慣導(dǎo)長時(shí)間工作導(dǎo)致的累積誤差,且在機(jī)器人運(yùn)行條件和慣導(dǎo)間安裝距離變化時(shí)均有效,能夠?yàn)樯咝螜C(jī)器人在地下燃?xì)夤艿纼?nèi)部檢測時(shí)提供可靠的定位信息,為實(shí)際工程應(yīng)用奠定了基礎(chǔ)。