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

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

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

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

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

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

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

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

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

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

1 單向無跡卡爾曼濾波

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

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

1）時間更新

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

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

2）量測更新

③ 計算卡爾曼增益

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

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

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

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

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

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

本文用四元數(shù)來描述飛機(jī)姿態(tài)，姿態(tài)解算時需要角速度。此外為了提高姿態(tài)濾波精度，用陀螺儀的隨機(jī)游走在線補(bǔ)償陀螺儀測量誤差，所以狀態(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)觀測方程

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

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

式中，

3 實驗結(jié)果與分析

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

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

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

為便于比較，將EKF和CKF濾波誤差也置于圖2～4中。由圖可知，SUKF濾波誤差小于EKF，較CKF也有改善，波動性小于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%的計算量，與EKF計算量基本相當(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ù)據(jù)進(jìn)行驗證，并給出了各方法計算量的統(tǒng)計對比。實驗表明：相對于EKF，SUKF在姿態(tài)濾波過程中無需求導(dǎo)計算繁瑣的Jacobian矩陣，實現(xiàn)容易，且使濾波精度得到提高。相對于CKF，SUKF有效地提高了運(yùn)算效率和實時性，是一種高效的姿態(tài)濾波方法。

（

）：

[1] 付夢印，鄧志紅，張繼偉. 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)角初始對準(zhǔn)[J]. 中國慣性技術(shù)學(xué)報，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雙向濾波的單站無源定位算法[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]. 中國慣性技術(shù)學(xué)報，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)切換無跡卡爾曼濾波器估計的飛行器姿態(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

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

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