一種組合導(dǎo)航的直接法濾波算法

聞 新，紀(jì) 龍，趙建新

（南京航空航天大學(xué) 航天學(xué)院，南京 210016）

1 引言

組合導(dǎo)航系統(tǒng)以慣性導(dǎo)航系統(tǒng) （inertial navigation system，INS）輔助全球定位系統(tǒng) （global positioning system，GPS）接收機(jī)增強(qiáng)系統(tǒng)抗干擾能力，保證組合后的導(dǎo)航精度高于各系統(tǒng)單獨(dú)工作的精度［1-2］。組合系統(tǒng)直接法濾波的系統(tǒng)狀態(tài)方程為非線性方程［2］，這給濾波器設(shè)計(jì)帶來(lái)一定困難。針對(duì)該問(wèn)題，部分學(xué)者在工作點(diǎn)處作線性化后采用擴(kuò)展卡爾曼濾波 （extended Kalman filter，EKF）進(jìn)行狀態(tài)估計(jì)，但當(dāng)INS精度較低或載體處于高動(dòng)態(tài)環(huán)境時(shí)，EKF算法參數(shù)估計(jì)精度不高甚至?xí)l(fā)生濾波發(fā)散；另有學(xué)者在假設(shè)系統(tǒng)噪聲為白噪聲前提下，采用無(wú)跡卡爾曼濾波 （unscented Kalman particle filter，UPF）直接對(duì)非線性系統(tǒng)方程進(jìn)行濾波估計(jì)，但這一方法僅在系統(tǒng)噪聲為白噪聲時(shí)成立。

粒子濾波 （particle filter，PF）在處理非線性、非高斯系統(tǒng)的參數(shù)估計(jì)問(wèn)題上具有獨(dú)特優(yōu)勢(shì)，對(duì)組合導(dǎo)航這類復(fù)雜問(wèn)題的求解可獲得較高精度［3-5］。本文提出了基于 UPF的INS/GPS組合導(dǎo)航系統(tǒng)直接法濾波狀態(tài)估計(jì)，以INS的導(dǎo)航參數(shù)及平臺(tái)誤差角作為系統(tǒng)狀態(tài)，慣導(dǎo)力學(xué)編排方程和姿態(tài)誤差方程作為狀態(tài)方程，GPS的輸出作為觀測(cè)值，采用UPF直接進(jìn)行組合導(dǎo)航系統(tǒng)參數(shù)解算。基于這一方案的系統(tǒng)狀態(tài)方程包含9個(gè)參數(shù)，減少了陀螺儀及加速度計(jì)漂移等狀態(tài)變量，降低了狀態(tài)變量維數(shù)易于粒子濾波實(shí)現(xiàn)。

2 INS/GPS組合導(dǎo)航系統(tǒng)濾波結(jié)構(gòu)

本文INS/GPS組合導(dǎo)航系統(tǒng)基于閉環(huán)組合方式采用直接法濾波，INS和GPS分別對(duì)載體運(yùn)動(dòng)參數(shù)進(jìn)行測(cè)量，將INS和GPS測(cè)得的參數(shù)分別送入U(xiǎn)KF濾波器進(jìn)行集中解算，從而獲得系統(tǒng)狀態(tài)的最優(yōu)估計(jì)并作為組合導(dǎo)航系統(tǒng)的輸出，濾波輸出對(duì)INS系統(tǒng)進(jìn)行狀態(tài)反饋修正。這種組合系統(tǒng)還能夠?qū)崿F(xiàn)GPS信號(hào)無(wú)效時(shí)的容錯(cuò)性，其結(jié)構(gòu)如圖1所示。

圖1 INS/GPS組合導(dǎo)航系統(tǒng)的直接法UPF濾波結(jié)構(gòu)圖

3 基于UPF的INS/GPS組合導(dǎo)航系統(tǒng)直接法濾波算法

導(dǎo)航坐標(biāo)系選為東北天地理坐標(biāo)系，則慣導(dǎo)系統(tǒng)的力學(xué)編排方程和姿態(tài)誤差方程如式 （1）和式 （2）所示。

式中，vE、vN、vU為東、北、天向速度，L、λ、h為經(jīng)度、緯度和高度，φE、φN和φU為東、北、天向姿態(tài)誤差角，系統(tǒng)以這9個(gè)參數(shù)作為狀態(tài)值。其中，fE、fU、fN為加速度計(jì)輸出的東北天向比力觀測(cè)值，δvE、δvN、δh和δL在實(shí)際計(jì)算中可以由INS和GPS的輸出差值近似獲得，εbE、εbN和εbU為陀螺的東、北、天向常值漂移。

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

