基于單向無跡卡爾曼濾波的飛機(jī)姿態(tài)算法

韓 萍，桑威林，趙愛軍，Daniel Alazard

（1. 中國(guó)民航大學(xué) 智能信號(hào)與圖像處理天津市重點(diǎn)實(shí)驗(yàn)室，天津 300300；2. 東方通用航空有限責(zé)任公司，天津 300300；3. 法國(guó)國(guó)立航空航天大學(xué) 數(shù)學(xué)與控制系，圖盧茲 31555，法國(guó)）

基于單向無跡卡爾曼濾波的飛機(jī)姿態(tài)算法

韓 萍1，桑威林1，趙愛軍2，Daniel Alazard3

（1. 中國(guó)民航大學(xué) 智能信號(hào)與圖像處理天津市重點(diǎn)實(shí)驗(yàn)室，天津 300300；2. 東方通用航空有限責(zé)任公司，天津 300300；3. 法國(guó)國(guó)立航空航天大學(xué) 數(shù)學(xué)與控制系，圖盧茲 31555，法國(guó)）

針對(duì)擴(kuò)展卡爾曼濾波方法在飛機(jī)姿態(tài)濾波中存在的線性化誤差大、需要解繁瑣的Jacobian矩陣等問題，將一種新型卡爾曼濾波方法——單向無跡卡爾曼濾波應(yīng)用于載有低精度、高噪聲傳感器的低成本飛機(jī)姿態(tài)濾波系統(tǒng)，并在精度及計(jì)算量上與EKF和容積卡爾曼濾波進(jìn)行了比較。利用實(shí)測(cè)飛行數(shù)據(jù)進(jìn)行實(shí)驗(yàn)，結(jié)果表明：相對(duì)于EKF，SUKF實(shí)現(xiàn)容易，且使姿態(tài)濾波精度提高到二階；相對(duì)于CKF，SUKF計(jì)算簡(jiǎn)單，比CKF減少約33%的計(jì)算量。

姿態(tài)濾波；單向無跡卡爾曼濾波；擴(kuò)展卡爾曼濾波；容積卡爾曼濾波；計(jì)算量

擴(kuò)展卡爾曼濾波（Extended Kalman Filter, EKF）是飛機(jī)姿態(tài)濾波中的常用方法，該方法將非線性函數(shù)Taylor展開式的一階項(xiàng)作為非線性函數(shù)的近似，從而將非線性問題轉(zhuǎn)換為線性問題進(jìn)行處理[1-5]。但對(duì)于精度低、噪聲大的傳感器，EKF自身存在的線性化誤差被加大，只能保證一階精度；另外EKF需要求解繁瑣的Jacobian矩陣，使得其計(jì)算相對(duì)復(fù)雜。

最近由常路賓等提出了一種新型卡爾曼濾波——單向無跡卡爾曼濾波（Simplex Unscented Kalman Filter, SUKF）[6]，該方法根據(jù)“近似非線性函數(shù)的概率分布比近似非線性函數(shù)更容易”的思想，并結(jié)合數(shù)學(xué)理論“對(duì)于n維的狀態(tài)矢量，只需要 n+1個(gè)合適的點(diǎn)就可以完全表征其均值和方差”，通過構(gòu)造 n+1個(gè)等權(quán)重的采樣點(diǎn)進(jìn)行狀態(tài)估計(jì)，使估計(jì)提高到二階精度。

本文將SUKF應(yīng)用于飛機(jī)姿態(tài)濾波，利用從微型實(shí)驗(yàn)飛機(jī)上獲取的飛行數(shù)據(jù)進(jìn)行實(shí)驗(yàn)，并將實(shí)驗(yàn)結(jié)果與EKF和CKF[7]得到的結(jié)果進(jìn)行比較。實(shí)驗(yàn)表明，在傳感器精度低，數(shù)據(jù)噪聲大的情況下，SUKF有效地提高了濾波精度、運(yùn)算效率和實(shí)時(shí)性，減少了計(jì)算量。

1 單向無跡卡爾曼濾波

