基于捷聯慣導的蛇形管道機器人定位算法

張成林， 王亞慧， 張秋逸

(北京建筑大學電氣與信息工程學院， 北京 100044)

自進入21世紀以來，中國城市化速度大大加快，導致燃氣管網的覆蓋率與使用范圍大大提高，燃氣管道的安全性將直接決定著城市是否能夠安全穩定的運行[1]。為了檢查與維護燃氣管道，鄧蕊等[2]、邢利輝等[3]設計了一種蛇形管道探測機器人，該機器人以14節舵機連接而成，可進行行波、蜿蜒、螺旋等運動，當機器人發現安全隱患和損傷后需要將位置信息回饋給地面上的工作人員，以方便進行定點挖掘和管道維護，為此蛇形管道探測機器人定位系統的準確與否尤為重要。

燃氣管道通常被掩埋于地下，目前應用于地下導航定位的技術主要有射頻識別技術(radio frequency identification，RFID)、無線網絡協議定位技術(ZigBee)、無線相容認證技術(wireless fidelity，WIFI)、超寬帶定位技術(ultra-wideband，UWB)及捷聯式慣性導航定位系統(strapdown inertial navigation system，SINS)技術等[4-6]。Zhang等[7]使用動態主動射頻識別標定系統的射頻識別定位技術，該定位技術使用4個電子讀取儀器、4個相鄰及28個用于參考的標簽，建立了一個改進的LANDMARC算法獲得理想的地下定位效果。但是閱讀器在接收射頻識別信息時，由于參考標簽數量的增加而產生信號沖突，產生沖突的信號強度指示(received signal strength indicator，RSSI)增加了系統的復雜性，使得定位時間大大增加。Zhou等[8]提出了一種基于RSSI值的ZigBee網絡測距定位技術。經過平均模型濾波和高斯濾波模型處理，得到RSSI范圍。但是基于RSSI值的ZigBee測距定位技術方法只能在一些簡單的環境中使用，對于地下管道這種可能有臟物、裂縫的場景可靠度不高。李劉頌等[9]提出了一種基于行人走路時識別姿態變化的慣導算法, 使用慣性測量器件(inertial measurement unit，IMU)采集人行走時的加速度和角速度數據, 根據該算法計算結果建立了行人的步長模型，在保證單步位移較高精度的前提下準確估計了行人的位移情況。但是該算法并未加入誤差補償，長時間進行定位會導致誤差逐漸積累無法獲得準確地位置信息。郭梁等[10]提出了一種基于捷聯慣性導航的礦用單軌吊機車定位算法。利用限幅濾波和去零偏等預處理等方法對數據處理，方向余弦矩陣法消除重力分量對數據的干擾，利用加速度變化率閾值法和零速修正法修正穩態誤差，實現了吊車的精確定位。但是未對機車急停時的加速度誤差進行分析。

針對上述存在的問題，為了能夠在無法接收衛星導航信號的地下管道內對蛇形管道機器人精確定位。采用完全自式的導航定位方法，即捷聯式慣性導航定位算法，在不接收任何外部信號的情況下實現定位功能。利用mpu9250九軸慣性單元采集蛇形機器人的加速度、角速度信息。通過四元數法構造并更新姿態矩陣，對系統和元器件造成的定位誤差進行分析并建模，融合卡爾曼濾波算法進行誤差補償。為了驗證定位算法的可靠性，利用本課題組自主開發設計的蛇形機器人攜帶慣性器件進行數據采集，發現解算后的定位效果滿足實際需求，為蛇形機器人在地下管道內部定位研究提供科學參考依據。

1 捷聯式慣性導航系統常用坐標系

參考坐標系是標定物體位置的重要參照，因此坐標系是捷聯式慣性導航的基礎，SINS經典坐標系，如圖1所示。

