基于USQUE的四元數容積卡爾曼濾波算法在組合導航中的應用*

張正彬

(海軍駐昆明地區軍事代表辦事處魚水雷室 昆明 650000)

?

基于USQUE的四元數容積卡爾曼濾波算法在組合導航中的應用*

張正彬

(海軍駐昆明地區軍事代表辦事處魚水雷室 昆明 650000)

組合導航是導航領域中一個重要的研究領域和應用方向。傳統的基于慣導誤差模型的組合導航模式,因模型在建立過程存在各種近似計算,這必然使得建立的模型中引入諸多近似誤差和模型誤差,因此,其信息融合的估計精度下降。論文基于USQUE計算流程,提出一種四元數容積卡爾曼濾波算法,并將該算法應用于SINS/GPS位置松組合導航問題當中,仿真結果驗證該算法的有效性并且具有較高的估計精度。

組合導航; 卡爾曼濾波; 四元數; USQUE

Class Number U666.11

1 引言

組合導航是導航領域中最為重要的研究分支,并已經成為目前導航領域的研究熱點和重要應用方向[1]。組合導航是現代濾波技術和信息融合理論的應用延伸,在組合導航中,相對于位置信息和速度信息,姿態信息的估計尤為重要,這是由于對于經典的SINS/GPS組合導航來說,高精度的位置信息和速度信息都可以通過高精度的外界組合設備獲得,而目前來說,對于姿態信息,無法直接有外部組合設備獲得,并且又由于該信息在慣導模型中往往耦合在速度信息當中,構成非線性的系統估計方程,因此,也無法直接通過經典的卡爾曼濾波(Kalman Filter,KF)來進行估計[2]。對此,學者們提出基于慣導誤差模型的組合導航模型,慣導誤差模型是基于慣導解算模型,并通過各種約定近似計算和部分解耦而獲得的,而這必然會引入近似誤差和模型誤差,造成估計精度的下降。

對此,Crassidis等學者提出了一種無味四元數估計方法(UnScented Quaternion Estimator,USQUE)[3～4],該算法在無味卡爾曼濾波(Unscented Kalman Filter,UKF)基礎上,利用修正羅格里德斯參數(MRP)搭建橋梁,成功將該算法應用到組合導航領域。USQUE算法的一個重要特點是對于濾波估計來說,其系統方程是基于慣導解算方程的,因此,這就避免了基于慣導誤差方程帶來的諸多問題。本文提出一種基于USQUE的四元數平方根容積卡爾曼濾波算法(Square-root Quaternion Cubature Kalman Filter,SR-QCKF),利用USQUE在組合導航中應用框架,并結合平方根容積卡爾曼濾波的狀態高維估計和數值計算的穩定性等特點,將該算法應用于SINS/GPS位置松組合導航仿真系統當中,仿真結果驗證了,該算法的有效性,并具有較高的估計精度。

2 USQUE算法

考慮一個非線性離散系統模型如下

(1)

xk∈in,yk∈im,f(·)和h(·)分別為系統的狀態函數與量測函數,qk-1和vk分別為零均值的系統狀態白噪聲和零均值的系統觀測白噪聲,即qk-1:N(0,Qk-1),vk:N(0,Rk)。

Crassidis等學者提出了一種USQUE的估計方法巧妙地解決了困擾四元數在非線性濾波當中應用的問題,以下給出USQUE算法的計算流程。

(2)

時間更新:

δq0,k-1(i)=

(3)

(4)

根據乘性四元數公式得到

(5)

為了計算一步預測狀態及狀態濾波方差,根據乘性四元數公式有

(6)

(7)

+Qk-1

(8)

量測更新:

(9)

+Rk

(10)

(11)

Kk=Pxy,k|k-1(Pyy,k|k-1)-1

(12)

(13)

Pk|k=Pk|k-1-KkPyy,k|k-1KkT

(14)

其中yk表示k時刻的觀測量。

姿態更新:

(15)

3 基于USQUE的四元數容積卡爾曼濾波算法

平方根容積卡爾曼濾波算法(SRCKF)是容積卡爾曼濾波算法(CKF)的平方根濾波形式,在該算法中不是用濾波方差進行傳遞,而是用濾波方差的平方根形式進行傳遞的。因此,其Sigma點采樣方式和權重計算為[5～6]

