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

(中國礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州　221116)

基于運(yùn)動(dòng)學(xué)約束的移動(dòng)車輛捷聯(lián)慣導(dǎo)定位方法及實(shí)驗(yàn)研究*

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

(中國礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州221116)

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

關(guān)鍵詞：移動(dòng)車輛；捷聯(lián)慣性導(dǎo)航；運(yùn)動(dòng)學(xué)約束；卡爾曼濾波

0引言

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

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

針對上述問題，本文提出一種基于運(yùn)動(dòng)學(xué)約束的捷聯(lián)慣性導(dǎo)航車輛定位算法。利用移動(dòng)車輛位置和速度建立系統(tǒng)狀態(tài)方程，通過其運(yùn)動(dòng)過程中車體橫向及天向的速度分量為0的運(yùn)動(dòng)學(xué)約束特性來建立觀測方程，運(yùn)用卡爾曼濾波算法對狀態(tài)空間方程進(jìn)行濾波，并搭建捷聯(lián)慣導(dǎo)移動(dòng)車輛定位實(shí)驗(yàn)平臺(tái)，實(shí)現(xiàn)對移動(dòng)車輛定位算法的實(shí)驗(yàn)驗(yàn)證。

1移動(dòng)車輛的定位解算模型

捷聯(lián)慣性導(dǎo)航系統(tǒng)數(shù)字算法是以迭代遞推的形式表示，即采用系統(tǒng)微分方程組的形式進(jìn)行描述，將移動(dòng)車輛過去前一時(shí)刻的導(dǎo)航信息和此時(shí)刻的慣性器件的采樣值作為輸入值，通過逐次遞推計(jì)算出當(dāng)前時(shí)刻的導(dǎo)航信息。

移動(dòng)車輛姿態(tài)更新的四元數(shù)微分方程為[9]：

(1)

(2)

移動(dòng)車輛速度更新的微分方程為[10]：

(3)

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

移動(dòng)車輛位置更新的微分方程為：

(4)

其中L，λ，h分別為移動(dòng)車輛所在位置的緯度，經(jīng)度和高度。vx，vy，vz分別為移動(dòng)車輛在導(dǎo)航坐標(biāo)系下的速度分量，RM，RN分別為當(dāng)?shù)刈游缛Π霃胶兔先Π霃健κ?4)積分即得到其所在位置的緯度、經(jīng)度和高度。

2系統(tǒng)定位數(shù)學(xué)模型建立

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

圖1　導(dǎo)航定位系統(tǒng)原理框圖

2.1運(yùn)動(dòng)學(xué)約束模型建立

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

(5)

圖2　移動(dòng)車輛運(yùn)動(dòng)學(xué)模型示意圖

2.2系統(tǒng)狀態(tài)方程

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

(6)

Bk,k-1為系統(tǒng)的控制輸入矩陣，定義為：

(7)

(8)

其中k為系統(tǒng)的采樣周期計(jì)數(shù)，T為采樣周期。Γk,k-1為系統(tǒng)轉(zhuǎn)移干擾矩陣，在該系統(tǒng)設(shè)為單位陣。ωk為狀態(tài)方程的系統(tǒng)噪聲，其向量形式表示為：

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

2.3系統(tǒng)觀測方程

(9)

從而得到車體坐標(biāo)系b系中的速度分量為：

(10)

zk=Hkxk+υk

(11)

Hk=H1H2

(12)

2.4卡爾曼濾波算法

通過上面的推導(dǎo)可以建立系統(tǒng)狀態(tài)空間方程：

(13)

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

(1)狀態(tài)量的初步更新預(yù)測：

(14)

(2)誤差協(xié)方差的更新預(yù)測：

(15)

(3)狀態(tài)估計(jì)矯正：

(16)

(4)求卡爾曼增益：

(17)

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

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

(18)

上述過程即為卡爾曼濾波的實(shí)現(xiàn)步驟，濾波器每次執(zhí)行都按此過程循環(huán)。

經(jīng)過卡爾曼濾波之后，就能得到導(dǎo)航坐標(biāo)系下k時(shí)刻移動(dòng)車輛的位置最優(yōu)解和速度最優(yōu)解，再次通過系統(tǒng)模型方程(13)便可得到移動(dòng)車輛k+1時(shí)刻移動(dòng)車輛的位置和速度信息。

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

為了驗(yàn)證本文所提出的運(yùn)動(dòng)學(xué)約束條件下卡爾曼濾波算法對移動(dòng)車輛導(dǎo)航系統(tǒng)的定位性能，搭建如圖3所示的移動(dòng)車輛定位實(shí)驗(yàn)平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。通過四元數(shù)位姿解算方法與運(yùn)動(dòng)學(xué)約束條件下卡爾曼濾波算法分別對移動(dòng)車輛的軌跡進(jìn)行跟蹤。

1.上位機(jī) 2.移動(dòng)車輛 3.捷聯(lián)慣性導(dǎo)航 4.遙控操作器 5.無線藍(lán)牙模塊

圖3移動(dòng)車輛實(shí)驗(yàn)平臺(tái)

3.1實(shí)驗(yàn)準(zhǔn)備及參數(shù)設(shè)定