Ω為地球自轉角速率；l為赤道線；O點為地心； N、E、U分別為東、北、天3個方向圖1 慣性導航常用坐標系Fig.1 Inertial navigation common coordinate system

(1)地心坐標系(簡稱i系)，i系采用地心O作為其坐標系的原點，xi軸和yi軸相互垂直且位于地球的赤道面內，zi軸垂直于xi軸和yi軸形成的平面，且以北極為zi軸的正向，其坐標系方向并不隨地球的自轉而轉動。

(2)地球坐標系(簡稱e系)，e系同i系相似，均采用地心O作為其坐標系的原點，xe軸和ye形成的平面與地球的赤道面重疊，xe軸指向格林威治子午線方向，e系以地球的自轉軸作為ze軸，隨著地球自轉軸而動。

(3)導航坐標系(簡稱n系)，以相關載體重心O為坐標系原點，以載體的東向為xn軸、以載體的北向為xn軸、以天為xz軸，因此n系也稱為東北天坐標系[11]。

(4)載體坐標系(簡稱b系)，b系與載體相連，將載體重心設為坐標系原點，xb軸為載體呈直線時正前方，yb軸指向載體的側方位，zb軸指向載體并向上。

2 定位系統算法設計

2.1 捷聯慣導算法

SINS摒棄笨重的物理平臺，以牛頓力學定律為基礎，采用加速度計和陀螺儀直接固聯在被測物體上的方式，通過加速度傳感器和陀螺儀實時采集線加速度和角加速度數據，融合卡爾曼濾波算法，根據慣性微分方程, 可對線加速度進行二次積分得到目標物體的位移[12]。利用陀螺儀測得載體在相對慣性空間下3個方向的角速度信息，經過坐標變換，最終算出其姿態信息。SINS的原理如圖2所示。

圖2 捷聯慣導原理圖Fig.2 SINS schematic diagram

SINS中最關鍵的步驟為姿態矩陣的求解，姿態矩陣包含被測物體的位姿和速度等信息，因此被求解的姿態矩陣越逼近真實的姿態矩陣，求解的位置信息就越精準。SINS的誤差主要來源于慣性器件，包括刻度因子誤差、安裝誤差、零偏誤差及隨機誤差等。此外，從加速度得到位置信息，需經兩次積分運算，但是積分運算會將誤差放大，因此引入卡爾曼濾波算法并將其融合蛇形機器人的加速度及角速度信息，有效的補償慣導誤差。

2.2 卡爾曼濾波誤差補償

卡爾曼濾波算法首先需要確定誤差模型，推導出誤差模型后再由卡爾曼濾波進行最優估計，卡爾曼濾波算法流程圖如圖3所示。其中誤差模型包含元件及系統誤差兩種，元件誤差包括加速度計誤差和陀螺儀誤差，系統誤差包括姿態誤差、速度誤差及位置誤差。

k為時間；K為卡爾曼濾波增益加權因子；Kk為卡爾曼濾波增益矩陣；fb為加速度計輸出比力；fn為比力在導航坐標系中的投影；為姿態轉換矩陣；為k時刻系統狀態；為系統上一時刻預測狀態值；Pk為狀態對應的協方差矩陣；Pk,k-1為狀態對應的協方差矩陣；Φk,k-1為系統矩陣圖3 卡爾曼濾波算法誤差補償流程Fig.3 Error compensation process of Kalman filter algorithm

陀螺儀誤差主要包括刻度因數誤差、隨機常數誤差、一階馬爾可夫過程誤差、白噪聲，經分析可以推導出陀螺儀誤差模型為

(1)

加速度計誤差與陀螺儀誤差相似，通過分析可推導出加速度計誤差為

(2)

蛇形機器人未啟動時為靜止狀態，因此是靜基座下的初始對準，在靜基座的前提下可以認為失準角為小角度失準角甚至角度為0°，則可得到精對準誤差模型為

(3)