SUKF是一種確定采樣型濾波方法，它采取了構(gòu)造單向采樣點(diǎn)的方式來達(dá)到逼近狀態(tài)后驗(yàn)分布的目的，采樣點(diǎn)的具體構(gòu)造過程如下[6]：

SUKF的具體實(shí)現(xiàn)過程如下：

1）時(shí)間更新

① 確定時(shí)間更新采樣點(diǎn)

chol(·)表示對(duì)矩陣進(jìn)行Cholesky分解：

2）量測(cè)更新

③ 計(jì)算卡爾曼增益

④ 狀態(tài)變量更新

⑤ 誤差協(xié)方差更新

2 飛機(jī)姿態(tài)濾波系統(tǒng)模型

2.1 姿態(tài)解算方程

由剛體轉(zhuǎn)動(dòng)理論，得姿態(tài)四元數(shù)運(yùn)動(dòng)方程為：

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

本文用四元數(shù)來描述飛機(jī)姿態(tài)，姿態(tài)解算時(shí)需要角速度。此外為了提高姿態(tài)濾波精度，用陀螺儀的隨機(jī)游走在線補(bǔ)償陀螺儀測(cè)量誤差，所以狀態(tài)矢量定為四元數(shù)、角速度和陀螺儀隨機(jī)游走的組合，即系統(tǒng)狀態(tài)矢量為：

陀螺儀廣泛采用的模型為[12]：

四元數(shù)與角速度的關(guān)系如圖1所示。

圖1 角速度和四元數(shù)處理模型Fig.1 Process model for angular rates and quaternion

由此建立飛機(jī)姿態(tài)濾波系統(tǒng)的非線性狀態(tài)方程如下[13]：

2.3 系統(tǒng)觀測(cè)方程

本實(shí)驗(yàn)所用姿態(tài)傳感器由三軸加速度計(jì)、三軸磁強(qiáng)計(jì)和三軸陀螺儀組成，由它們獲得飛機(jī)機(jī)體坐標(biāo)系下的重力矢量、磁場(chǎng)矢量和轉(zhuǎn)動(dòng)角速度，將這些參量作為觀測(cè)矢量，即：

根據(jù)捷聯(lián)慣性導(dǎo)航基本原理，利用姿態(tài)矩陣將導(dǎo)航坐標(biāo)系矢量轉(zhuǎn)換成機(jī)體坐標(biāo)系矢量。由此建立系統(tǒng)觀測(cè)方程如下[13]：

式中，

3 實(shí)驗(yàn)結(jié)果與分析

本文實(shí)驗(yàn)數(shù)據(jù)采自于裝有美國(guó)CH Robotics公司生產(chǎn)的CHR-6dm傳感器和法國(guó)SBG Systems公司生產(chǎn)的SBG傳感器的微型實(shí)驗(yàn)飛機(jī)。兩種傳感器均含有正交的三軸陀螺儀、三軸加速度計(jì)和三軸磁強(qiáng)計(jì)，并可提供未經(jīng)處理的機(jī)體坐標(biāo)系下三個(gè)方向的角速度、重力矢量和地磁場(chǎng)矢量。利用加速度計(jì)和磁強(qiáng)計(jì)對(duì)地球重力場(chǎng)和地磁場(chǎng)的測(cè)量值補(bǔ)償陀螺儀漂移，可以提高系統(tǒng)濾波精度，增強(qiáng)系統(tǒng)穩(wěn)定性。兩種傳感器內(nèi)部集成的均是EKF濾波方法，由于制作工藝不同，SBG傳感器精度比CHR-6dm傳感器精度要高，但價(jià)格也要高很多，約為CHR-6dm傳感器7倍。為了驗(yàn)證SUKF在高噪聲、低精度數(shù)據(jù)源時(shí)姿態(tài)濾波的有效性，將高精度SBG傳感器輸出的姿態(tài)角作為基準(zhǔn)，CHR-6dm傳感器獲得的數(shù)據(jù)作為源信號(hào)進(jìn)行處理。

