基于運動學約束的移動車輛捷聯慣導定位方法及實驗研究*

司卓印，李　威，楊　海，張金堯

(中國礦業大學 機電工程學院，江蘇 徐州　221116)

基于運動學約束的移動車輛捷聯慣導定位方法及實驗研究*

司卓印，李威，楊海，張金堯

(中國礦業大學 機電工程學院，江蘇 徐州221116)

摘要：針對移動車輛捷聯慣性導航系統中長時間存在累計誤差使得系統定位精度較低的問題，利用卡爾曼濾波算法，提出了一種基于運動學約束的移動車輛捷聯慣性導航算法。該算法利用車輛運動參數信息構建狀態方程，同時根據車輛運動過程中的運動學約束來構建速度觀測方程，并搭建實驗平臺進行實驗驗證。實驗結果表明，該方法能夠有效抑制捷聯慣導定位誤差發散現象，x方向和y方向的位置誤差分別為0.0328m和-0.049m，相對于傳統解算方法定位精度分別提高了84.8%和88.9%，能夠準確得到移動車輛的位置信息。

關鍵詞：移動車輛；捷聯慣性導航；運動學約束；卡爾曼濾波

0引言

隨著工業自動化技術的發展，移動車輛定位技術受到越來越多的關注，其應用領域和范圍也在不斷的擴展，在面對一些較復雜的任務時，如工廠搬運車輛的智能監控、軍事移動裝備定位、救援機器人等，定位技術發揮著越來越重要的作用。由于應用環境的特殊性存在諸多未知因素，對定位技術提出了更高的要求。目前移動車輛定位技術主要有捷聯慣性導航系統(Strap down Inertial Navigation System，SINS)[1]、全球定位系統(Global positioning systems,GPS)[2]、無線傳感器網絡(Wireless Sensors Network,WSN)[3-4]、紅外線定位技術[5]、視覺定位技術[6]等。

現如今對移動車輛進行定位的主要技術是GPS定位，但GPS無法在室內和建筑物密集區域使用；而紅外線定位技術只能在直線視距內應用，其傳輸距離較短，且容易受外界環境的干擾，局限性比較大；捷聯慣性導航系統不依賴任何外界信息，也不向外發射信息，利用測量載體自身的加速度和角速度信息，通過四元數法解算來實時獲取載體位姿信息[7]，具有短時間高精度，數據更新率高，可實現自主定位導航等特點。然而慣性導航在長時間工作過程中不可避免地存在固有的累積誤差，從而導致測量精度嚴重下降。針對這一缺陷，文獻[8]提出對移動車輛進行間歇停車操作，利用停車時刻的真實速度為零對慣性導航系統中的誤差進行辨識和補償，但是這種校正方法必須進行停車，不能夠滿足實際工況要求。

針對上述問題，本文提出一種基于運動學約束的捷聯慣性導航車輛定位算法。利用移動車輛位置和速度建立系統狀態方程，通過其運動過程中車體橫向及天向的速度分量為0的運動學約束特性來建立觀測方程，運用卡爾曼濾波算法對狀態空間方程進行濾波，并搭建捷聯慣導移動車輛定位實驗平臺，實現對移動車輛定位算法的實驗驗證。

1移動車輛的定位解算模型

捷聯慣性導航系統數字算法是以迭代遞推的形式表示，即采用系統微分方程組的形式進行描述，將移動車輛過去前一時刻的導航信息和此時刻的慣性器件的采樣值作為輸入值，通過逐次遞推計算出當前時刻的導航信息。

移動車輛姿態更新的四元數微分方程為[9]：

(1)

(2)

移動車輛速度更新的微分方程為[10]：

(3)

其中：fb是加速度計測量的比力；gn為重力加速度矢量。

移動車輛位置更新的微分方程為：

(4)

其中L，λ，h分別為移動車輛所在位置的緯度，經度和高度。vx，vy，vz分別為移動車輛在導航坐標系下的速度分量，RM，RN分別為當地子午圈半徑和卯酉圈半徑。對式(4)積分即得到其所在位置的緯度、經度和高度。

