1、卡爾曼濾波在GPS/SINS組合導(dǎo)航系統(tǒng)中的應(yīng)用一、引言慣性導(dǎo)航系統(tǒng)不僅能夠連續(xù)提供載體運(yùn)動(dòng)參數(shù)，還能方便地提供載體的三維姿態(tài)參數(shù)， 但是，慣導(dǎo)系統(tǒng)有它難以克服的缺點(diǎn)，其導(dǎo)航定位誤差隨時(shí)間增長(zhǎng)，因而難以長(zhǎng)時(shí)間地獨(dú) 立工作。20世紀(jì)70年代發(fā)展起來(lái)了GPS全球定位系統(tǒng)，它的特點(diǎn)是定位和測(cè)速精度高， 且基本上不受時(shí)間、地區(qū)的限制，這使得GPS在載體導(dǎo)航系統(tǒng)中屬佼佼者。但由于載體飛 行動(dòng)作方面的原因，以及載體不能保證空間衛(wèi)星軌道布局100%無(wú)故障，使GPS定位的準(zhǔn)確 性會(huì)受到影響；且GPS接收機(jī)輸出信號(hào)的更新頻率一般等于12 Hz，有時(shí)不能滿足載體 飛機(jī)控制對(duì)導(dǎo)航信號(hào)更新頻率的要求，致使航行載體上單

2、獨(dú)使用GPS接收機(jī)的方案受到限 制。但GPS導(dǎo)航誤差不隨時(shí)間發(fā)散，在足夠長(zhǎng)的時(shí)間內(nèi)均值為零。結(jié)合GPS與慣導(dǎo)這兩種 系統(tǒng)的優(yōu)點(diǎn)，可使導(dǎo)航系統(tǒng)的精度提高，可靠性增加。組合導(dǎo)航系統(tǒng)一般采用卡爾曼濾波 技術(shù)，即在兩個(gè)（或兩個(gè)以上）導(dǎo)航系統(tǒng)輸出的基礎(chǔ)上，利用卡爾曼濾波去估計(jì)系統(tǒng)的各種 誤差狀態(tài)，再用誤差狀態(tài)估值去校正系統(tǒng)，達(dá)到綜合的目的。校正一般有兩種方式：輸出 校正（開(kāi)環(huán)卡爾曼濾波器）和反饋校正（閉環(huán)卡爾曼濾波器）。本文通過(guò)對(duì)GPS/SINS組合導(dǎo)航 系統(tǒng)的半物理仿真，研究了這兩種校正方式的導(dǎo)航精度，分析了導(dǎo)航誤差產(chǎn)生的根源。二、組合導(dǎo)航卡爾曼濾波方法組合導(dǎo)航系統(tǒng)采用卡爾曼濾波器進(jìn)行參數(shù)估計(jì)的主要對(duì)

3、象是導(dǎo)航參數(shù)，如經(jīng)度入、緯 度L、對(duì)地速度（VE ,VN，V）。根據(jù)濾波器狀態(tài)選取的不同，估計(jì)方法分直接法和間接法 兩種。直接法直接以各種導(dǎo)航參數(shù)X為主要狀態(tài)，濾波器估值的主要部分就是導(dǎo)航參數(shù)估 值X；間接法以組合導(dǎo)航系統(tǒng)中某一種導(dǎo)航系統(tǒng)（經(jīng)常采用慣導(dǎo)系統(tǒng)）輸出的導(dǎo)航參數(shù)的 誤差X為濾波器主要狀態(tài)，濾波器估值的主要部分是導(dǎo)航參數(shù)誤差估值X，用X去校正X。 利用直接法進(jìn)行估計(jì)時(shí)，慣導(dǎo)的系統(tǒng)方程和量測(cè)方程是非線性的。利用間接法進(jìn)行估計(jì)時(shí)， 系統(tǒng)方程中的主要部分是導(dǎo)航參數(shù)誤差方程，由于這種方法的誤差很小，通?？珊雎远A 小量，所以間接法的系統(tǒng)方程和量測(cè)方程一般都是線性的，故而組合導(dǎo)航系統(tǒng)的綜合濾波

