孟 朱立新

電子工程學(xué)院電子制約技術(shù)安徽省重點(diǎn)實(shí)驗(yàn)室，合肥230037

?

基于GPS/INS緊組合導(dǎo)航的UKF和CKF濾波精度分析

電子工程學(xué)院電子制約技術(shù)安徽省重點(diǎn)實(shí)驗(yàn)室，合肥230037

GPS/INS組合導(dǎo)航系統(tǒng)采用以誤差量作為狀態(tài)變量的非線性緊組合模型，通過函數(shù)泰勒展開式，理論分析了無跡卡爾曼濾波(UKF)和容積卡爾曼濾波(CKF)2種算法的濾波精度，引入了數(shù)值穩(wěn)定因子，并證明當(dāng)狀態(tài)維數(shù)高于三維時(shí)，UKF積分精度變差，而CKF的濾波精度高于UKF。將2種算法應(yīng)用于組合導(dǎo)航中，通過仿真實(shí)驗(yàn)，驗(yàn)證了理論分析的正確性，所得結(jié)果具有工程應(yīng)用參考價(jià)值。

GPS/INS組合導(dǎo)航; UKF; CKF; 泰勒展開式; 穩(wěn)定因子; 導(dǎo)航精度

GPS/INS(Global Positioning System/ Inertial Navigation System)組合導(dǎo)航系統(tǒng)克服了各自缺點(diǎn)，使組合后的導(dǎo)航精度高于兩個(gè)系統(tǒng)單獨(dú)工作的精度[1]，其常見的組合模式有松組合(采用位置、速度組合)和緊組合(采用偽距、偽距率組合)[2]。研究表明，緊組合模式可以提高組合導(dǎo)航應(yīng)對(duì)復(fù)雜環(huán)境的能力，可見衛(wèi)星數(shù)目少于4顆甚至僅有1顆時(shí)，依然能保持組合模式，并可提供高于單一慣性導(dǎo)航系統(tǒng)的導(dǎo)航精度[3]。由于導(dǎo)航環(huán)境的復(fù)雜性和多變性，GPS/INS組合導(dǎo)航的系統(tǒng)模型往往具有非線性和不確定性的特點(diǎn)[4]，本文采用緊組合導(dǎo)航的非線性模型，選取了UKF (無跡卡爾曼濾波，Unscented Kalman Filter)和CKF(容積卡爾曼濾波，Cubature Kalman filter)2種基于確定性采樣的典型非線性濾波，分析其濾波估計(jì)精度。

1 GPS/INS緊組合模型

圖1 緊組合工作框圖

1.1 狀態(tài)方程

GPS誤差狀態(tài)變量為2維的XG(t)，INS誤差XI(t)包括平臺(tái)誤差角(3維)、速度誤差(3維)、位置誤差(3維)、INS元件誤差(9維)共計(jì)18維。GPS/INS組合導(dǎo)航系統(tǒng)的狀態(tài)方程可寫作：

即

其中：

1.2 量測方程

設(shè)載體在地球大地坐標(biāo)系中的慣導(dǎo)輸出位置可表示為經(jīng)度λI、緯度LI、高度hI，則載體真實(shí)位置可表示為λ=λI-δλ，L=LI-δL，h=hI-δh，將其轉(zhuǎn)換為地心地球固連(ECEF)坐標(biāo)系為：

則衛(wèi)星到載體真實(shí)距離ρj(j=1,2,3,4)可表示為：

GPS接收機(jī)測得到衛(wèi)星j之間的偽距可表示為：

將x,y,z帶入上式得：

將x,y,z帶入上式即可得到偽距率的量測方程，以接收機(jī)輸出的偽距信號(hào)作為觀測量的量測方程(限于篇幅不再展開)表示為：

2 UKF和CKF濾波算法

2.1 無跡卡爾曼濾波

無跡卡爾曼濾波是一種典型的非線性濾波，在組合導(dǎo)航中應(yīng)用廣泛，學(xué)者已經(jīng)研究表明其濾波精度優(yōu)于擴(kuò)展卡爾曼濾波(EKF)[6]。

