露天礦區(qū)里程計(jì)輔助的GPS/INS車輛導(dǎo)航算法研究

高井祥李增科

（1.中國礦業(yè)大學(xué)環(huán)境與測繪學(xué)院，江蘇 徐州221116;2.中國礦業(yè)大學(xué)江蘇省資源環(huán)境信息工程重點(diǎn)實(shí)驗(yàn)室，江蘇 徐州221116）

0 引 言

露天礦區(qū)車輛利用GPS/INS組合導(dǎo)航過程中，由于數(shù)據(jù)傳輸或者衛(wèi)星信號遮擋問題，GPS信號可能發(fā)生中斷，會導(dǎo)致INS誤差積累，或者GPS信號恢復(fù)后引起濾波發(fā)散[1]，進(jìn)而降低導(dǎo)航的精度甚至使導(dǎo)航數(shù)據(jù)完全不能使用。陳允芳[2]指出陸地車載測圖應(yīng)用中GPS信號頻繁中斷，仍需增加傳感器以輔助GPS/INS完成高精度的位置和姿態(tài)測定。RBF神經(jīng)網(wǎng)絡(luò)訓(xùn)練收斂速度和穩(wěn)定性有較大優(yōu)勢[3]，所以利用神經(jīng)網(wǎng)絡(luò)預(yù)測GPS信號缺失情況的各導(dǎo)航參數(shù)誤差方法被應(yīng)用[4]。Godha[5]通過零速更新方法通過增加虛擬觀測值的方式來限制INS誤差累積。神經(jīng)網(wǎng)絡(luò)、支持向量機(jī)等方法的引入會增加組合導(dǎo)航算法的復(fù)雜性，影響實(shí)時(shí)效果，容易引起數(shù)據(jù)解算延遲等問題，而在GPS和INS之外增加其他低精度傳感器控制短時(shí)間的INS誤差漂移，既不會大幅增加算法的復(fù)雜性，又能得到良好效果。

基于此，本文在傳統(tǒng)GPS/INS組合導(dǎo)航基礎(chǔ)上，引入了里程計(jì)傳感器。使GPS、INS和里程計(jì)融合，實(shí)現(xiàn)GPS信號中斷過程中INS誤差實(shí)時(shí)修正，保證露天礦區(qū)車輛導(dǎo)航的精度和連續(xù)性。

1 里程計(jì)速度和位置解算

設(shè)定里程計(jì)所輸出的載體運(yùn)動(dòng)速度記為v O，其方向指向載體的正前，因此里程計(jì)觀測的載體的運(yùn)動(dòng)速度的表達(dá)式在導(dǎo)航坐標(biāo)系可以寫成[6-7]:

式（1）中，為載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的方向余弦轉(zhuǎn)移矩陣。

里程計(jì)的速度可以通過直接觀測進(jìn)行坐標(biāo)系轉(zhuǎn)化獲取，位置則是通過轉(zhuǎn)化的速度積分計(jì)算獲得，位置的微分計(jì)算方程表達(dá)式寫為[8]:

式（2）中，L O、λO和h O為里程計(jì)通過速度積分計(jì)算所得到的經(jīng)度、緯度和高度信息，R M和R N為運(yùn)動(dòng)載體所在位置的地球子午圈和卯酉圈曲率半徑，、和為導(dǎo)航坐標(biāo)系下NED三個(gè)方向的速度分量。

由于里程計(jì)的位置和速度信息采集屬于離散采樣，所以根據(jù)位置的微分方程，可以推導(dǎo)出里程計(jì)的位置離散化計(jì)算方法[9-10]:

式（3）中，（k-1）表示里程計(jì)在k-1時(shí)刻的位置信息，（k）表示里程計(jì)在k時(shí)刻位置信息，Δt為里程計(jì)數(shù)據(jù)采樣時(shí)間間隔。

2 組合導(dǎo)航模型

2.1 組合導(dǎo)航動(dòng)力學(xué)模型