2系統定位數學模型建立

本文主要針對SINS移動車輛定位系統中定位誤差隨時間累積的問題，提出的基于運動學約束的卡爾曼濾波誤差補償模型，如圖1所示。慣性導航系統固定安裝在移動車輛上，實時測量移動車輛的三軸加速度和三軸角速度，并且通過慣性導航四元數解算模型實時解算出其位置和姿態信息，建立系統狀態方程。根據運動學約束特性建立觀測方程，并進行卡爾曼濾波，實現對捷聯慣導定位誤差的矯正。

圖1　導航定位系統原理框圖

2.1運動學約束模型建立

移動車輛的載體坐標系為b系，Yb軸指向車輛行駛正前方，Xb軸指向右側，Zb軸豎直向上，并規定導航坐標系下Xn、Yn、Zn軸的指向依次為東、北、天，其運動學模型如圖2所示。移動車輛運動學約束條件是指當車輛在水平面正常行駛過程中，如果沒有發生跳躍和側滑，則在b系中Xb軸速度、Zb軸速度分量為零，即：

(5)

圖2　移動車輛運動學模型示意圖

2.2系統狀態方程

xk=Φk,k-1xk-1+Bk,k-1Uk-1+Γk,k-1ωk-1

(6)

Bk,k-1為系統的控制輸入矩陣，定義為：

(7)

(8)

其中k為系統的采樣周期計數，T為采樣周期。Γk,k-1為系統轉移干擾矩陣，在該系統設為單位陣。ωk為狀態方程的系統噪聲，其向量形式表示為：

其統計特性為：E[ωk]=0，Rωω(k,j)=Qkδkj，Qk為系統噪聲的協方差。B1可定義為：

2.3系統觀測方程

(9)

從而得到車體坐標系b系中的速度分量為：

(10)

zk=Hkxk+υk

(11)

Hk=H1H2

(12)

2.4卡爾曼濾波算法

通過上面的推導可以建立系統狀態空間方程：

(13)

根據卡爾曼濾波的原理[11]，卡爾曼濾波算法流程如下：

(1)狀態量的初步更新預測：

(14)

(2)誤差協方差的更新預測：

(15)

(3)狀態估計矯正：

(16)

(4)求卡爾曼增益：

(17)

(5)誤差協方差矩陣的更新：

Pk|k=[I-KkHk]Pk|k-1

(18)

上述過程即為卡爾曼濾波的實現步驟，濾波器每次執行都按此過程循環。

經過卡爾曼濾波之后，就能得到導航坐標系下k時刻移動車輛的位置最優解和速度最優解，再次通過系統模型方程(13)便可得到移動車輛k+1時刻移動車輛的位置和速度信息。

3實驗驗證

為了驗證本文所提出的運動學約束條件下卡爾曼濾波算法對移動車輛導航系統的定位性能，搭建如圖3所示的移動車輛定位實驗平臺進行實驗驗證。通過四元數位姿解算方法與運動學約束條件下卡爾曼濾波算法分別對移動車輛的軌跡進行跟蹤。

1.上位機 2.移動車輛 3.捷聯慣性導航 4.遙控操作器 5.無線藍牙模塊

圖3移動車輛實驗平臺

3.1實驗準備及參數設定

實驗選用型號為IMU800CA-200捷聯慣性導航，其中陀螺計零偏穩定性為0.025(°/h)，加速度計零偏穩定性為0.15mg，其融合了先進的MEMS速率陀螺技術，具有優異的可靠性和穩定性；選用無線藍牙模塊，能夠將SINS的數據與上位機實現實時通信，波特率為115200bit/s，從而實現對移動車輛運動軌跡的實時校正；選用基于Arduino可編程無線控制器，實現對移動車輛運動的實時控制；移動車輛裝備4路紅外循跡模塊，實現對移動車輛真實軌跡的測量標定。

3.2移動車輛位置跟蹤實驗