實(shí)驗(yàn)中，兩個(gè)傳感器采樣頻率均為100 Hz，狀態(tài)矢量初值設(shè)為：

實(shí)驗(yàn)初始階段飛機(jī)處于地面啟動(dòng)狀態(tài)，利用此時(shí)段采集的數(shù)據(jù)計(jì)算過程噪聲協(xié)方差矩陣Q和觀測(cè)噪聲協(xié)方差矩陣R。實(shí)驗(yàn)結(jié)果如圖2～4所示。

為便于比較，將EKF和CKF濾波誤差也置于圖2～4中。由圖可知，SUKF濾波誤差小于EKF，較CKF也有改善，波動(dòng)性小于EKF，且容易達(dá)到穩(wěn)定，是一種有效的姿態(tài)濾波方法。

圖2 航向角濾波誤差Fig.2 Yaw filtering error

圖3 俯仰角濾波誤差Fig.3 Pitch filtering error

圖4 滾轉(zhuǎn)角濾波誤差Fig.4 Roll filtering error

由式(12)(13)可得CKF與SUKF的運(yùn)算量差值 Cdif為：

由表1知，CKF/EKF約為1.69，SUKF/EKF約為1.14；在保證二階精度的情況下，SUKF比CKF少約32.89%的計(jì)算量，與EKF計(jì)算量基本相當(dāng)。

表1 EKF、CKF與SUKF運(yùn)算量Tab.1 Computational Cost of EKF, CKF and SUKF

4 結(jié) 論

本文將單向無跡卡爾曼濾波應(yīng)用于由低精度高噪聲傳感器組成的低成本飛機(jī)姿態(tài)濾波系統(tǒng)，利用多組實(shí)測(cè)數(shù)據(jù)進(jìn)行驗(yàn)證，并給出了各方法計(jì)算量的統(tǒng)計(jì)對(duì)比。實(shí)驗(yàn)表明：相對(duì)于EKF，SUKF在姿態(tài)濾波過程中無需求導(dǎo)計(jì)算繁瑣的Jacobian矩陣，實(shí)現(xiàn)容易，且使濾波精度得到提高。相對(duì)于CKF，SUKF有效地提高了運(yùn)算效率和實(shí)時(shí)性，是一種高效的姿態(tài)濾波方法。

（

）：

[1] 付夢(mèng)印，鄧志紅，張繼偉. Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]. 北京：科學(xué)出版社，2003：82-84.

FU Meng-yin, DENG Zhi-hong, ZHANG Ji-wei. Kalman filtering theory and its application in navigation system [M]. Beijing: Science Press, 2003: 82-84.

[2] 王小旭，潘泉，黃鶴，等. 非線性系統(tǒng)確定采樣型濾波算法綜述[J]. 控制與決策，2012，27(6)：801-812.

WANG Xiao-xu, PAN Quan, HUANG He, et al. Overview of deterministic sampling filtering algorithms for nonlinear system[J]. Control and Decision, 2012, 27(6): 801-812.

[3] XU Yuan, CHEN Xi-yuan, LI Qing-hua. Unbiased tightlycoupled INS/WSN integrated navigation based on extended Kalman filter[J]. Journal of Chinese Inertial Technology, 2012, 20(3): 292-295.

[4] 郝燕玲，楊峻巍，陳亮，等. 基于平方根中心差分卡爾曼濾波的大方位失準(zhǔn)角初始對(duì)準(zhǔn)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(2)：180-189.

HAO Yan-ling, YANG Jun-wei, CHEN Liang, et al. Initial alignment of large azimuth misalignment based on square root central difference Kalman filter[J]. Journal of Chinese Inertial Technology, 2011, 19(2): 180-189.

[5] 黃耀光，高博，李建新，等. 基于平方根UKF雙向?yàn)V波的單站無源定位算法[J]. 數(shù)據(jù)采集與處理，2013，28(2)：207-212.

HUANG Yao-guang, GAO bo, LI Jian-xin, et al. Singleobserver passive location algorithm based on square-root UKF with forward-backward filtering[J]. Journal of Data Acquisition & Processing, 2013, 28(2): 207-212.