組合導(dǎo)航的動(dòng)力學(xué)模型的構(gòu)建基于運(yùn)動(dòng)學(xué)物理學(xué)模型及INS誤差方程[11]:

式（4）中，，和分別為INS的位置、速度和姿態(tài)三者在三個(gè)不同方向的誤差向量;為地理坐標(biāo)系相對于地球坐標(biāo)系的旋轉(zhuǎn)角速度;為地理坐標(biāo)系相對于慣性坐標(biāo)系的旋轉(zhuǎn)角速度;為地理坐標(biāo)系相對于慣性坐標(biāo)系的旋轉(zhuǎn)角速度;是加速度計(jì)所測量的比力值。加速度計(jì)觀測值誤差和陀螺儀觀測值誤差模型為:

式（5）—（6）中，和是高斯白噪聲。

綜合（4）-（6）式，動(dòng)力學(xué)模型為:

將其寫成矩陣表達(dá)式:

其中，為誤差狀態(tài)向量，Φ為狀態(tài)轉(zhuǎn)移矩陣，為狀態(tài)噪聲。

2.2 組合導(dǎo)航觀測模型

GPS和里程計(jì)均可以輸出載體的位置和速度作為Kalman濾波的觀測值，所以兩者和INS組合導(dǎo)航的觀測模型相似。取傳感器（GPS和里程計(jì)）和INS輸出的位置和速度之差構(gòu)造Kalman濾波的觀測值[12-13]:

式（9）中，Z p（t）是 Kalman濾波位置觀測輸入值;Z v（t）是Kalman濾波速度觀測輸入值，H k為觀測矩陣;ν為觀測噪聲。

Z p（t）和Z v（t）的計(jì)算式為[14]:

式（10）—（11）中:r S（t）和v S（t）分別是傳感器的位置和速度觀測值;r INS（t）和v INS（t）分別是INS計(jì)算的位置和速度值。

3 里程計(jì)輔助的GPS/INS組合導(dǎo)航

里程計(jì)和GPS都能提供組合導(dǎo)航的位置和速度觀測值，作為觀測值輸入到Kalman濾波器中。相對于里程計(jì)，GPS數(shù)據(jù)精度更高[15]，所以，在GPS信號未中斷的情況下，使用里程計(jì)數(shù)據(jù)會降低整個(gè)觀測值的精度，從而影響濾波效果。因此當(dāng)有GPS信號的時(shí)候，只采用GPS信號作為觀測值，當(dāng)GPS缺失時(shí)，采用里程計(jì)信號作為觀測值。GPS和里程計(jì)兩者提供的觀測值精度不同，所以濾波器觀測值的方差-協(xié)方差陣存在很大區(qū)別，而且，兩者的觀測信息輸入的濾波器信息采樣頻率不同，因此，兩種傳感器采用的Kalman濾波器應(yīng)該分別設(shè)置（圖1）。

圖1 里程計(jì)輔助的GPS/INS組合導(dǎo)航算法

4 實(shí)驗(yàn)分析

為驗(yàn)證本文算法的導(dǎo)航效果，進(jìn)行車載實(shí)驗(yàn)。實(shí)驗(yàn)采用2臺LAICA-GPS接收機(jī)、1臺里程計(jì)和1臺慣性測量單元，1臺GPS接收機(jī)作為靜止參考基站，另1臺GPS接收機(jī)和慣性測量單元、里程計(jì)一起安置在運(yùn)動(dòng)車輛內(nèi)（GPS天線安裝在車頂），實(shí)驗(yàn)采用MEMS級別慣性測量單元，將慣性測量單元性能參數(shù)統(tǒng)計(jì)如下（表1）。INS數(shù)據(jù)采集頻率為100 Hz，里程計(jì)數(shù)據(jù)更新頻率為10 Hz，GPS數(shù)據(jù)采樣周期為1 s。GPS-RTK解算的位置值作為參考值。

