改進四元數(shù)無味卡爾曼濾波算法

李開龍, 胡柏青, 常路賓

(海軍工程大學導航工程系, 湖北 武漢 430033)

?

改進四元數(shù)無味卡爾曼濾波算法

李開龍, 胡柏青, 常路賓

(海軍工程大學導航工程系, 湖北 武漢 430033)

摘要：針對四元數(shù)無味卡爾曼濾波計算量較大等問題,提出一種改進的四元數(shù)無味卡爾曼濾波算法,通過四元數(shù)與修正羅德里格斯參數(shù)的線性變換,避免了四元數(shù)無味卡爾曼濾波中,四元數(shù)與誤差羅德里格斯參數(shù)頻繁切換所帶來的計算量增大的問題,從而降低了算法的計算量。推導了修正羅德里格斯參數(shù)出現(xiàn)奇異值時,改進四元數(shù)無味卡爾曼濾波的奇異值避免的轉換公式。在捷聯(lián)慣性導航系統(tǒng)/全球衛(wèi)星定位系統(tǒng)(strapdown inertial navigation system / global positioning system,SINS/GPS)直接式位置松組合仿真試驗中,對比了乘性擴展卡爾曼濾波、四元數(shù)無味卡爾曼濾波和改進四元數(shù)無味卡爾曼濾波的估計性能,證明了所提出算法的有效性。

關鍵詞：四元數(shù); 姿態(tài)估計; 濾波; 組合導航

0引言

姿態(tài)估計技術在航空航天、導航等領域中擁有廣泛的應用[1-4]。姿態(tài)表示方法是研究姿態(tài)估計問題的基礎,其中有四參量的四元數(shù)表示方法和三參量的旋轉矢量、羅德里格斯參數(shù)族以及Euler角等表示方法[5]。其中羅德里格斯參數(shù)族表示方法包含羅德里格斯參數(shù)(Rodrigues parameter, RP)、修正羅德里格斯參數(shù)(modified Rodrigues parameter,MRP)等姿態(tài)表示方法。不同的表示方法具有其特殊性。但是,其中尤為突出的一個特點就是任何三參量的姿態(tài)表示方法都無法全局非奇異,而四參量的四元數(shù)表示方法卻具有全局非奇異的優(yōu)勢[6-7],基于四元數(shù)的優(yōu)越性,其逐漸成為姿態(tài)表示當中應用最為廣泛的表示方法[8-9]。

姿態(tài)估計實現(xiàn)的根本途徑是現(xiàn)代濾波技術,Kalman濾波是現(xiàn)代濾波技術當中最主要的技術手段,從線性最優(yōu)的經典Kalman濾波到應用于非線性系統(tǒng)的擴展Kalman濾波(extended Kalman filter,EKF)、無味Kalman濾波(unscented Kalman filter,UKF),Kalman濾波技術的發(fā)展極大地促進了姿態(tài)估計研究的進步。在非線性Kalman濾波中,基于無味變換(unscented transformation, UT)的UKF具有比模型線性化的EKF更高的濾波精度[10-11],因此,UKF已經成為非線性濾波體系中研究的重點與熱點。

然而,如何將四元數(shù)同UKF濾波結合起來,卻長期遇到一個關鍵性問題,即四元數(shù)在非線性濾波當中的約束性問題。文獻[12-13]提出的四元數(shù)無味Kalman濾波算法(quaternion unscented Kalman filter,USQUE)成功解決了該問題,并將該算法從航空姿態(tài)估計當中引入到組合導航領域中來,受到廣泛地關注。文獻[14]也提出了一種“分層濾波”的四元數(shù)濾波算法,即采用不存在奇異性的四元數(shù)進行外層的姿態(tài)更新,采用不存在約束性的旋轉矢量表示姿態(tài)誤差進行內層濾波算法遞推。另外,文獻[15-16]在文獻[14]的基礎上,采用慣導基本方程作為濾波模型,研究了在低精度微機械系統(tǒng)(micro electro mechanical systems,MEMS)與衛(wèi)星導航系統(tǒng)(global navigation satellite system,GNSS)緊組合中的應用等問題。雖然,USQUE算法能夠得到較好的姿態(tài)估計效果,但是,為了避免四元數(shù)維數(shù)同四元數(shù)濾波方差的維數(shù)不匹配的問題,需要利用四元數(shù)與誤差修正羅德里格斯參數(shù)(error-MRP)反復的切換,并且這種切換是通過乘性四元數(shù)公式進行的,增大了算法的計算量,使得算法在實際問題中的應用受到了限制。雖然,后續(xù)學者對此展開了卓有成效的研究[17-18],但是總結來說其算法都是沒有考慮四元數(shù)維數(shù)同四元數(shù)濾波方差的維數(shù)匹配性問題的,為了達到降低計算量的效果,將四元數(shù)姿態(tài)表示部分的濾波方差直接設置為4×4,然而,由于四元數(shù)是沒有物理意義的參量,因此,這種濾波方差設置是帶有極強主觀性的。