初始時刻，通過初始設定，使得移動車輛的載體坐標系和導航坐標系完全重合，且車輛相對捷聯慣導安裝偏差角為一個可忽略的小角度范圍，從而實現慣性導航與移動車輛的初始對準。實驗初始位置為x0=(0 0 0)m；初始速度為v0=(0 0 0)m/s；初始姿態角為ψ=0°，θ=0°，γ=0°；實驗采集3000個采樣點，采樣周期為0.01s。本實驗移動車輛行走的軌跡如圖4所示，分為下面五個階段：第一段直線OA—第一次拐彎(偏航角為-40.73°)—第二段直線AB—第二次拐彎(偏航角為43.97°)—第三段直線BC。

圖4　移動車輛行走軌跡

利用捷聯慣性導航裝置測量出移動車輛實時運行狀態下的姿態角和加速度信息，并根據四元數法解算出移動車輛在運行過程中的SINS跟蹤軌跡信息，并與運動學約束下的卡爾曼濾波后的軌跡作對比，并且根據設置好的基準參考軌跡，計算出移動車輛的定位誤差，得到此算法下移動車輛捷聯慣導系統的定位精度。

從圖5中可以看出卡爾曼濾波下的定位導航系統在第一次轉彎處出現較大的定位誤差這是因為在轉彎時速度方向變化較大；而SINS跟蹤軌跡在移動車輛行走第一段直線時和真實軌跡相吻合，這主要是因為SINS系統在短時間內能夠實現高精度定位。

圖5　移動車輛軌跡對比圖

在小車行走到第二段直線時，卡爾曼濾波下的定位導航系統能夠很好地跟蹤移動車輛的真實軌跡，然而SINS跟蹤軌跡在此階段由于累計誤差導致定位精度有所下降，位置誤差為0.46m。

移動車輛在第二次轉彎時，由于小車的速度波動，導致卡爾曼濾波下的定位導航系統在短時間出現波動現象，而此后能夠很好地跟蹤小車的真實軌跡，直至小車停止；此階段由于SINS累積誤差的影響，導致定位精度嚴重下降，無法滿足定位要求。

卡爾曼濾波定位導航系統的位置誤差曲線如圖6所示，在x方向和y方向(移動車輛前進方向)上均有誤差，最大相對誤差分別為-0.11m和0.26m，且在兩次轉彎過程中明顯看出相對誤差波動性比較大，這主要是因為轉彎時速度方向變化較大。而在小車運行到終點的過程中均保持較高定位精度，到達終點時x方向和y方向上的相對誤差分別為0.0328m和-0.049m，滿足實際的工程要求。

純SINS定位位置誤差曲線如圖7所示，起始階段定位精度高，隨著時間累積，定位精度嚴重下降，在x方向和y方向最大相對誤差為0.85m和-1.98m。圖6和圖7對比可以看出采用運動學約束下的卡爾曼濾波定位系統具有很好的誤差收斂能力，雖然在定位初期和噪聲變化下產生兩個誤差峰值，但定位誤差快速穩定下來，定位精度明顯優于純SINS導航定位系統。在x方向和y方向上較純SINS解算的定位誤差分別減少84.8%和88.9%。引入運動學束條件下的卡爾曼濾波算法有效地抑制了純SINS誤差隨時間積累而發散的現象，使得SINS能夠很好地跟蹤移動車輛的真實軌跡，且能滿足SINS移動車輛系統長時間的導航要求，是一種可行的定位濾波算法。

圖6　卡爾曼濾波定位導航系統的位置誤差曲線

圖7　純SINS定位位置誤差曲線

4結束語

針對移動車輛純SINS定位系統長時間運行時存在累積誤差導致位置發散的問題，本文提出一種基于速度的運動學約束條件下的卡爾曼濾波算法，并且在智能移動小車上搭建了捷聯慣導定位實驗平臺。實驗結果表明，該算法的定位精度相對于純捷聯慣導解算模型，在x方向和y方向上分別提高84.8%和88.9%，并且提高了SINS自主導航的持續性和穩定性。本文的研究算法對提高導航系統的定位性能和應用有一定的指導意義。

