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

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

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

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

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

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

摘要：針對(duì)移動(dòng)車(chē)輛捷聯(lián)慣性導(dǎo)航系統(tǒng)中長(zhǎng)時(shí)間存在累計(jì)誤差使得系統(tǒng)定位精度較低的問(wèn)題，利用卡爾曼濾波算法，提出了一種基于運(yùn)動(dòng)學(xué)約束的移動(dòng)車(chē)輛捷聯(lián)慣性導(dǎo)航算法。該算法利用車(chē)輛運(yùn)動(dòng)參數(shù)信息構(gòu)建狀態(tài)方程，同時(shí)根據(jù)車(chē)輛運(yùn)動(dòng)過(guò)程中的運(yùn)動(dòng)學(xué)約束來(lái)構(gòu)建速度觀測(cè)方程，并搭建實(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，相對(duì)于傳統(tǒng)解算方法定位精度分別提高了84.8%和88.9%，能夠準(zhǔn)確得到移動(dòng)車(chē)輛的位置信息。

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

0引言

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

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

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

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

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

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

(1)

(2)

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

(3)

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

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

(4)

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

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

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

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

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

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

(5)

圖2　移動(dòng)車(chē)輛運(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)觀測(cè)方程

(9)

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

(10)

zk=Hkxk+υk

(11)

Hk=H1H2

(12)

2.4卡爾曼濾波算法

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

(13)

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

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

(14)

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

(15)

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

(16)

(4)求卡爾曼增益：

(17)

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

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

(18)

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

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

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

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

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

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

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

實(shí)驗(yàn)選用型號(hào)為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)定性；選用無(wú)線藍(lán)牙模塊，能夠?qū)INS的數(shù)據(jù)與上位機(jī)實(shí)現(xiàn)實(shí)時(shí)通信，波特率為115200bit/s，從而實(shí)現(xiàn)對(duì)移動(dòng)車(chē)輛運(yùn)動(dòng)軌跡的實(shí)時(shí)校正；選用基于Arduino可編程無(wú)線控制器，實(shí)現(xiàn)對(duì)移動(dòng)車(chē)輛運(yùn)動(dòng)的實(shí)時(shí)控制；移動(dòng)車(chē)輛裝備4路紅外循跡模塊，實(shí)現(xiàn)對(duì)移動(dòng)車(chē)輛真實(shí)軌跡的測(cè)量標(biāo)定。

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

初始時(shí)刻，通過(guò)初始設(shè)定，使得移動(dòng)車(chē)輛的載體坐標(biāo)系和導(dǎo)航坐標(biāo)系完全重合，且車(chē)輛相對(duì)捷聯(lián)慣導(dǎo)安裝偏差角為一個(gè)可忽略的小角度范圍，從而實(shí)現(xiàn)慣性導(dǎo)航與移動(dòng)車(chē)輛的初始對(duì)準(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)車(chē)輛行走的軌跡如圖4所示，分為下面五個(gè)階段：第一段直線OA—第一次拐彎(偏航角為-40.73°)—第二段直線AB—第二次拐彎(偏航角為43.97°)—第三段直線BC。

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

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

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

圖5　移動(dòng)車(chē)輛軌跡對(duì)比圖

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

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

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

純SINS定位位置誤差曲線如圖7所示，起始階段定位精度高，隨著時(shí)間累積，定位精度嚴(yán)重下降，在x方向和y方向最大相對(duì)誤差為0.85m和-1.98m。圖6和圖7對(duì)比可以看出采用運(yùn)動(dòng)學(xué)約束下的卡爾曼濾波定位系統(tǒng)具有很好的誤差收斂能力，雖然在定位初期和噪聲變化下產(chǎn)生兩個(gè)誤差峰值，但定位誤差快速穩(wěn)定下來(lái)，定位精度明顯優(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)車(chē)輛的真實(shí)軌跡，且能滿(mǎn)足SINS移動(dòng)車(chē)輛系統(tǒng)長(zhǎng)時(shí)間的導(dǎo)航要求，是一種可行的定位濾波算法。

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

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

4結(jié)束語(yǔ)

針對(duì)移動(dòng)車(chē)輛純SINS定位系統(tǒng)長(zhǎng)時(shí)間運(yùn)行時(shí)存在累積誤差導(dǎo)致位置發(fā)散的問(wèn)題，本文提出一種基于速度的運(yùn)動(dòng)學(xué)約束條件下的卡爾曼濾波算法，并且在智能移動(dòng)小車(chē)上搭建了捷聯(lián)慣導(dǎo)定位實(shí)驗(yàn)平臺(tái)。實(shí)驗(yàn)結(jié)果表明，該算法的定位精度相對(duì)于純捷聯(lián)慣導(dǎo)解算模型，在x方向和y方向上分別提高84.8%和88.9%，并且提高了SINS自主導(dǎo)航的持續(xù)性和穩(wěn)定性。本文的研究算法對(duì)提高導(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)車(chē)輛導(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ú)線傳感器網(wǎng)絡(luò)定位理論和算法[J].計(jì)算機(jī)研究與發(fā)展,2011,48(3):353-363.

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

[5] 燕學(xué)智,王樹(shù)勛,馬中勝,等.基于超聲紅外定位導(dǎo)航研制自動(dòng)引導(dǎo)車(chē)輛系統(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] 馬韜,陳杰,陳文頡.對(duì)偶四元數(shù)捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(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] 袁亮,楚仕彬.基于卡爾曼濾波的無(wú)人機(jī)姿態(tài)測(cè)量研究[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

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

DOI:10.13462/j.cnki.mmtamt.2016.06.016

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

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

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

中圖分類(lèi)號(hào)：TH166;TG659

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