對此,本文提出一種改進的四元數(shù)無味卡爾曼濾波算法(modified USQUE, MUSQUE),利用四元數(shù)與MRP線性變換的特點,避免了USQUE算法中乘性四元數(shù)變換增大計算量的問題。濾波算法同樣采用“分層濾波”的思想,兼顧考慮到四元數(shù)維數(shù)與四元數(shù)濾波方差維數(shù)匹配性的問題,在外層采用四元數(shù)進行姿態(tài)更新,在內層采用MRP進行姿態(tài)的遞推,同時推導MRP出現(xiàn)奇異值的避免方法。將改進算法應用到捷聯(lián)慣性導航系統(tǒng)(strapdown inertial navigation system,SINS)/全球衛(wèi)星定位系統(tǒng)(global positioning system，GPS)直接式組合導航問題中,與乘性擴展卡爾曼濾波(multiplicative extended Kalman filter， MEKF)和USQUE算法進行比較,證明了改進算法在保持較高精度情況下,具有更好的時效性和可信性。

1MRP與USQUE算法

1.1MRP

MRP是一種姿態(tài)表示的方法,同四元數(shù)和旋轉矢量有式(1a)、式(1b)的變

(1a)

(1b)

另外,作為一種三參量的姿態(tài)表示方法,MRP是存在姿態(tài)奇異點的,通過式(1a)中MRP與旋轉矢量的關系可知,MRP將在-2π與2π以及其整數(shù)周期點處出現(xiàn)奇異,因此,在采用羅德里格斯參數(shù)族姿態(tài)表示進行濾波時,需要考慮姿態(tài)表示的切換問題以避免奇異,對于MRP來說切換判斷點為‖σ‖>1。

1.2USQUE算法

USQUE算法本質上解決了四元數(shù)在非線性濾波當中應用的兩個問題,其一是解決了四元數(shù)在非線性濾波當中約束性的問題;其二是采用 “分層濾波”框架,解決了四元數(shù)同濾波方差維數(shù)匹配性問題。USQUE算法外層采用四元數(shù)進行姿態(tài)更新,內層采用error-MRP進行姿態(tài)遞推方式進行,總結其算法流程如下：

首先定義一個離散的非線性模型

(2)

式中,xk∈Rn為k時刻的狀態(tài)量；yk∈Rm為k時刻的觀測量；f(·)和h(·)分別為系統(tǒng)的狀態(tài)方程與觀測方程；uk-1和vk分別為零均值的系統(tǒng)狀態(tài)白噪聲和零均值的系統(tǒng)觀測白噪聲,即qk-1～N(0,Qk-1),vk～N(0,Rk)。

(3)

時間更新產生Sigma點

外層姿態(tài)更新

(4)

內層姿態(tài)遞推

(5)

(6)

(7)

量測更新

如果觀測方程是非線性的,那么步驟同時間更新類似,如果觀測方程是線性的,那么可以利用線性Kalman量測更新過程進行,這里不再贅述。

姿態(tài)更新

(8)

通過以上USQUE算法流程可知,在每步中USQUE算法都將利用乘性四元數(shù)關系進行多次error-MRP與四元數(shù)的反復切換,這使得算法的計算量增大。據此,提出一種改進的四元數(shù)無味卡爾曼濾波算法,在保證精度的前提下,降低其計算量。

2改進四元數(shù)無味卡爾曼濾波算法

MUSQUE算法利用四元數(shù)與MRP線性變換特點及“分層濾波”思想進行的,其算法流程如下：

時間更新

產生Sigma點:

(9)

(10)

計算出一步預測狀態(tài)與濾波方差如式(6)和式(7)所示。