UKF通過引入U(xiǎn)T變換，用一組確定的樣本點(diǎn)近似求解測量條件下系統(tǒng)狀態(tài)的后驗(yàn)概率P(xk|zk)的均值和方差，實(shí)現(xiàn)系統(tǒng)狀態(tài)遞推均值和方差的估計(jì)[7]。

設(shè)系統(tǒng)的狀態(tài)方程和量測方程為：

xk+1=f(xk)+wk

zk+1=h(xk+1)+vk+1

其中wk，vk+1分別是系統(tǒng)噪聲和量測噪聲，其協(xié)方差分別是Qk，Rk+1?；赨T變換的標(biāo)準(zhǔn)UKF算法過程如下：

2) 根據(jù)系統(tǒng)狀態(tài)方程求取樣本點(diǎn)傳遞值：

xsi(k+1,k)=f(si(k))

3) 系統(tǒng)狀態(tài)均值和方差的一步預(yù)測，即：

4)根據(jù)系統(tǒng)量測方程求取狀態(tài)一步預(yù)測的傳遞值：

zsi(k+1,k)=h(xsi(k+1,k))

5)預(yù)測量均值和協(xié)方差：

式中：Pzz是量測方差矩陣；Pxz是狀態(tài)向量與量測向量的協(xié)方差矩陣。

6)計(jì)算UKF增益，更新狀態(tài)向量和方差，即：

2.2 容積卡爾曼濾波

容積卡爾曼濾波[8]是一種基于容積數(shù)值積分準(zhǔn)則的遞推型貝葉斯濾波器，該方法既無需計(jì)算雅可比矩陣，又不必預(yù)先設(shè)定參量。容積卡爾曼濾波是在容積積分準(zhǔn)則的基礎(chǔ)上提出的一種濾波算法。

容積積分準(zhǔn)則：

對(duì)于一般的積分問題：

令x=ry(r∈[0,+∞))，yyT=1，得xTx=r2，則上式轉(zhuǎn)化為徑向與超球面的二重積分為：

式中，Un表示單位超球面,σ(·)為積分微元。

對(duì)上面兩式分別采用Gauss-Hermite準(zhǔn)則和Spherical準(zhǔn)則,最終得

一般令mr=1，ms=2n(n是系統(tǒng)狀態(tài)維數(shù))即可滿足精度要求。容積卡爾曼濾波算法中容積點(diǎn)數(shù)目是狀態(tài)維數(shù)的2倍(各點(diǎn)成對(duì)對(duì)稱出現(xiàn))，樣本數(shù)較UKF要少，一定程度上減少了計(jì)算量。

容積卡爾曼濾波也分為時(shí)間更新階段和量測更新階段，其具體算法步驟如下：

1)時(shí)間更新

②計(jì)算傳播后的積分點(diǎn)，

③估計(jì)k時(shí)刻的狀態(tài)預(yù)測值和協(xié)方差

2) 量測更新

① Cholesky分解并計(jì)算積分點(diǎn)

②估計(jì)量測值

③估計(jì)自相關(guān)、互相關(guān)協(xié)方差陣

④計(jì)算CKF濾波增益，更新狀態(tài)和協(xié)方差

3 濾波精度分析

3.1 CKF估計(jì)精度

則CKF預(yù)測的f(x)的均值為

考慮到對(duì)稱性，奇階矩為0，所以

其中,aij為矩陣Sk-1|k-1中的第i行第j列元素。

進(jìn)一步可以寫成

其中，ξ為各分量之間的交叉項(xiàng)。

從而CKF預(yù)測的f(x)的均值為

3.2 UKF估計(jì)精度

由3.1節(jié)推導(dǎo)CKF估計(jì)精度的過程可類似求得UKF的估計(jì)精度為：

3.3 濾波精度對(duì)比