[6] HU B Q, Chang L B, Li A, et al. Computationally efficient simplex unscented Kalman filter based on numerical integration[J]. Asian Journal of Control, 2014, 16(2): 1-7.

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

[8] LIU Hua, LIU Tong. Quaternion based constrained algorithm in federated Kalman filtering for MEMS IMU/GNSS[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 392-396.

[9] 高社生，李偉，桑春萌. 基于四元數(shù)的SINS/SAR組合導(dǎo)航系統(tǒng)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，2010，18(1)：63-69.

GAO She-sheng, LI Wei, SANG Chun-meng. SINS/SAR Integrated navigation system using quaternion model [J]. Journal of Chinese Inertial Technology, 2010, 18(1): 63-69.

[10] 喬相偉，周衛(wèi)東，吉宇人. 用四元數(shù)狀態(tài)切換無跡卡爾曼濾波器估計(jì)的飛行器姿態(tài)[J]. 控制理論與應(yīng)用，2012，29(1)：97-103.

QIAO Xiang-wei, ZHOU Wei-dong, JI Yu-ren. Aircraft attitude estimation based on quaternion state-switching unscented Kalman filter[J]. Control Theory & Applications, 2012, 29(1): 97-103.

[11] Choi M H, Porter R, Shirinzadeh B. Comparison of attitude determination methodologies with low cost inertial measurement unit for autonomous aerial vehicle [C]//IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Wollongong, Australia, 2013: 1349-1354.

[12] Karlgaard C D, Schaub H. Adaptive huber-based filtering using projection statistics: Application to spacecraft attitude estimation[C]//AIAA, Guidance, Navigation, and Control Conference, Honolulu, USA, 2008: 1-20.

[13] Han Ping, Gan Haoliang, He Weikun, et al. Aircraft attitude estimation based on central difference Kalman filter[C]// Proceedings of 2012 IEEE the 11th International Conference on Signal Processing. Beijing, China, 2012: 294-298.

[14] Press W H, Teukolsky S A, Vetterling W T, Flannery B P. Numerical recipes in C: The art of scientific computing [M]. 2nd Edition. New York, Cambridge University Press, 1997: 96-98.

[15] Chapra S C, Canale R P. Numerical methods for engineers [M]. 6th Edition. New York: Mc Graw Hill Higher Education, 2010: 283-286.

Simplex unscented Kalman filter for aircraft attitude algorithm

HAN Ping1, SANG Wei-lin1, ZHAO Ai-jun2, Daniel Alazard3
(1. Tianjin Key Lab for Advanced Signal Processing, Civil Aviation University of China, Tianjin 300300, China; 2. Eastern General Aviation CO.LTD, Tianjin 300300, China; 3. Department of Mathematics and Control, Institut Supérieur de l'Aéronautique et de l'Espace, Toulouse 31555, France)

In view that the Extended Kalman Filter(EKF) in aircraft’s attitude filtering has such problems as large linearization error and having to solve complex Jacobian matrix, etc., a new Kalman filter algorithm named Simplex Unscented Kalman Filter(SUKF) is applied into a low-cost aircraft attitude filtering system equipped with less accurate and high noisy sensors. Meanwhile, a comparison on accuracy and calculation cost among EKF, Cubature Kalman filter(CKF) and SUKF is conducted. Experimental results with real flying data show that the SUKF is easy to implement and increases the attitude filtering accuracy to the second order with respect to EKF. Moreover, the SUKF is simple in computation, and the computational cost decreases about 33% compared with that of CKF.

attitude filtering; simplex unscented Kalman filter; extended Kalman filter; cubature Kalman filter; computational cost

1005-6734(2014)05-0629-05

10.13695/j.cnki.12-1222/o3.2014.05.014

V249.32

A

2014-05-09；

2014-09-02

國(guó)家自然科學(xué)基金（61231017）；中央高?；穑╖XH2012D001）

韓萍（1966—），女，博士，教授，從事數(shù)字信號(hào)處理與模式識(shí)別研究。E-mail：hanpingcauc@163.com