表1 慣性測量單元性能

為驗(yàn)證GPS丟失情況下GPS/INS組合導(dǎo)航的性能，選取的GPS時(shí)間序列在導(dǎo)航過程中有丟數(shù)的現(xiàn)象（主要是數(shù)據(jù)傳輸?shù)膯栴}）（圖2），并驗(yàn)證本文加入里程計(jì)后的GPS/INS組合導(dǎo)航的精度。

圖2 GPS數(shù)據(jù)丟失情況

車載觀測數(shù)據(jù)處理采用兩種方案進(jìn)行。

方案1:無里程計(jì)輔助的GPS/INS組合導(dǎo)航。

方案2:里程計(jì)輔助的GPS/INS組合導(dǎo)航。

以下分別是兩種方案在北東地三個(gè)方向的誤差（圖3-4）。由于位置誤差（與參考值比較）較小，以經(jīng)緯度為單位的方式表達(dá)位置誤差，不便于直觀理解，因此，把位置誤差以參考位置為原點(diǎn)轉(zhuǎn)換到NED坐標(biāo)系。從圖3-4可以看出，由于GPS信號的中斷，GPS/INS導(dǎo)航解部分區(qū)域誤差較大，NED三個(gè)方向的誤差最大值均大于30 m，嚴(yán)重影響了組合導(dǎo)航的效果，尤其是在GPS信號缺失的時(shí)間段，誤差最大值甚至達(dá)到了100 m。在車輛導(dǎo)航過程中，即使引入地圖修正方法，很難使100 m的誤差得到很好修正。引入里程計(jì)輔助的組合導(dǎo)航，導(dǎo)航參數(shù)誤差得到了及時(shí)修正，NED三個(gè)方向的誤差最大值均小于20 m，能夠保證導(dǎo)航的效果。

為更加清晰直觀地比較兩種方案的誤差，對NED三個(gè)方向的誤差最大值和均方差進(jìn)行統(tǒng)計(jì)（表2）。可以看出，方案1在NED三個(gè)方向的誤差最大值分別達(dá)到了40.324 m、38.776 m和79.693 m，不能滿足導(dǎo)航的要求，說明在GPS信號缺失的情況下，INS導(dǎo)航效果較差，可靠性低，加入了里程計(jì)輔助的GPS/INS組合導(dǎo)航誤差在NED三個(gè)方向的最大值分別為11.336 m、8.056 m和19.782 m，除D方向外，其他兩個(gè)方向的誤差基本在10 m以內(nèi)，可以為導(dǎo)航提供良好保證。比較方案1和方案2的均方差，可以得到類似結(jié)論:加入里程計(jì)輔助，可以很好修正GPS信號缺失階段的導(dǎo)航參數(shù)誤差。

圖3 無里程計(jì)輔助的GPS/INS組合導(dǎo)航誤差

圖4 里程計(jì)輔助的GPS/INS組合導(dǎo)航

表2 位置誤差

5 結(jié) 語

針對露天礦區(qū)車載GPS信號中斷過程中INS誤差快速累積導(dǎo)致導(dǎo)航精度降低的問題，提出了利用里程計(jì)輔助的GPS/INS組合導(dǎo)航的算法，利用里程計(jì)計(jì)算的位置和速度修正GPS信號中斷過程導(dǎo)航參數(shù)誤差。車載實(shí)驗(yàn)證明，里程計(jì)可以較好完成INS誤差的修正，避免誤差的快速積累，確保高精度導(dǎo)航結(jié)果的輸出。本文算法是在GPS信號中斷過程中利用里程計(jì)輸出的坐標(biāo)和位置對導(dǎo)航參數(shù)進(jìn)行修正的，GPS信號正常情況下的里程計(jì)觀測值沒有得到有效利用，浪費(fèi)了數(shù)據(jù)信息，如何將GPS信息和里程計(jì)信息進(jìn)行高效融合是下一步的研究重點(diǎn)。