4、器常采用間接法。利用狀態(tài)估值X對(duì)系統(tǒng)進(jìn)行校正也有兩種方法，輸出校正和反饋校正的 原理框圖如圖1所示。輸出校正所謂輸出校正，就是用導(dǎo)航參數(shù)誤差的估值X去校正系統(tǒng)輸出的導(dǎo)航參數(shù)，得到組合 導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)估值X （即經(jīng)過(guò)校正后的系統(tǒng)導(dǎo)航參數(shù)值，見(jiàn)圖1），即：X=Xj - AXZ輸 出校正的離散卡爾曼濾波方程即是離散卡爾曼濾波基本方程，實(shí)際是開(kāi)環(huán)卡爾曼濾波器。 濾波方程如下：ok,k-1 = ek,k-1為tKi時(shí)刻的一步轉(zhuǎn)移矩陣，Ak,k-1為系統(tǒng)噪聲驅(qū)動(dòng)陣；氣 為量測(cè)矩陣；vk為量測(cè)噪聲序列；町為系統(tǒng)激勵(lì)噪聲矩陣。假設(shè)狀態(tài)模型噪聲的協(xié)方差 陣為E（Wk - W）=Qk ,量測(cè)模型噪聲的協(xié)方差陣

5、為E（VK - VKT）= Rk , KK為濾波增益;PK/K1為一步預(yù)測(cè)均方差；PK為估計(jì)均方差。Kk =外心研（地4心1成+母尸=。梭-,上-1、1）反饋校正采用反饋校正是將慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)誤差X的估值*反饋到慣導(dǎo)系統(tǒng)內(nèi)，對(duì)誤差狀 態(tài)進(jìn)行校正。根據(jù)待校正的參數(shù)的性質(zhì)，又可分為脈沖校正和連續(xù)校正，其卡爾曼濾波方 程不同于離散卡爾曼濾波基本方程，脈沖校正方程見(jiàn)式（2），連續(xù)校正的濾波方程即為帶 控制量的卡爾曼濾波方程。Kk = PkHkPkHl + J?,-1i式子中參數(shù)含義與上面相同。三、GPS/SINS組合導(dǎo)航系統(tǒng)模型GPS/SINS組合導(dǎo)航系統(tǒng)的方框圖如圖2所示，IMU測(cè)得機(jī)體相對(duì)于慣性

6、空間的角速度和加速度，再通過(guò)捷聯(lián)式慣導(dǎo)（SINS）力學(xué)編排算法實(shí)時(shí)計(jì)算導(dǎo)航信息，然后利用SINS算法得到的位置和機(jī)載GPS接收機(jī)測(cè)得的可見(jiàn)星星歷選擇最佳導(dǎo)航星座，計(jì)算出慣導(dǎo)輸出的位置到被選中衛(wèi)星的偽距P，再?gòu)闹袦p去GPS接收機(jī)輸出的偽距pG，得到偽距之差作為 組合系統(tǒng)卡爾曼濾波器的觀測(cè)量ap。然后利用卡爾曼濾波器，估計(jì)出慣導(dǎo)誤差狀態(tài)。系統(tǒng)的結(jié)構(gòu)框圖如圖2所示。校止導(dǎo)航溟差失準(zhǔn)角，位置，速度）（反慣校正）圖2 GPS/SINS組合系統(tǒng)方框圖IN1U捷聯(lián)慣孚力學(xué)編排狀！ 態(tài)： 估：卡爾曼計(jì)！值！濾波器慣性導(dǎo)航系統(tǒng)的誤差模型導(dǎo)航坐標(biāo)系為東北天坐標(biāo)系。狀態(tài)變量包括三個(gè)失準(zhǔn)角中E，N，U、三個(gè)速度誤差a

7、VE ,aVN a%、三個(gè)位置誤差（經(jīng) 度，緯度，高度）aL,aX,ah、三個(gè)陀螺常值漂移誤差電，標(biāo),bz、三個(gè)加速度計(jì)常值誤差 Vx ,Vy ,Vz，共15維狀態(tài)變量?？柭鼮V波系統(tǒng)方程如下： TOC o 1-5 h z 出走+任職山）其中Y -格隊(duì)如吟M吟就1泯前命&筆VT弓.K LGPS誤差模型在GPS誤差模型中，主要考慮時(shí)鐘誤差bt和時(shí)鐘頻率誤差bf。其狀態(tài)方程為：XsslX&psQXe或）+ 吒禎），.*=； 一，H-aps =：“WGPS為偽距測(cè)量噪聲和載波相位測(cè)量噪聲。系統(tǒng)狀態(tài)方程及量測(cè)方程將慣導(dǎo)誤差模型式（3）和GPS誤差模型式（4）合并，得到系統(tǒng)狀態(tài)方程：X（t） = F（t