當(dāng)I>1時(shí)，積分公式將引入大量的截?cái)嗾`差[9，11]。

由3.1節(jié)和3.2節(jié)可知，對(duì)比2種方法的濾波精度，只需比較nk-1和(n+λ)k-1。對(duì)于真實(shí)均值的四階矩，為了捕獲其大部分信息，UKF濾波過程中選擇n+λ=3[12]，其中n為狀態(tài)變量維數(shù)，λ為微調(diào)因子。

當(dāng)n<3時(shí)，nk-1<(n+λ)k-1，即UKF的濾波精度高于CKF；

當(dāng)n=3時(shí)，nk-1=(n+λ)k-1，即UKF和CKF的濾波精度相當(dāng)；

由IUKF的表達(dá)式可知，IUKF與狀態(tài)維數(shù)n為線性關(guān)系，隨著n的線性增加，積分的數(shù)值估計(jì)中出現(xiàn)很大的誤差，積分公式精度變差。

由以上分析可以看出，當(dāng)狀態(tài)變量的維數(shù)大于3維時(shí)，CKF的濾波精度會(huì)高于UKF。第1節(jié)中GPS/INS緊組合模型為狀態(tài)變量為20維的狀態(tài)，使用CKF可以提供更加精確的導(dǎo)航信息。

4 仿真驗(yàn)證

設(shè)計(jì)仿真時(shí)長為500s的飛行過程，其中包括勻速、加速、減速、轉(zhuǎn)彎、爬升、俯沖、平飛等過程，具體飛行軌跡如圖2所示。

圖2 飛行軌跡仿真圖

仿真參數(shù)設(shè)置如下：SINS的陀螺隨機(jī)漂移εb為0.01(°)/h；陀螺儀一階馬爾科夫過程隨機(jī)噪聲εr為0.01(°)/h；加速度計(jì)漂移誤差Δ為10-4g；GPS隨機(jī)偽距誤差為10m，隨機(jī)偽距率測量誤差為0.05m/s；設(shè)模擬飛行器的采樣周期為0.01s，捷聯(lián)慣性導(dǎo)航的更新頻率為50Hz，GPS接收機(jī)的更新頻率為1Hz，濾波器的數(shù)據(jù)融合頻率同樣為1Hz。在上述實(shí)驗(yàn)環(huán)境中，圖3～8為分別采用UKF和CKF算法對(duì)導(dǎo)航誤差進(jìn)行估計(jì)的曲線圖，表1為位置、速度均方根誤差表。

圖3 經(jīng)度誤差曲線

圖4 緯度誤差曲線

圖5 高度誤差曲線

圖6 東向速度誤差曲線

圖7 北向速度誤差曲線

從圖3～8及表1可以看出，GPS/INS 組合導(dǎo)航中，采用CKF濾波后的各參數(shù)誤差小于UKF的估計(jì)誤差，位置、速度估計(jì)精度得到了改善，提高了40%以上。

5 結(jié)束語

在分析了GPS/INS緊組合導(dǎo)航的基礎(chǔ)上，采用非線性模型，然后從函數(shù)泰勒展開式及數(shù)值穩(wěn)定性上分析了UKF和CKF兩種濾波方法。理論分析表明三維以上的非線性系統(tǒng)采用CKF的濾波精度將高于UKF。

在GPS/INS緊組合導(dǎo)航中用CKF對(duì)導(dǎo)航誤差進(jìn)行濾波估計(jì)，并與UKF對(duì)比，其導(dǎo)航誤差得到了降低，提高了導(dǎo)航定位的精度。CKF的采樣點(diǎn)比UKF少，減少了濾波過程的計(jì)算量，在GPS/INS組合導(dǎo)航工程應(yīng)用中有一定的參考價(jià)值。

圖8 天向速度誤差曲線

表1 位置、速度均方根誤差(RMSE)

[1] 全偉.慣性/天文/衛(wèi)星組合導(dǎo)航技術(shù)[M].北京: 國防工業(yè)出版社, 2011.(QUAN Wei. INS/CNS/GNSS Integrated Navigation Technology[M]. Beijing: National Defense Industry Press, 2011.)

[2] 王惠南.GPS導(dǎo)航原理與應(yīng)用[M].北京：科學(xué)出版社, 2003.(WANG Huinan. Principles and Applications of GPS Navigation[M]. Beijing: Science Press, 2003.)

[3] 許剛，黃國榮，彭興釗，等.衛(wèi)星信號(hào)失效條件下SINS/GPS不同組合方式的性能比較[J].計(jì)算機(jī)應(yīng)用研究，2012，29(10):3888-3896.(XU Gang, HUANG Guorong, PENG Xingzhao, et al. Comparison of Different Integrated Patterns of SINS/GPS System under Condition of GPS Signal Invalidation[J]. Application Research of Computers, 2012, 29(10):3888-3896.)

[4] 趙琳, 王小旭, 丁繼成.組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J].中國慣性技術(shù)學(xué)報(bào)，2009, 17(1): 46-54.(ZHAO Lin, WANG Xiaoxu, DING Jicheng. Overview of Nonlinear Filter Methods Applied in Integrated Navigation System[J]. Journal of Chinese Inertial Technology, 2009,17(1):46-54.)[5] 劉建業(yè),曾慶化,趙偉,等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社, 2010.(LIU Jianye, ZENG Qinghua, ZHAO Wei,et al. Theory and Applications of Navigation System[M]. Xi′an: Northwestern Polytechnical University Press, 2010.)

[6] Farid Gul, Fang Jiancheng. Alternate of GPS for Ballistic Vehicle Navigation[C]. Systems and Control in Aerospace and Astronautics, 2006. ISSCAA 2006. 1st International Symposium on IEEE, 2006.

[7] 卞鴻巍，李安，覃方君，等.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M].北京: 國防工業(yè)出版社,2010.(BIAN Hongwei, LI An, TAN Fangjun, et al. The Application of Modern Information Fusion Technology in Integrated Navigation[M]. Beijing: National Defense Industry Press, 2010.)

[8] Ienkaran Arasaratnam, Simon Haykin. Cubature Kalman filters[J]. IEEE Trans. on Automatic Control, 2009, 54 (6): 1254-1269.

[9] Genz A, Keister B D. Fully Symmetric Interpolatory Rules for Multiple Integrals over Infinite Regions with Gaussian Weight[J]. Journal of Computational and Applied Mathematics,1996, 71(2): 299-309.

[10] Wu Yuanxin, Hu Dewen, Wu Meiping, et al. A Numerical-integration Perspective on Gaussian Filters[J]. IEEE Trans. on Signal Processing, 2006, 54(8): 2910-2921.

[11] Stroud A H. Approximate Calculation of Multiple Integrals[M]. NJ: Prentice Hall, 1971: 98-100.

[12] Julier S, Uhlman J, Durrant-Whyte H F. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators[J]. IEEE Trans. on Automatic Control,2000, 45(3): 477-482.

Cubature and Unscented Kalman Filter Accuracy Analysis Based on Tightly-Coupled GPS/INS Integrated Navigation System

MENG Yan ZHU Lixin

Electronic Engineering Institute Anhui Province Key Laboratory of Electronic Restriction, Hefei 230037, China

Theusingnon-linearandtightly-coupledmodelisusedinGPS/INSintegratednavigationsystem,whichisbasedonerrorsasstatevariables.AnalysisaccuracyofunscentedKalmanfilter(UKF)andcubatureKalmanfilter(CKF)fromtheTaylorexpansionofthefunctionandstabilityfactorisintroducedtoprovethattheaccuracyofUKFisdeterioratedandthefilteringaccuracyofCKFisbetterthanUKFwhenthestatedimensionismorethanthreedimensions.Twoalgorithmsareusedinintegratednavigationsystemandsimulationresultsverifythecorrectnessofthetheoreticalanalysisandoffercertainreferencevalueforengineeringapplications.

GPS/INS;UKF;CKF;Taylorexpansion;Stabilityfactor;Navigationaccuracy

2013-08-29

孟(1989-),男，山東淄博人，碩士研究生，主要研究方向?yàn)榻M合導(dǎo)航、信息處理；朱立新(1963-)，女，安徽無為人，副教授，主要研究方向?yàn)樾畔⑻幚?、目?biāo)跟蹤和衛(wèi)星導(dǎo)航等。

TN967.2

A

1006-3242(2014)01-0010-06