該輔助算法可用于SINS/GPS組合導航系統中，用于當GPS失效時，實時提高純SINS的定位精度，改善SINS/GPS移動車輛導航系統的導航精度，提高定位系統的可持續性。

[參考文獻]

[1] Fan Qigao, Li Wei,Luo Chengming.Error analysis and reduction for shearer positioning using the strapdown inertial navigation system[J].International Journal of Computer Science Issues,2012,9(5):49-54.

[2] Mohinder S Grewal,Lawrence R Weill,Augus P Andrews,et al.GPS慣性導航組合[M].陳軍,易翔,梁高波,等,譯.2版.北京：電子工業出版社,2011.

[3] 王小平,羅軍,沈昌祥.無線傳感器網絡定位理論和算法[J].計算機研究與發展,2011,48(3):353-363.

[4] 鄧琛,王永琦.基于模糊控制的無線傳感器網絡室內定位算法[J].計算機應用,2011,31(8):2062-2064.

[5] 燕學智,王樹勛,馬中勝,等.基于超聲紅外定位導航研制自動引導車輛系統[J].吉林大學學報：工學版,2006,36(2):242-246.

[6] Li J Y, Yang C,Wang J,et al.A Robot Handling System Based on Visual Orientation[J].Manufacturing Automation, 2011.33(2):40-42.

[7] 秦永元.慣性導航[M].2版.北京：科學出版社,2014.

[8] Park SK,SuhY S.A zero velocity detection algorithm using inertial sensors for pedestrian navigation systems.[J].Sensors,2010,10(10):9163-9178.

[9] 馬韜,陳杰,陳文頡.對偶四元數捷聯慣性導航系統初始對準方法[J].北京理工大學學報,2012,32(1):56-61.

[10] 魏紀林.高動態下激光捷聯慣導系統算法研究與實現[D].哈爾濱：哈爾濱工業大學,2012.

[11] 袁亮,楚仕彬.基于卡爾曼濾波的無人機姿態測量研究[J].組合機床與自動化加工技術,2015(7):110-113.

(編輯趙蓉)

Experimental Study on SINS Positioning Technology for Mobile Vehicle Based on Kinematics Constraint

SI Zhuo-yin，LI Wei，YANG Hai，ZHANG Jin-yao

(School of Mechatronic Engineering,China University of Mining and Technology,Xuzhou Jiangsu 221116,China)

Abstract：A solution method for mobile vehicle based on kinematics constraint using Kalman filter(KF) is proposed to solve the problem of low positioning accuracy of mobile vehicle in terms of SINS(Strap down Inertial Navigation System)existing the cumulative error with long-term.The state equation of KF is constructed by the mobile vehicle parameters information and the observation equation of velocity is constructed by the process of the kinematics constraint.Meanwhile,the test platform is constructed for the experiment.Results indicate that this method can restrain effectively the position error divergence of the SINS.The position error of mobile vehicle is 0.0328m and -0.049m in x and y direction respectively.Compared with the traditional method,the position accuracy improves 84.8% and 88.9% respectively,which means that the solution method for mobile vehicle based on kinematics constraint using KF can track the position effectively

Key words：mobile vehicle；strap down inertial navigation system(SINS)；kinematics constraint；kalman filter

文章編號：1001-2265(2016)06-0063-04

DOI:10.13462/j.cnki.mmtamt.2016.06.016

收稿日期：2015-12-14;修回日期：2016-01-11

*基金項目：國家高技術研究發展計劃(863)資助項目(2013AA06A411)；江蘇省“333工程”科研資助項目(BRA2015300)；江蘇省研究生培養創新工程資助項目(KYLX_1374)

作者簡介：司卓印(1990—)，男，江蘇徐州人，中國礦業大學研究生，研究方向為移動裝備組合定位技術，(E-mail)szy900820@163.com。

中圖分類號：TH166;TG659

文獻標識碼：A