量測更新同時間更新類似,這里不再贅述。

對于‖σ‖>1情況下,采用將四元數(shù)與MRP切換轉換為四元數(shù)與error-MRP切換的方法,即:

濾波方差切換用Pσ表示基于MRP的濾波方差,用Pδσ表示基于error-MRP的濾波方差。

采用UT變換的形式來保證轉換精度,δσ與σ變換采用乘性MRP公

(11)

(12)

(13)

(14)式中

(15)

值得指出得是σ(0)是零時刻的MRP值,這里將零時刻的MRP值作為參考MRP值來進行乘性MRP公式,另外,對于參考MRP的選取,也可以采用類似文獻[20]中選取參考四元數(shù)的運算形式,雖然文獻[20]方法保證了算法的完備性,但是卻增大了計算量,因此,這里仍采用零時刻的MRP值作為參考MRP值。

3仿真試驗

3.1SINS/GPS直接式組合導航模型

本文采用SINS/GPS直接式位置松組合導航模型進行仿真試驗,其中,直接式是指系統(tǒng)狀態(tài)方程采用捷聯(lián)慣導基本方程,位置松組合是指觀測方程采用GPS提供的位置信息作輔助校正信息。記SINS的載體坐標系 (右-前-上)為b系,地理坐標系或導航坐標系 (東-北-天)為n系,地球坐標系為e系,慣性坐標系為i系?？紤]SINS/GPS直接式組合導航方

(16a)

(16b)

(16c)式中

(16d)

(16e)

另外,觀測模型考慮SINS/GPS位置松組合模式,則

(17)

式中，vk為零均值的觀測白噪聲,其方差為Rk。則式(16a)～式(16e)和式(17)構成了SINS/GPS直接式位置松組合導航的系統(tǒng)模型。

3.2對比試驗

對比試驗中應用SINS/GPS直接式組合導航模型,比較MUSQUE與USQUE和MEKF算法對于姿態(tài)、速度與位置估計的精度如圖1～圖3所示,仿真軌跡如圖4所示,記錄運動軌跡的姿態(tài)trj(qk)、速度trj(vk)和位置trj(pk)信息作為參考,仿真步長Δt為0.1 s,運動軌跡的仿真時間T為1 300 s。

圖1　姿態(tài)估計結果

特別地,選取如圖4所示的仿真軌跡,旋轉矢量是大于2π的,MRP將會產生奇異,如果未采取奇異值避免方法,則會產生濾波結果的發(fā)散。如圖5所示,采用3σ準則來衡量未進行奇異值避免的基于MRP濾波估計結果的可信性,σ是標準差,3σ準則是指理論上濾波估計結果將以99.73%的概率落在±3σ曲線范圍內,并且±3σ越接近估計曲線,那么濾波算法越可信。在未進行奇異值避免情況下,基于MRP的濾波估計結果將很快發(fā)散,并且3σ準則也證明了濾波結果將不再可信,也就是已經發(fā)散。由于MUSQUE是基于四元數(shù)與MRP切換的,會遇到奇異性問題。因此,考慮仿真全面性,在該情況下來驗證MUSQUE具有較好的估計效果和更小的計算量。

圖2　速度估計結果

圖3　位置估計結果

圖4　仿真軌跡圖

圖5　奇異值對MRP的估計影響

設置初始化參數(shù)如下所示:

圖1～圖3是3種濾波的姿態(tài)、速度與位置估計誤差的結果。從圖中可以發(fā)現(xiàn)基于UKF濾波框架的USQUE和MUSQUE算法精度均高于基于EKF濾波框架的MEKF。另外,從圖中可以發(fā)現(xiàn)MUSQUE算法的估計精度同USQUE算法精度是基本相當?shù)?但是算法計算量卻降低,這里用算法仿真計算時間來衡量計算量大小,在該次仿真試驗中USQUE、MUSQUE與MEKF計算時間分別為172.183 s、136.564 s、105.048 s。MEKF比USQUE和MUSQUE計算時間都要少,這是由于UKF濾波是基于UT變換的,在直接式組合導航中,UT變換產生的每個Sigma點本質上都將進行一次導航解算過程,狀態(tài)維數(shù)越大,產生的Sigma點越多,導航解算的次數(shù)也就越多,算法占用的時間也就比模型線性化處理的EKF濾波要大很多。但是,從濾波精度上考慮,USQUE與MUSQUE算法要明顯優(yōu)于MEKF,基于這一點,MUSQUE在精度基本保持不變情況下,降低了算法的計算量,相比USQUE更具有時效性。另外,考慮大失準角度情況下,濾波算法可信性與有效性,設置初始角度為真實角度的30倍,比較USQUE和MUSQUE算法姿態(tài)估計精度如圖6所示。

