卡爾曼濾波算法在里程計(jì)/GPS組合導(dǎo)航中的研究

王丹丹，盧春華，李 立，趙建周

（安陽工學(xué)院電子信息與電氣工程學(xué)院，河南安陽45500）

0 引言

由陀螺系統(tǒng)表示的慣性導(dǎo)航系統(tǒng)首次應(yīng)用于德國科學(xué)家布勞恩主持下的火箭制導(dǎo)，這為現(xiàn)代慣性的發(fā)展開辟了大門[1-2]。在民用這一方面上，在精度政策的選擇上，民用用戶在取消政策后只能享受2000年標(biāo)準(zhǔn)精密服務(wù)中100米的誤差，普通民用用戶可享受到儀表內(nèi)精度以外的精度。在軍事方面，設(shè)備的科技含量和獨(dú)立性是衡量國家實(shí)力的重要指標(biāo)，導(dǎo)航設(shè)備要求更快、更準(zhǔn)確、更強(qiáng)、更快，這意味著解決問題的時(shí)間較短、更準(zhǔn)確。

人們在衛(wèi)星導(dǎo)航系統(tǒng)中已經(jīng)發(fā)明許多種類的導(dǎo)航系統(tǒng)并且得以應(yīng)用，但每一種導(dǎo)航都有其自身的優(yōu)缺點(diǎn)。諸如授權(quán)用戶之類的GPS導(dǎo)航系統(tǒng)[3-5]具有高導(dǎo)航精度并且導(dǎo)航精度穩(wěn)定，但是在非美國盟友中使用技術(shù)將成為一大障礙，同時(shí)，這種導(dǎo)航數(shù)據(jù)的更新頻率一般不高，當(dāng)載波運(yùn)行到隧道等信號(hào)丟失的位置或存在遮擋時(shí)，載波移動(dòng)高度也可能出現(xiàn)丟失的現(xiàn)象。組合導(dǎo)航的優(yōu)點(diǎn)是可以將兩個(gè)導(dǎo)航系統(tǒng)結(jié)合到一個(gè)導(dǎo)航系統(tǒng)中。在組合導(dǎo)航中一般使用卡爾曼濾波[6-8]來處理兩個(gè)系統(tǒng)的數(shù)據(jù)。自20世紀(jì)80年代以來，綜合導(dǎo)航已引起人們的廣泛關(guān)注。兩者的結(jié)合可以克服信號(hào)在小范圍內(nèi)無法精確測量，信號(hào)容易丟失，導(dǎo)航誤差在很長一段時(shí)間內(nèi)累積的缺點(diǎn)。目前，綜合導(dǎo)航系統(tǒng)已廣泛應(yīng)用于許多領(lǐng)域，尤其是軍用導(dǎo)彈、飛機(jī)、潛艇等的導(dǎo)航應(yīng)用，具有重要的現(xiàn)實(shí)意義和工程應(yīng)用價(jià)值。

1 GPS的誤差模型

基于GPS的動(dòng)力學(xué)模型及狀態(tài)向量為

在這里面，φ,λ,h表示經(jīng)、緯度以及高度，Vn,Ve,Vu表示北、東以及天的速度，A加速度擾動(dòng)系數(shù)，RM曲率半徑，RN卯酉圈曲率半徑，W為白噪聲驅(qū)動(dòng)。

2 里程計(jì)的誤差模型

在平坦路面下移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型

基于里程計(jì)系統(tǒng)的航位推算公式為

里程計(jì)的誤差包括系統(tǒng)誤差和非系統(tǒng)誤差。而里程計(jì)的系統(tǒng)誤差是由移動(dòng)機(jī)器人驅(qū)動(dòng)輪的安裝位置和左右輪的不同半徑，以及兩個(gè)車輪之間距離的不精確測量所導(dǎo)致的。里程計(jì)引起的誤差是隨機(jī)產(chǎn)生的，主要原因是車輪打滑。由上面的信息我們可以知道以下結(jié)論：

3 里程計(jì)／GPS的卡爾曼濾波器設(shè)計(jì)

GPS/里程計(jì)的最本質(zhì)的思想就是想辦法去利用最優(yōu)估計(jì)算法去統(tǒng)一GPS和里程計(jì)的信息，然后達(dá)到集成緊湊方案的基本思想，最優(yōu)估計(jì)算法所采用的是GPS偽距離、偽距離率信息和里程計(jì)速度信息進(jìn)行融合的一種方案。