式(3)中：ΔVE為東向誤差；ΔVN為北向誤差；φN、φE、φU分別為東、北、天3個方向的姿態誤差角；ωie為地球自轉角速率；g為重力加速度；L為緯度；ωie為地球自轉角速率；φE、φN、φU分別為東、北、天3個方向上的姿態誤差角；?x為加速度計零偏在數學平臺x軸的投影；?y為加速度計零偏在數學平臺y軸的投影；ΔL為緯度誤差；R為地球半徑；εE、εN、εU分別為陀螺儀東、北、天方向的陀螺漂移誤差。

IMU元件的偏置誤差及漂移誤差默認為常數，再以東、北、天坐標系為導航坐標系的前提下，則有

(4)

式(4)中：?E為加速度計東向偏置誤差；?N為加速度計北向偏置誤差；?U為加速度計天向偏置誤差；?x、?y、?z分別為載體坐標系3個軸向上的加速度計偏執誤差；εx、εy、εz分別為載體坐標系3個軸向上的脫落漂移誤差。

則可得誤差狀態方程為

(5)

式(5)中:X(t)為系統狀態向量；A為狀態轉移矩陣；W(t)為高斯白噪聲。

Z(t)=HX(t)+V(t)

(6)

式(6)中：Z(t)為量測矩陣；H為常值矩陣；V(t)為隨機噪聲。

式(5)、式(6)的離散形式為

Xk=Φk,k-1Xk-1+Γk,k-1Wk-1

(7)

Zk=HkXk+Vk

(8)

式中：Γk,k-1為噪聲驅動矩陣；Wk-1為過程白噪聲；Xk為k時刻系統狀態量；Zk為k時刻系統的觀測量；Vk為系統量測噪聲矩陣。

Φk,k-1和Γk,k-1可表示為

(9)

式(9)中：T為卡爾曼濾波的時間周期；I為單位陣；G為常量矩陣；Ak-1為前一時刻的系統矩陣。

在tk時刻，量測噪聲矩陣Vk和激勵噪聲矩陣Wk互不相關且為零均值的白噪聲矩陣，則可得到系統噪聲方差矩陣Qk和量測噪聲方差矩陣Rk，可表示為

(10)

將求出的Qk、Rk、Φk,k-1、Γk,k-1及量測矩陣代入卡爾曼濾波即可。

2.3 四階龍格-庫塔法求解四元數微分方程

導航坐標系Oxnynzn分別繞zn軸、xn軸、yn的順序旋轉三次可得到載體坐標系Oxbybzb，則從導航坐標系轉換到載體坐標系的矩陣運算可表示為

(11)

四元數法具有計算量小、精度高、避免產生奇點等優勢。四元數Q是包括一個實部3個虛部的超復數，剛體沿著以時變矢量，旋轉一定的角度[13]，用轉角表示四元數的首項，旋轉的方向由后三項表示，q0、q1、q2、q3為實數，i、j、k為相互正交的單位矢量，其定義為

Q=q0+q1i+q2j+q3k

(12)

龍格-庫塔法是一種具有高精度微分方程的數值解法[14-16]。龍格-塔庫算法采用在積分區間內插值的方式，平滑總斜率。插值越多，得到的結果就越精確，但是工作量也會隨之大大提高。四階龍格-塔庫算法在解算四元數微分方程上，不僅求解結果精準，而且計算量也較少。其原理為

(13)

式(13)中：K1為初始時刻斜率；K2、K3為中間兩點斜率；K4為該段時間終點的斜率；τ為計算周期。

(14)

從式(14)中可以看出，四元數對應姿態矩陣中的不同元素，因此，可以通過四元數解算出姿態更新矩陣及導航信息。其相應的姿態信息可表示為

(15)

式(15)中：θ為載體的俯仰角；γ為載體的橫滾角；φ為載體的航向角。

捷聯慣導算法的速度微分方程為

(16)

位置更新方程為

(17)

此時根據前一刻載體信息得到當前時刻載體位置信息。