圖6　大初始誤差角情況下算法比較

圖6是在大初始誤差角情況下USQUE與MUSQUE算法估計效果比較,由圖中可知,USQUE與MUSQUE均落在±3σ區(qū)域內,但顯然,MUSQUE算法的3σ更接近估計曲線,說明MUSQUE比USQUE算法更為可信的。

為了進一步說明算法的有效性,采用蒙特卡羅仿真方法,初始條件不變,蒙特卡羅仿真50次,并分別求取姿態(tài)、速度、位置的估計誤差均方根值及仿真50次3種算法計算時間的平均值,總結如表1所示(CPUofIntelcorei3-2328M, 2.20GHz,MatlabR2011a)。

表1　平均估計精度及運行時間比較

4結論

USQUE算法為了保持四元數(shù)狀態(tài)維數(shù)同四元數(shù)濾波方差維數(shù)匹配性問題,頻繁將四元數(shù)與error-MRP進行切換,增大了算法的計算量。針對USQUE算法計算量較大的問題,在兼顧估計精度與匹配性問題的基礎上,提出了一種改進的MUSQUE算法,通過四元數(shù)與MRP進行線性切換,避免了USQUE算法中通過乘性四元數(shù)關系進行切換計算量大的問題,保持了算法的估計精度。同時,推導了在MRP出現(xiàn)奇異時,奇異值避免的解決途徑。仿真結果表明,MUQUE相比USQUE在估計精度相當?shù)那闆r下,具有更低的計算量和更高的可信度,并相比MEKF具有更高的估計精度。

參考文獻：

[1] Crassidis J L, Markley F L. Unscented filtering for spacecraft attitude estimation[J].JournalofGuidance,Control,andDynamics, 2003, 26(4): 536-542.

[2] Li J G, Cui H T, Tian Y. Space attitude estimation algorithm abased on multiplicative quaternion and constrained filter[J].SystemsEngineeringandElectronics,2013,35(5):1032-1036.(李建國,崔祜濤,田陽.基于四元數(shù)與約束濾波的飛行器姿態(tài)估計算法[J].系統(tǒng)工程與電子技術,2013,35(5):1032-1036.)

[3] Suh Y S. Attitude estimation by multiple-mode Kalman filters[J].IEEETrans.onIndustrialElectronics, 2006, 53(4): 1386-1389.

[4] Markley F L, Mortari D. Quaternion attitude estimation using vector observations[J].JournaloftheAstronauticalSciences, 2000, 48(2): 359-380.

[5]Markley F L, Crassidis J L.Fundamentalsofspacecraftattitudedeterminationandcontrol[M]. New York:Springer, 2014.

[6] O’Keefe S A, Schaub H. Shadow set considerations for modified rodrigues parameter attitude filtering[J].JournalofGuidance,Control,andDynamics, 2014: 1-5.

[7] Karlgaard C D, Schaub H. Nonsingular attitude filtering using modified rodrigues parameters[J].JournaloftheAstronauticalSciences, 2009, 57(4): 777-791.

[8] Marins J L, Yun X, Bachmann E R, et al. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors[C]∥Proc.oftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystems, 2001: 2003-2011.

[9] Markley F L. Fast quaternion attitude estimation from two vector measurements[J].JournalofGuidance,Control,andDynamics, 2002, 25(2): 411-414.

[10] Zhou W D,Qiao X W, Ji Y R, et al. SINS/GPS tightly integrated navigation system based on quaternion square root unscented Kalman filter[J].SystemsEngineeringandElectronics,2010,32(12):2643-2647.(周衛(wèi)東,喬相偉,吉宇人,等.基于四元數(shù)平方根UKF算法的SINS/GPS緊組合導航系統(tǒng)研究[J].系統(tǒng)工程與電子技術,2010,32(12):2643-2647.)