為簡(jiǎn)化系統(tǒng)模型并提高濾波計(jì)算效率，本文僅對(duì)東、北、天向位置、速度和姿態(tài)誤差建模，不對(duì)加速度計(jì)和陀螺進(jìn)行誤差建模，則INS/GPS組合導(dǎo)航系統(tǒng)狀態(tài)向量為

根據(jù)選取的系統(tǒng)狀態(tài)向量x，結(jié)合式 （1）、式（2）和式 （3），可列寫出INS/GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程為

式 中，f［·］為 非 線 性 連 續(xù) 函 數(shù)，W（t）＝［wgE，wgN，wgU］T為系統(tǒng)噪聲。

3.2 觀測(cè)方程

以GPS輸出的速度信息vEGPS、vNGPS、vUGPS和位置信息LGPS、λGPS、hGPS為觀測(cè)值Z，即

據(jù)前面所選取的系統(tǒng)狀態(tài)x和觀測(cè)值Z，可獲得組合導(dǎo)航系統(tǒng)的觀測(cè)方程

式中，Zk和xk為Z和x在k時(shí)刻的值，H（·）為線性函數(shù)，Vk＝ ［VVE，VVN，VVU，VL，Vλ，Vh］T為觀測(cè)噪聲，強(qiáng)度為Rk。

3.3 UPF濾波算法解算過(guò)程

基于個(gè)人計(jì)算機(jī)的解算過(guò)程要采用離散的系統(tǒng)方程，而INS/GPS組合導(dǎo)航系統(tǒng)狀態(tài)方程為微分方程。因此，需要對(duì)式 （4）進(jìn)行離散化。離散化后的系統(tǒng)狀態(tài)方程

式中，F(xiàn)（·）為非線性離散函數(shù)，Wk為系統(tǒng)噪聲，噪聲方差強(qiáng)度為Qk。其中

為減小系統(tǒng)離散化誤差，仿真過(guò)程中取前三項(xiàng)以減小離散化誤差。并且有

式中，Δt為濾波更新周期，＝Q。狀態(tài)方程和觀測(cè)噪聲如下

利用上面推導(dǎo)出的INS/GPS組合導(dǎo)航系統(tǒng)離散方程，結(jié)合UKF濾波法的遞推更新過(guò)程，得出基于UPF算法的組合導(dǎo)航系統(tǒng)濾波過(guò)程如下

1）初始化：k＝0

其中，＝x（t）t＝k。

2）重要性密度函數(shù)，k＝1，2，…，T，使用UKF更新粒子

其中na＝nx＋nw＋nv為擴(kuò)展后系統(tǒng)狀態(tài)的維數(shù)，且，通過(guò)該式對(duì)狀態(tài)向量進(jìn)行擴(kuò)展。

（2）對(duì)每個(gè)采樣點(diǎn)，i＝1，2，…，N，應(yīng)用UKF算法得到，，從而得到簡(jiǎn)易分布函數(shù)

（3）從建議分布中抽取粒子：

（4）計(jì)算粒子權(quán)值并歸一化

3）重采樣

對(duì) ｛，｝ ，i＝1，2，…，N重采樣，得到新的粒子集合 ｛，｝ ，權(quán)值＝1/N，i＝1，2，…，N。

4）結(jié)果輸出

根據(jù)組合導(dǎo)航系統(tǒng)的離散狀態(tài)方程式 （7）和觀測(cè)方程式 （6），只須設(shè)定濾波初值，就可獲得第k時(shí)刻的系統(tǒng)狀態(tài)最優(yōu)估計(jì)值xk，k＝1，2，3…。

4 仿真分析

本文主要研究INS/GPS組合導(dǎo)航數(shù)據(jù)融合算法的精度，仿真期間保證系統(tǒng)在組合模式下工作。INS/GPS組合導(dǎo)航系統(tǒng)的初始參數(shù)和仿真參數(shù)如表1和表2所示。仿真選取典型的飛機(jī)機(jī)動(dòng)軌跡，該軌跡包含滑跑、加速拉起、爬高、改平、平飛、預(yù)轉(zhuǎn)彎、轉(zhuǎn)彎、減速飛行、低頭、俯沖等機(jī)動(dòng)動(dòng)作，仿真時(shí)長(zhǎng)為3 600s，其軌跡如圖2所示。