3 實驗及結果分析

為了驗證蛇形管道探測機器人定位子系統算法的可行性，搭建了包括蛇形管道探測機器人、直流穩壓電源、筆記本電腦、示波器、慣導芯片等實驗器材的實驗平臺。

實驗數據采集對慣性傳感器的精度提出了較高要求，本實驗采用的MPU9250慣性傳感器有3個16位加速度AD輸出，3個16位陀螺儀AD輸出，3個16位磁力計AD輸出。較MPU6250傳感器多了三軸磁力計，z軸航向角融合磁力傳感器濾波，可以減小漂移誤差，穩定航向角度數據。能夠進行精密的慢速和快速運動跟蹤。

實驗基于蛇形管道探測機器人的行波運動進行測量分析。慣性傳感器采樣間隔為0.1 s，一共采樣67 s，蛇形機器人一共行走了512.54 cm，以平均每秒7.65 cm的速度向正北方向進行行波運動。蛇形機器人的機械結構由14個舵機首尾依次連接而成，將MPU9250傳感器安裝在蛇形機器人的第6個舵機上進行加速度及角速度信息采集。截取慣性傳感器在采集數據時蛇形機器人的部分運動姿態效果如圖4所示。

根據IMU器件采集到的數據，可以得到加速度、角速度及角度時域波形如圖5所示。

圖5 慣性傳感器輸出數據時域波形圖Fig.5 Time domain waveform of IMU output data

蛇形機器人通過偶數位舵機上下擺動來拖帶動奇數位舵機，帶動整體在正北方向呈直線向前運動。經上述算法解算后利用MATLAB仿真軟件的到蛇形機器人在東、北、天3個方向上速度及位移的波形圖如圖6所示。

圖6 解算后的蛇形機器人速度及移變化曲線圖Fig.6 Velocity and displacement curve of snake robot after solution

本實驗在透明管道內進行，管壁底部呈U形，所以在行波運動的過程中會出現輕微的側滑現象，從圖6可以看出，蛇形機器人在x軸方向，即東西兩向最大位移在1 cm左右。z軸方向，即天向呈規律的上下運動，最大位移不超過15 cm。

y軸方向，即北向，蛇形機器人的主要位移方向上的位移距離經算法解算后與實際位移對比得出的誤差如表1所示。

實驗過程中每隔5 s準確記錄一次蛇形機器人的位移距離，與算法解算出的位移進行對比，捷聯式慣性導航算法隨著時間變長會導致誤差不斷累積，從表1中可以看出,定位誤差隨著時間的變化會逐漸增大，67 s內蛇形機器人共行走512.54 cm，實驗過程記錄最大誤差20.51 cm，誤差率為4%。最大誤差率出現在第55秒，誤差率為4.7%，本實驗誤差率在30 s之后逐步收斂至4%～5%，證明卡爾曼濾波算法確實起到了誤差補償作用。

表1 蛇形機器人實際位移及誤差Table 1 Actual displacement and error of snake robot

4 結論

基于捷聯式慣性導航算法，初步為蛇形管道探測機器人設計了一種適用于地下管道內的定位導航算法。為了更有利于觀測蛇形機器人實際的位移，方便與算法解算得到的結果做比較，實驗在透明的玻璃管道內進行，得出如下結論。

(1)通過不同算法的對比與可行性分析，設計了可應用于蛇形管道探測機器人的捷聯慣導定位方法。該方法通過九軸慣性傳感器采集加速度和角速度數據，采用卡爾諾曼濾波算法融合數據、四元數龍格-庫塔算法解算微分方程，大大降低了定位誤差和計算量。

(2)搭建了實驗平臺，實驗結果表明該定位系統能夠在難以接受外界信號的地下管道內，為機器人精確的定位信息。相比于人工查探損傷位置更具安全性、經濟性和準確性。為地下管道內定位提供一種新的思路。