(16)

(17)

(18)

因此,基于這種采樣方式給出基于USQUE的平方根容積卡爾曼濾波算法流程如下

時間更新:

(19)

i=1,2,…,2n

(20)

量測更新:

Syy,k|k-1=Tria([Yk|k-1,SR,k])

(21)

(22)

其中SR,k是Rk的平方根形式,Xk|k-1和Yk|k-1表示為

(23a)

(23b)

(24)

(25)

Sk|k=Tria([Xk|k-1-KkYk|k-1,KkSR,k])

(26)

姿態更新:姿態更新同USQUE的算法相同,這里不再贅述。

4 仿真試驗

本文將該算法應用于SINS/GPS組合導航當中進行仿真計算,首先給出SINS/GPS組合導航系統方程和量測方程如下所示[2]。

(27a)

(27b)

(27c)

通過仿真一段組合導航軌跡如圖1所示。

圖1 組合導航仿真軌跡

應用該算法分別對姿態、速度及位置信息進行估計,其估計結果如圖2～圖4所示。

圖2～圖4表示SR-QCKF對于仿真軌跡姿態、速度和位置估計的誤差,從圖中可以發現該算法可以將穩態時的姿態估計誤差控制在幾個角分以內,速度估計誤差控制在1m/s以內,位置誤差控制在大約1m以內,這個結果對于組合導航來說是估計精度很高的,因此,這也驗證了該算法的有效性,為了更直接說明估計效果,給出估計軌跡圖如圖5所示。

圖2 姿態估計誤差

圖3 速度估計誤差

圖4 位置估計誤差

圖5 估計軌跡比較

5 結語

本文研究了一種應用于慣導解算方程的直接式組合導航算法,基于USQUE算法的基礎上,結合平方根容積卡爾曼濾波算法的特點,提出四元數平方根容積卡爾曼濾波算法,并通過應用在SINS/GPS組合導航當中進行仿真驗證,仿真結果驗證了該算法的有效性。然而,值得注意得是,由于利用了USQUE算法的框架,本文提出的算法在計算量上會比較大,因此,為了更好地能夠將該算法應用于實際,有必要進一步對降低計算量進行研究。

[1] 秦永元,張洪鉞,汪叔華.卡爾曼濾波與組合導航原理[M].西安:西北工業大學出版社.

[2] 秦永元.慣性導航[M].北京:科學出版社,2006.

[3] Crassidis J.L.,Markley F.L.Attitude Estimation Using Modified Rodrigues Parameters[C]//Proceedings of the Flight Mechanices/Estimation Theory Symposium,(NASA/CP-1996-3333) NASA-Goddard Space Flight Center,Greenbelt,MD,1996,:71-83.

[4] Crassidis J.C.,Markley F.L.Unscented Filtering for Spacecraft Attitude Estimation[J].Journal of Guidance,Control,and Dynamics,2003,26(4):536-542.

[5] Arasaratnam,I.Cubature Kalman filtering: theory & applications[D].Canada: Diss.Ph.D.Thesis,Department of Electrical & Computer Eng.,McMaster University,2009.

[6] Arasaratnam,I.,Haykin,S.Cubature kalman filters.Automatic Control[J].IEEE Transactions on,2009,54(6):1254-1269.

Application of Quaternion Cubature Kalman Filter Based on USQUE in Integrated SINS/GPS

ZHANG Zhengbin

(Torpedo and Mine Military Representatives Office of Navy in Kunming,Kunming 650000)

Integrated navigation is an important research field and application of navigation field.Because there are a variety of approximate calculation in the establishing process,the traditional model based on the inertial navigation error model is involved with many approximation error and model error,leading to the decrease of information fusion’s estimation accuracy.Based on the USQUE,a quaternion cubature Kalman filter is proposed,which is applied in SINS/GPS located integrated navigation.The simulation results verify the effectiveness and high estimation accuracy of the algorithm.

integrated navigation,Kalman filter,quaternion,USQUE

2014年8月12日,

2014年9月22日

張正彬,男,碩士,助理工程師,研究方向:魚水雷維修及姿態估計等。

U666.11

10.3969/j.issn1672-9730.2015.02.017