卡爾曼濾波器在本質(zhì)上就是我們所說的一種線性最小方差估計(jì)的最優(yōu)估計(jì)算法。本文對移動(dòng)機(jī)器人的位置誤差δX(k)、δY(k)和航向角誤差δθ(k)進(jìn)行濾波，以實(shí)現(xiàn)位置誤差δX?、δY?以及方位角誤差δθ?的最優(yōu)估計(jì)。導(dǎo)航系統(tǒng)濾波方案如圖1所示。

通過對系統(tǒng)的分析和建模，給出了數(shù)據(jù)融合算法，結(jié)果表明，該算法具有良好的穩(wěn)定性和高精度的導(dǎo)航。

圖1 導(dǎo)航系統(tǒng)濾波方案

組合導(dǎo)航系統(tǒng)的狀態(tài)變量、狀態(tài)方程以及狀態(tài)矩陣描述為

觀測矩陣

狀態(tài)變量預(yù)測

協(xié)方差陣預(yù)測

卡爾曼濾波增益

狀態(tài)更新

協(xié)方差矩陣更新

通過以上的式子就可以求出機(jī)器人的δX、δY、δθ這幾個(gè)量最優(yōu)估計(jì)，進(jìn)而就可以求出機(jī)器人小車的位置以及航向角X、Y、θ，然后實(shí)現(xiàn)載體的精確導(dǎo)航。

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

由于本論文主要目的是設(shè)計(jì)組合導(dǎo)航系統(tǒng)，在本系統(tǒng)的設(shè)計(jì)中運(yùn)用了卡爾曼濾波器，但是為了檢驗(yàn)本系統(tǒng)的性能能否達(dá)到預(yù)期要求以及卡爾曼濾波器的使用能否達(dá)到本系統(tǒng)的要求，所以我們需要對其驗(yàn)證，采用Matlab對系統(tǒng)進(jìn)行仿真。紅線為真實(shí)誤差值δX、δY、δθ，綠線為最優(yōu)估計(jì)值。從仿真結(jié)果可以看出，能夠準(zhǔn)確地估計(jì)出真實(shí)誤差δX、δY、δθ，該系統(tǒng)設(shè)計(jì)的卡爾曼濾波是可行的。當(dāng)狀態(tài)向量初值為：X0=[0 10 0 0 0 0]；估計(jì)誤差協(xié)方差初值：P0=diag([1 00,1,0.04,100,1,0.04] )時(shí)，東北向位置誤差和位置誤差信息如圖2和3所示。

圖2 東北向位置誤差

圖3 位置軌跡

根據(jù)以上仿真結(jié)果可以得到如下結(jié)論，在坐標(biāo)軸的東方向和北方向上沒有進(jìn)行濾波前的誤差最大值分別達(dá)到了33m和41m，當(dāng)進(jìn)行了卡爾曼濾波處理過后，誤差得到了很好的控制，對應(yīng)的誤差最大值降到了14m和13m，完全達(dá)到了預(yù)期的要求。由圖3得到的仿真軌跡與真實(shí)軌跡擬合度非常高。

5 結(jié)束語

GPS能夠在全世界范圍內(nèi)進(jìn)行實(shí)時(shí)性的不間斷地精準(zhǔn)導(dǎo)航，而且擁有由設(shè)備造成的原始誤差不會(huì)隨著時(shí)間的延長而增大和制造價(jià)格低的優(yōu)勢，因此GPS在生活中被大量使用。然而，GPS導(dǎo)航也存在著一個(gè)最大的問題，就是對外界環(huán)境的抗干擾能力非常差。里程計(jì)擁有很強(qiáng)的抗干擾能力，但是它也存在著一個(gè)很大的缺點(diǎn)，就是隨著時(shí)間的延長，系統(tǒng)的原始誤差會(huì)變得越來越大。因此里程計(jì)并不適合長時(shí)間的導(dǎo)航任務(wù)中，但是它的工作特點(diǎn)使它能夠在生活中大量應(yīng)用。經(jīng)過對這兩種導(dǎo)航系統(tǒng)的分析可知，若二者融合在一起，將會(huì)極大提升整體系統(tǒng)的導(dǎo)航精準(zhǔn)度。通過合適的方法，也能使這兩種導(dǎo)航系統(tǒng)更好發(fā)揮各自的優(yōu)勢，避免各自的缺點(diǎn)。本文在卡爾曼濾波算法的基礎(chǔ)上，將GPS和里程計(jì)兩個(gè)不同系統(tǒng)的信息進(jìn)行融合，改變了單一系統(tǒng)不可靠的缺陷，提升了整體系統(tǒng)的穩(wěn)定性，達(dá)到了一定的導(dǎo)航精準(zhǔn)度要求。