實(shí)驗(yàn)選用型號為IMU800CA-200捷聯(lián)慣性導(dǎo)航，其中陀螺計(jì)零偏穩(wěn)定性為0.025(°/h)，加速度計(jì)零偏穩(wěn)定性為0.15mg，其融合了先進(jìn)的MEMS速率陀螺技術(shù)，具有優(yōu)異的可靠性和穩(wěn)定性；選用無線藍(lán)牙模塊，能夠?qū)INS的數(shù)據(jù)與上位機(jī)實(shí)現(xiàn)實(shí)時(shí)通信，波特率為115200bit/s，從而實(shí)現(xiàn)對移動(dòng)車輛運(yùn)動(dòng)軌跡的實(shí)時(shí)校正；選用基于Arduino可編程無線控制器，實(shí)現(xiàn)對移動(dòng)車輛運(yùn)動(dòng)的實(shí)時(shí)控制；移動(dòng)車輛裝備4路紅外循跡模塊，實(shí)現(xiàn)對移動(dòng)車輛真實(shí)軌跡的測量標(biāo)定。

3.2移動(dòng)車輛位置跟蹤實(shí)驗(yàn)

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

圖4　移動(dòng)車輛行走軌跡

利用捷聯(lián)慣性導(dǎo)航裝置測量出移動(dòng)車輛實(shí)時(shí)運(yùn)行狀態(tài)下的姿態(tài)角和加速度信息，并根據(jù)四元數(shù)法解算出移動(dòng)車輛在運(yùn)行過程中的SINS跟蹤軌跡信息，并與運(yùn)動(dòng)學(xué)約束下的卡爾曼濾波后的軌跡作對比，并且根據(jù)設(shè)置好的基準(zhǔn)參考軌跡，計(jì)算出移動(dòng)車輛的定位誤差，得到此算法下移動(dòng)車輛捷聯(lián)慣導(dǎo)系統(tǒng)的定位精度。

從圖5中可以看出卡爾曼濾波下的定位導(dǎo)航系統(tǒng)在第一次轉(zhuǎn)彎處出現(xiàn)較大的定位誤差這是因?yàn)樵谵D(zhuǎn)彎時(shí)速度方向變化較大；而SINS跟蹤軌跡在移動(dòng)車輛行走第一段直線時(shí)和真實(shí)軌跡相吻合，這主要是因?yàn)镾INS系統(tǒng)在短時(shí)間內(nèi)能夠?qū)崿F(xiàn)高精度定位。

圖5　移動(dòng)車輛軌跡對比圖

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

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

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

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

圖6　卡爾曼濾波定位導(dǎo)航系統(tǒng)的位置誤差曲線

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

4結(jié)束語

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

該輔助算法可用于SINS/GPS組合導(dǎo)航系統(tǒng)中，用于當(dāng)GPS失效時(shí)，實(shí)時(shí)提高純SINS的定位精度，改善SINS/GPS移動(dòng)車輛導(dǎo)航系統(tǒng)的導(dǎo)航精度，提高定位系統(tǒng)的可持續(xù)性。

[參考文獻(xiàn)]

[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慣性導(dǎo)航組合[M].陳軍,易翔,梁高波,等,譯.2版.北京：電子工業(yè)出版社,2011.

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

[4] 鄧琛,王永琦.基于模糊控制的無線傳感器網(wǎng)絡(luò)室內(nèi)定位算法[J].計(jì)算機(jī)應(yīng)用,2011,31(8):2062-2064.

[5] 燕學(xué)智,王樹勛,馬中勝,等.基于超聲紅外定位導(dǎo)航研制自動(dòng)引導(dǎo)車輛系統(tǒng)[J].吉林大學(xué)學(xué)報(bào)：工學(xué)版,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] 秦永元.慣性導(dǎo)航[M].2版.北京：科學(xué)出版社,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] 馬韜,陳杰,陳文頡.對偶四元數(shù)捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)方法[J].北京理工大學(xué)學(xué)報(bào),2012,32(1):56-61.

[10] 魏紀(jì)林.高動(dòng)態(tài)下激光捷聯(lián)慣導(dǎo)系統(tǒng)算法研究與實(shí)現(xiàn)[D].哈爾濱：哈爾濱工業(yè)大學(xué),2012.

[11] 袁亮,楚仕彬.基于卡爾曼濾波的無人機(jī)姿態(tài)測量研究[J].組合機(jī)床與自動(dòng)化加工技術(shù),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

*基金項(xiàng)目：國家高技術(shù)研究發(fā)展計(jì)劃(863)資助項(xiàng)目(2013AA06A411)；江蘇省“333工程”科研資助項(xiàng)目(BRA2015300)；江蘇省研究生培養(yǎng)創(chuàng)新工程資助項(xiàng)目(KYLX_1374)

作者簡介：司卓印(1990—)，男，江蘇徐州人，中國礦業(yè)大學(xué)研究生，研究方向?yàn)橐苿?dòng)裝備組合定位技術(shù)，(E-mail)szy900820@163.com。

中圖分類號：TH166;TG659

文獻(xiàn)標(biāo)識(shí)碼：A