基于USQUE的四元數(shù)容積卡爾曼濾波算法在組合導(dǎo)航中的應(yīng)用*

張正彬

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

?

基于USQUE的四元數(shù)容積卡爾曼濾波算法在組合導(dǎo)航中的應(yīng)用*

張正彬

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

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

組合導(dǎo)航; 卡爾曼濾波; 四元數(shù); USQUE

Class Number U666.11

1 引言

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

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

2 USQUE算法

考慮一個非線性離散系統(tǒng)模型如下

(1)

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

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

(2)

時間更新:

δq0,k-1(i)=

(3)

(4)

根據(jù)乘性四元數(shù)公式得到

(5)

為了計算一步預(yù)測狀態(tài)及狀態(tài)濾波方差,根據(jù)乘性四元數(shù)公式有

(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時刻的觀測量。

姿態(tài)更新:

(15)

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

平方根容積卡爾曼濾波算法(SRCKF)是容積卡爾曼濾波算法(CKF)的平方根濾波形式,在該算法中不是用濾波方差進行傳遞,而是用濾波方差的平方根形式進行傳遞的。因此,其Sigma點采樣方式和權(quán)重計算為[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)

姿態(tài)更新:姿態(tài)更新同USQUE的算法相同,這里不再贅述。

4 仿真試驗

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

(27a)

(27b)

(27c)

通過仿真一段組合導(dǎo)航軌跡如圖1所示。

圖1 組合導(dǎo)航仿真軌跡

應(yīng)用該算法分別對姿態(tài)、速度及位置信息進行估計,其估計結(jié)果如圖2～圖4所示。

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

圖2 姿態(tài)估計誤差

圖3 速度估計誤差

圖4 位置估計誤差

圖5 估計軌跡比較

5 結(jié)語

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

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

[2] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,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日

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

U666.11

10.3969/j.issn1672-9730.2015.02.017