8、）X（t） + G（t）W（t）（ 5）偽距量測(cè)方程中與慣導(dǎo)給出的位置對(duì)應(yīng)的偽距為：Pli =（工I -焰）+ S -+（ZI _）212I口O。網(wǎng)氣#財(cái)伉）。斗血）口 12（益）。13（益）1由Pj = Plr -PGps -得量測(cè)方程，&卜1 =5）受）010砰心）哇伉）1。42（上）口書 ftt）1十 =其中，匕為偽距測(cè)量噪聲上式中：真實(shí)位置x，y，z，Rn = Re(1 + fsin2L)R： = 678137m,f=1/298.257% = 2 (x xs.)2 + (y- ys.)2 + (z- zs.)2羽 x -SPa y y SPn z -混dt r;uv nti rian

9、- R=+ / ) | - tusiiiLcosA- ens itiAsiiiA + Rm(1 - f2) + esLtit2 Rn + 11) cj2C(sLcc)sA eaco&sinAj + 臼渤inLUi3 = eticx)sAiC()sA -I- encosLsinX f 松3sinL= 1.2, 3, |4)四、結(jié)果分析采用0.02 /h的中精度陀螺，初始誤差角取東向20、北向20、天向6，加速度 計(jì)隨機(jī)常值偏置為100X1O-6g，飛機(jī)的初始地理位置為北緯40、東經(jīng)116.3、高度68m， 偽距測(cè)量噪聲為標(biāo)準(zhǔn)差為8 m，載波相位測(cè)量噪聲0.049m。系統(tǒng)每1 s進(jìn)行一次組合，仿真

10、時(shí)間為3 000 s。1.采用輸出校正卡爾曼濾波器輸出校正卡爾曼濾波方程見(jiàn)式(1),仿真結(jié)果如圖3。比較圖3、圖4,顯然組合導(dǎo)航系 統(tǒng)輸出的導(dǎo)航數(shù)據(jù)隨時(shí)間的增加產(chǎn)生了偏差，這是由于組合導(dǎo)航系統(tǒng)以導(dǎo)航誤差為狀態(tài)變 量，一般認(rèn)為這些狀態(tài)變量為一階小量，所以在線性化過(guò)程中，略去了高階項(xiàng)。但當(dāng)仿真 時(shí)間較長(zhǎng)時(shí)，狀態(tài)變量不再為一階小量，線性化處理將導(dǎo)致?tīng)顟B(tài)方程的不準(zhǔn)確，從而使濾 波結(jié)果失真。2.采用反饋校正卡爾曼濾波器僅將導(dǎo)航位置、速度估計(jì)誤差反饋回原系統(tǒng)，屬于脈沖校正，且假設(shè)反饋?zhàn)兞客耆?控。反饋校正卡爾曼濾波方程見(jiàn)式(2)，仿真結(jié)果見(jiàn)圖4。反饋校正方案輸出的導(dǎo)航參數(shù)隨 時(shí)間增加同樣出現(xiàn)了偏差，這是

11、由于閉環(huán)卡爾曼濾波方程始終在每一次濾波計(jì)算時(shí)都將估 計(jì)狀態(tài)預(yù)測(cè)值置零。實(shí)際上，卡爾曼濾波器在濾波開(kāi)始到穩(wěn)定需要一段時(shí)間。濾波剛開(kāi)始 的時(shí)刻，估計(jì)誤差太大，不宜進(jìn)行反饋。若在整個(gè)系統(tǒng)運(yùn)行過(guò)程中，濾波與反饋兩個(gè)過(guò)程 完全同步，即將每一步得到的估計(jì)值反饋回慣導(dǎo)系統(tǒng)，再將下一步的預(yù)測(cè)值置零，將導(dǎo)致 整個(gè)系統(tǒng)的精度降低。由以上兩種校正可以看出:第一，不論采取何種方式的卡爾曼濾波方法，組合系統(tǒng)的協(xié)方差特性曲線不變，即系統(tǒng)所 能達(dá)到的精度不變，這是因?yàn)橄到y(tǒng)精度是由系統(tǒng)的狀態(tài)方程及系統(tǒng)的噪聲特性所決定的， 不受校正方式的影響。以上結(jié)論的前提是模型系統(tǒng)方程與觀測(cè)方程能正確反映系統(tǒng)本身； 當(dāng)這一前提不滿足時(shí)，采取不同校正方案得到的組合系統(tǒng)導(dǎo)航精度不同。第二，輸出校正利用濾波器的估計(jì)來(lái)校正慣導(dǎo)系統(tǒng)輸出的導(dǎo)