[11] Chang L, Hu B, Chang G, et al. Huber-based novel robust unscented Kalman filter[J].IETScience,Measurement&Technology, 2012, 6(6): 502-509.

[12] Crassidis J L. Sigma-point Kalman filtering for integrated GPS and inertial navigation[J].IEEETrans.onAerospaceandElectronicSystems, 2006, 42(2): 750-756.

[13] Crassidis J L, Markley F L, Cheng Y. Survey of nonlinear attitude estimation methods[J].JournalofGuidance,Control,andDynamics, 2007, 30(1): 12-28.

[14] Shin E H, El-Sheimy N. An unscented Kalman filter for in-motion alignment of low-cost IMUs[C]∥Proc.oftheIEEEPositionLocationandNavigationSymposium, 2004: 273-279.

[15] Yang Y, Zhou J, Loffeld O. Quaternion-based Kalman filtering on INS/GPS[C]∥Proc.ofthe15thIEEEInternationalConferenceonInformationFusion, 2012: 511-518.

[16] Zhou J, Edwan E, Knedlik S, et al. Low-cost INS/GPS with nonlinear filtering methods[C]∥Proc.ofthe13thIEEEConferenceonInformationFusion, 2010: 1-8.

[17] Qian H M, Huang W, Sun L. Cubature Kalman filter with quaternion constraint and its application[J].JournalofHarbinInstituteofTechnology,2014,46(7):41-46.(錢華明,黃蔚,孫龍.四元數(shù)約束的容積卡爾曼濾波及其應用[J].哈爾濱工業(yè)大學學報,2014,46(7):41-46.)

[18] Zhou W D, Qiao X W, Ren L, et al. Quaternion unsecnted Kalman filter and its application to initial alignment of SINS[J].ChineseJournalofScientificInstrument,2010(2):264-269.(周衛(wèi)東,喬相偉,任蕾,等.QUKF算法及其在SINS初始對準中的應用[J].儀器儀表學報,2010(2):264-269.)

[19] Soken H E, Hajiyev C. REKF and RUKF for pico satellite attitude estimation in the presence of measurement faults[J].JournalofSystemsEngineeringandElectronics,2014,25(2):288-297.

[20] Chang L, Hu B, Chang G. Modified unscented quaternion estimator based on quaternion averaging[J].JournalofGuidance,Control,andDynamics, 2013, 37(1): 305-309.

[21] Chang L, Hu B, Li A, et al. Strapdown inertial navigation system alignment based on marginalised unscented Kalman filter[J].IETScience,Measurement&Technology, 2013, 7(2): 128-138.

李開龍(1988-),男,博士研究生,主要研究方向慣性導航技術及應用。

E-mail:lee_kailong@163.com,

胡柏青(1964-),男,教授,博士研究生導師,博士,主要研究方向為慣性導航技術及應用。

E-mail:hubaiqing2005@163.com

常路賓(1987-),男,講師,博士,主要研究方向為慣性導航技術及應用。

E-mail:changlubin@163.com

Modified quaternion unscented Kalman filter

LI Kai-long, HU Bai-qing, CHANG Lu-bin

(DepartmentofNavigationEngineering,NavalUniversityofEngineering,Wuhan430033,China)

Abstract:Aiming at the problem of larger calculation burden of the quaternion unscented Kalman filter (USQUE), a modified USQUE (MUSQUE) is proposed. Based on the transformation between the quaternion and the modified rodriguez parameter (MRP), the novel method can reduce calculation amount by avoiding frequent transformation between the quaternion and the error-MRP for the USQUE. Meanwhile, the singularity avoidance method is also derived in case when the MRP is singularity. By simulating strapdown inertial navigation system/global positioning system (SINS/GPS) direct integrated navigation, the results prove the validation of the novel method comparing with the multiplicative extended Kalman filter and USQUE.

Keywords:quaternion; attitude estimation; filter; integrated navigation

收稿日期：2015-04-04；修回日期：2015-11-17；網絡優(yōu)先出版日期：2016-02-15。

基金項目：國家重點基礎研究發(fā)展計劃(973計劃)(2012CB719902)；國家自然科學基金(61304241,61374206,41374082)資助課題

中圖分類號：U 666.12

文獻標志碼：A

DOI:10.3969/j.issn.1001-506X.2016.06.28

作者簡介：

網絡優(yōu)先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160215.1436.006.html