表1 INS/GPS組合導(dǎo)航系統(tǒng)初始參數(shù)設(shè)置

表2 INS/GPS組合導(dǎo)航系統(tǒng)仿真參數(shù)設(shè)置

圖2 飛行軌跡仿真

在上述條件下，分別對(duì)INS/GPS組合導(dǎo)航系統(tǒng)的直接法UPF和UKF進(jìn)行了仿真研究，并對(duì)仿真結(jié)果進(jìn)行比較分析。為保證組合導(dǎo)航系統(tǒng)直接法濾波的精度并控制濾波計(jì)算量，濾波粒子數(shù)N取為1 000，并取α＝1，β＝0，k＝2。由于篇幅有限，且東向和北向?qū)Ш浇Y(jié)果具有相似性，本文僅對(duì)東向 （經(jīng)度）、天向 （高度）的位置和速度誤差進(jìn)行數(shù)值計(jì)算分析，其仿真結(jié)果如圖3、圖4所示。

從圖3和圖4可以看出，應(yīng)用UKF組合導(dǎo)航系統(tǒng)經(jīng)度誤差可控制在±13m以內(nèi)，高度誤差在±15m以內(nèi)，速度誤差在±0.6m/s以內(nèi)。而應(yīng)用UPF的組合導(dǎo)航系統(tǒng)經(jīng)緯度誤差控制在±6m以內(nèi)，高度誤差在±10m以內(nèi)，速度誤差在±0.5 m/s以內(nèi)。通過(guò)上述兩種濾波計(jì)算結(jié)果的比較，說(shuō)明了基于UPF濾波的直接法INS/GPS組合導(dǎo)航系統(tǒng)可以獲得更高的導(dǎo)航精度，并能更準(zhǔn)確地反映真實(shí)狀態(tài)的演變情況。

圖3 基于UPF和UKF算法的經(jīng)度和高度位置誤差

圖4 基于UPF和UKF算法的東向和天向速度誤差

5 結(jié)論

直接法UPF濾波在組合導(dǎo)航系統(tǒng)狀態(tài)結(jié)算過(guò)程中不含截?cái)嗾`差，濾波過(guò)程避免了慣導(dǎo)力學(xué)編排方程的許多重復(fù)計(jì)算，降低了算法冗余度。同時(shí)，閉環(huán)結(jié)構(gòu)的濾波方法在GPS信號(hào)突然失效時(shí)系統(tǒng)可實(shí)現(xiàn)有效切換。仿真結(jié)果表明UPF算法與UKF算法相比，具有更高的估計(jì)效率和估計(jì)精度，解算過(guò)程具有較強(qiáng)的魯棒性。

［1］全偉，劉百奇，宮曉琳.慣性/天文/衛(wèi)星組合導(dǎo)航技術(shù)［M］.北京：國(guó)防工業(yè)出版社，2011.

［2］劉建業(yè)，曾慶化，趙偉.導(dǎo)航系統(tǒng)理論與應(yīng)用［M］.西安：西北工業(yè)大學(xué)出版社，2010.

［3］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，1998.

［4］程水英，張劍云.粒子濾波評(píng)述［J］.宇航學(xué)報(bào)，2008，29（4）：1099－1112.

［5］趙琳，王小旭，丁繼成.組合導(dǎo)航系統(tǒng)非線性濾波算法綜述［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17（1）：46－54.

［6］楊波，秦永元，柴艷.UKF在INS/GPS直接法卡爾曼濾波中的應(yīng)用［J］.傳感技術(shù)學(xué)報(bào)，2007，20（4）：842－846.

［7］汪秋婷，胡修林.基于UKF的新型北斗/SINS組合系統(tǒng)直接法卡爾曼濾波［J］.系統(tǒng)工程與電子技術(shù)，2010，32（2）：376－379.

［8］BENGTSSON T，SNYDER C，NYCHKA D.Toward a Nonlinear Ensemble Filter for High Dimensional Systems［J］.Journal of Geophysical Research，2003，108（D24）：8775－8785.

［9］VAN DER MERWE R，DOUCET A，DE FREITAS N，etal.The Unscented Particle Filter［EB/OL］.［2013－02－16］.http：//automatica.dei.unipd.it/tl＿files/utenti/lucaschenato/Classes/PSC10 ＿11/UPF ＿slides.pdf.

［10］VAN DER MERWE R，DOUCET A，DE FREITAS N，eta1.The Unscented Particle Filter Technical Report［R］.Cambridge：Department of Engineering，University of Cambridge，2000.