基于組合濾波的低成本飛行器姿態解算算法

王守華, 鄧桂輝，紀元法，孫希延

(1.桂林電子科技大學 信息與通信學院，廣西 桂林 541004; 2.認知無線電與信息處理教育部重點實驗室，廣西 桂林 541004)

基于組合濾波的低成本飛行器姿態解算算法

王守華1,2, 鄧桂輝1*，紀元法1,2，孫希延1,2

(1.桂林電子科技大學 信息與通信學院，廣西 桂林 541004; 2.認知無線電與信息處理教育部重點實驗室，廣西 桂林 541004)

(*通信作者電子郵箱674149099@qq.com)

在低成本的飛行器姿態檢測系統中，互補濾波器由于原理簡單、計算量小，而被廣泛應用。針對低成本飛行器在非勻速運動時因為加速度計不能區分重力加速度與運動加速度引起基于互補濾波的姿態解算誤差較大的問題，提出了一種互補與自適應限幅組合濾波的姿態解算算法，并給出了自適應限幅濾波門閥的設計方法。通過融合陀螺儀輸出的角速度與加速度計輸出的加速度獲取限幅濾波的限幅閥值; 然后將歸一化的加速度計輸出增量通過限幅濾波的結果代替原互補濾波的加速度計輸入，提高非勻速運動下姿態解算精度。經實際系統實時性能測試表明，所提算法估計精度高、計算量小，易于在低成本飛行器控制系統中實現。

姿態解算；互補濾波器；限幅濾波；四元數法；自適應

0 引言

近年來，低成本飛行器在物流業、環境監測、地質勘測及軍事偵察等領域的應用越來越廣泛，作為控制低成本飛行器的關鍵技術的姿態解算算法成為當前熱點研究之一[1]。陀螺儀、加速度計和磁場計等低成本慣性測量單元輸出的測量數據是飛行器載體姿態解算的原始數據，但是受到器件自身測量誤差、溫度和各種噪聲等諸多因素的影響，姿態解算往往誤差較大[2-3]。通過信息融合算法將多種傳感器的數據進行融合往往能取長補短，解算出更精確的姿態角[4]。

在多傳感器姿態解算的信息融合方案上，國內外研究者們提出了多種算法和改進策略。賈瑞才[5]、吳濤等[6]提出利用擴展卡爾曼濾波(Extended Kalman Filter, EKF)實現對陀螺儀隨機漂移誤差的補償，提高了姿態解算精度，但忽略了加速度計的高頻干擾問題，及EKF算法固有的線性化、截斷誤差和發散問題。為克服EKF固有的缺陷，Pan等[7]實現基于無跡卡爾曼濾波(Unscented Kalman Filter, UKF)的姿態估算算法，取得了比EKF更高的穩定性、解算精度及收斂速度; 而Tang等[8]表示應用在航天飛行器姿態解算上的容積卡爾曼濾波(Cubature Kalman Filter, CKF)算法估算性能要比UKF好, 但在低成本飛行器中，基于卡爾曼濾波的姿態算法存在計算量大、硬件要求高、難實現等缺點。Euston等[9]、Wu等[10]在低成本無人機上利用互補濾波器(Complementary Filter, CF)實現載體姿態估算，充分體現了CF原理簡單、計算量小、容易實現、收斂穩定等優點，但是未考慮因加速度計不能區分運動加速度與重力加速度而產生的CF算法解算誤差。Wu等[11]、閻世梁等[12]在應用CF解算姿態時通過計算三軸加速度計的輸出的合加速度的大小來判斷載體的運動狀態，然后根據載體運動狀態調節CF的轉換頻率，提高了基于CF的姿態解算精度，但是存在PI參數離散和高動態下互補濾波作用大大降低等問題。孫金秋等[13]設計了PI參數能自適應的CF姿態解算算法，通過合加速度大小來連續調節PI參數，實現連續使姿態估算誤差小于1.47°，但存在對姿態三角無區分地使用同一個PI參數調節的問題，依然有高動態下互補濾波作用大大降低的問題。

本文提出自適應限幅濾波與互補濾波組合濾波的策略，其中自適應限幅濾波能較準確區分三軸重力加速度分量和三軸運動加速度分量，同時還保留了互補濾波器的計算量小、收斂穩定及易于在低成本飛行器中實現的優點，使基于重力加速度輸入的互補濾波算法也能在載體帶有運動加速度時解算出更精確的姿態角。

1 四元數法姿態解算

載體坐標系與導航坐標系的轉動關系可以用四元數標記為:

Q=q0+q1i+q2j+q3k

(1)

四元數確定出b系到n系的坐標變換矩陣為:

(2)

n系與b系基本旋轉對應的坐標變換矩陣記為:

(3)

對比式(2)、(3)可得:

(4)

可見載體的姿態更新實質上是如何計算四元數，記b系中xyz三軸旋轉的角速率向量[ωxωyωz]T為ω，四元數向量[q0q1q2q3]T為q，則更新四元數的微分方程為:

(5)

其中，式(5)中的ω由式(6)求取:

(6)

2 基于互補濾波器的四元數法姿態解算

短時間內，陀螺儀提供高精度的姿態結算數據，而加速度計和磁場計精度較差；長時間內，陀螺儀因漂移的影響，產生積分誤差，姿態精度隨時間增長而下降，但是加速度計和磁場計的測量誤差不會隨時間增加。因此，根據兩者在頻域上的特性的互補，可以采用互補濾波器融合這三種傳感器的數據，提高姿態解算的精度和系統的動態特性[13]。

記姿態角為η=[φθψ]，由三軸加速度計輸出的加速度向量a和磁場計輸出的地磁場強度向量m可以解算出一個帶有高頻噪聲的姿態角η1=η+ΔηH，同時陀螺儀輸出ω通過積分可以輸出一個含低頻噪聲的姿態角η2=η+ΔηL。將η1通過低通濾波器HL(s)=C(s)/(s+C(s))濾除加速度計和磁場計的測量誤差，并將η2通過高通濾波器HH(s)=s/(s+C(s))濾除陀螺儀因漂移的影響產生的積分誤差。那么，最后輸出接近真實值的姿態角:

(7)

取C(s)=kP+kI/s，kP為低通濾波器與高通濾波器的轉接頻率，引入積分環節kI/s是為了補償陀螺儀的漂移誤差[15]。在四元數法姿態解算算法流程中應用互補濾波器時，算法流程為圖1。

圖1 基于互補濾波器的四元數法姿態解算過程Fig. 1 Quaternion attitude calculation process based on complementary filter

由基于互補濾波器的姿態解算過程可見，跟卡爾曼濾波算法相比，互補濾波法有計算量少、容易實現的優點。

但是，圖中的要求加速度計輸出的是重力加速度g在b系的三軸分量時，才能使更新四元數得到高精度解算。實際上，在飛行器動態飛行過程中，往往受到的是重力加速度和運動加速度的合力，而加速計無法區分這二者。這個原因當飛行器作非勻速飛行時，即運動加速度非零時，基于互補濾波器的姿態解算誤差隨運動加速度的誤差增大而增大。

3 自適應限幅濾波與互補濾波組合

給限幅濾波器設置一個閥值，通過限制輸入的幅值可消除無用或者有害幅值。于是可以設計一個自適應限幅濾波器，使其限幅閥值始終跟隨到略大于重力加速度的大小的位置，在飛行器非勻速飛行時最大化地濾除運動加速度。設計的自適應限幅濾波器結構如圖2。

圖2 自適應限幅濾波器的設計方案Fig. 2 Design scheme of adaptive limited filter

(8)

記ρ為設計的限幅濾波器的自適應限幅閥值，ρ的生成規則為:

(9)

若η0表示姿態角任一時刻的角度值，由于低成本飛行器的輸出頻率高，旋轉角速度較小，所以ω·Δt也很小，那么有:

(10)

(11)

4 實驗測試及分析

4.1 實驗平臺

算法設計驗證及分析平臺為Matlab R2011b。用于實驗測試的軟件及裝置如圖3所示。

航姿參考系統和數據采集選用微型航姿參考系統MAHRS 3DM-E10A，該系統通過RS-232串行接口方便連接，提供100 Hz的實時三軸慣性原始數據和參考姿態角。3DM-E10A提供的橫滾角、俯仰角靜態誤差±0.1°，動態誤差±1.0°；航向角靜態誤差±0.5°，動態誤差±2.0°。

如圖3(a)所示，雙軸電動轉臺使用TT-3DM-2E-10 Version 1.2, 具備串行輸出實時角度位置數據、串行輸入控制指令及旋轉角位置動態跟蹤測量與控制等優點，主軸與俯仰軸轉角范圍連續無限、角度位置綜合測量精度為±0.08°、控制到位分辨率為±0.01°及速率范圍為0.1°/s～300°/s，滿足實驗測試算法需求。

圖3(b)為TT-3DM-2E-10配套的轉臺控制Labview軟件。圖3(c)為作者開發的用于通過串口記錄理論姿態角及3DM-E10A系統輸出的9個原始數據的Labview軟件，記錄頻率與3DM-E10A輸出頻率同樣。

圖3 用于實驗測試的軟件及裝置Fig. 3 Software and equipment for test

4.2 靜態及低動態測試

靜態及低動態測試數據采集方法為：通過圖3(b)所示控制轉臺的Labview開發的軟件通過串口控制如圖3(a)所示雙軸轉臺轉動，如圖3(c)所示的Labview開發的軟件通過串口記錄理論姿態角及慣性元件輸出9個原始數據。

利用自適應限幅濾波與互補濾波器組合濾波算法解算的靜態及低動態下載體的姿態角解算及與航姿參考系統的誤差如圖4。

圖4 靜態及低動態測試Fig. 4 Static and low dynamic test

由圖4可見，在靜態狀態下，基于組合濾波算法解算的姿態角誤差在±0.2°內；在低動態狀態下，誤差在±0.7°內。基于典型互補濾波算法的解算結果，在靜態下的誤差與基于組合濾波算法解算的姿態角誤差基本一致，但是低動態下，特別是角速率比較大時，解算最大誤差可達±2°。

靜態及低動態測試表明，在載體靜態下，該算法保留了典型互補濾波算法的優點；在低動態下，在載體角速率較大時，自適應限幅濾波也發揮作用，有效減弱了運動加速度帶來的影響，提高了姿態解算的精度。

4.3 快速滑動測試

為了驗證算法消除運動加速度對互補濾波法姿態解算影響的效果，將3DM-E10A置于一個接近水平面的平面上滑動，并通過串口采集原始數據。其中該實驗用的平面為固定鋼化玻璃且幾乎沒有彈性，經反復測量橫滾角約-0.3°，俯仰角約-0.5°，由于測試不同算法時使用的是同一個滑動過程采集的數據，所以不會對對比實驗產生影響，可視為理論上的0°。

圖5為滑動時，三軸加速度計輸出的重力加速度和運動加速度的合加速度。圖6為基于組合濾波、典型互補濾波及經典卡爾曼濾波這三個姿態解算算法的比較。

圖5 平面快速滑動過程載體的三軸加速度Fig. 5 Three axis accelerations of carrier during fast moving on level surface

圖6 姿態解算算法比較Fig. 6 Comparison of attitude calculation algorithms

結合圖5和圖6可見: 理論上均為0°的橫滾角和俯仰角由于較大的運動加速度的對互補濾波算法解算的影響產生了約6°的誤差，對經典卡爾曼濾波的解算產生了7°的影響，而基于組合濾波基本不受影響。基于互補濾波解算的航向角在停止滑動時也未能達到收斂的穩定值，還需要約200 ms后才能穩定，經典卡爾曼濾波收斂速度較快；基于組合濾波收斂最快。

快速滑動測試表明，在運動加速度突變或較大時，基于自適應限幅濾波與互補濾波器組合的姿態解算算法極大地減少了運動加速度對姿態解算影響，性能優于基于典型的互補濾波算法及經典卡爾曼濾波的姿態解算。

5 結語

本文分析了基于互補濾波器的姿態解算原理和實現方法，指出了運動加速度對姿態解算的影響，并提出了根據陀螺儀輸出和運動狀態的判斷來設計一個具有自適應限幅門閥的限幅濾波器的方法，將三軸加速度計的輸出通過這個限幅濾波器后再用互補濾波算法解算出載體的姿態角。測試表明該算法在勻速或者較高動態狀態下，都能達到較高的解算精度，提高了姿態解算的精度。

References)

[1] 傅忠云, 劉文波, 孫金秋,等. 自適應混合濾波算法在微型飛行器姿態估計中的應用[J]. 傳感技術學報, 2014,27(5):698-703.(FU Z Y, LIU W B, SUN J Q, et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude[J]. Chinese Journal of Sensors and Actuators, 2014,27(5):698-703.)

[2] 金光明, 張國良, 陳林鵬,等. MEMS陀螺儀靜態漂移模型與濾波方法研究[J]. 傳感器與微系統, 2007, 26(11):48-50.(JIN G M, ZHANG G L, CHEN L P, et al. Research on filter method and model of MEMS gyro static drift[J]. Transducer and Microsystem Technologies, 2007, 26(11):48-50.)

[3] DI L, FROMM T, CHEN Y Q. A data fusion system for attitude estimation of low-cost miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012, 65(1):621-635.

[4] 彭孝東, 張鐵民, 李繼宇,等. 基于傳感器校正與融合的農用小型無人機姿態估計算法[J].自動化學報, 2015, 41(4):854-860.(PENG X D, ZHANG T M, LI J Y, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4):854-860.)

[5] 賈瑞才. 基于四元數EKF的低成本MEMS姿態估計算法[J]. 傳感技術學報, 2014, 27(1):90-95.(JIA R C. Attitude estimation algorithm for low cost MEMS based on quaternion EKF[J]. Chinese Journal of Sensors and Actuators, 2014,27(1):90-95.)

[6] 吳濤, 白茹, 朱禮堯,等. 基于卡爾曼濾波的航姿參考系統設計[J]. 傳感技術學報, 2016, 29(4):531-535.(WU T, BAI R, ZHU L Y, et al. Design of AHRS based on Kalman filter[J]. Chinese Journal of Sensors and Actuators, 2016, 29(4):531-535.)

[7] PAN Y, SONG P, LI K, et al. Attitude estimation of miniature unmanned helicopter using unscented Kalman filter[C]// Proceedings of the 2011 International Conference on Transportation, Mechanical, and Electrical Engineering. Piscataway, NJ: IEEE, 2011:1548-1551.

[8] TANG X, WEI J, CHEN K. Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation[C]// Proceedings of the 2012 15th International Conference on International Conference on Information Fusion. Piscataway, NJ: IEEE, 2012:1406-1412.

[9] EUSTON M, COOTE P, MAHONY R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[C]// Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2008:340-345.

[10] WU J, ZHOU Z, CHEN J, et al. Fast complementary filter for attitude estimation using low-cost MARG sensors[J]. IEEE Sensors Journal, 2016, 16(18): 6997-7007.

[11] WU Z Y, ZENG Y J, SHAO H X, et al. Betterment of attitude estimation based on complementary filter applied in drumstick[C]// Proceedings of the 2012 World Congress on Information and Communication Technologies. Piscataway, NJ: IEEE, 2012:1162-1165.

[12] 閻世梁, 王銀玲, 張華. 基于改進互補濾波器的低成本微小飛行器姿態估計方法[J]. 計算機應用, 2013, 33(7):2078-2082.(YAN S L, WANG Y L, ZHANG H. Improved complementary filter for attitude estimation of micro air vehicles using low-cost inertial measurement units[J]. Journal of Computer Applications, 2013, 33(7):2078-2082.)

[13] 孫金秋, 游有鵬, 傅忠云. 基于自適應顯式互補濾波的姿態解算方法[J]. 測控技術, 2015, 34(4):24-27.(SUN J Q, YOU Y P, FU Z Y. Attitude estimation based on adaptive explicit complementary filter[J]. Measurement & Control Technology, 2015, 34(4):24-27.)

[14] 秦永元.慣性導航[M].2版.北京:科學出版社, 2014:244-260.(QIN Y Y. Inertial Navigation[M]. 2nd ed. Beijing: Science Press, 2014:244-260.)

[15] 梁延德, 程敏, 何福本,等. 基于互補濾波器的四旋翼飛行器姿態解算[J]. 傳感器與微系統, 2011, 30(11):56-58.(LIANG Y D, CHENG M, HE F B, et al. Attitude estimation of a quad-rotor aircraft based on complementary filter[J]. Transducer and Microsystem Technologies, 2011, 30(11):56-58.)

This work is supported by the National Natural Science Foundation of China (61362005), the Key Laboratory of Cognitive Radio and Information Processing, Ministry of Education (CRKL150113), the Innovation Project of Graduate Education in Guilin University of Electronic Technology (2016YJCX90).

WANG Shouhua, born in 1975, M. S., associate professor. His research interests include information processing, navigation and positioning.

DENG Guihui, born in 1989, M. S. candidate. His reserch interests include integrated navigation, indoor naigation.

JI Yuanfa, born in 1975, Ph. D, professor. His research interests include satelite communication, satellite navigation and digital signal processing.

SUN Xiyan, born in 1973, Ph. D, professor. His research interests include satellite navigation, electronic countermeasure.

Attitude calculation algorithm of low-cost aircraft based on combined filter

WANG Shouhua1,2, DENG Guihui1*, JI Yuanfa1,2, SUN Xiyan1,2

(1.CollegeofInformationandCommunication,GuilinUniversityofElectronicTechnology,GuilinGuangxi541004,China;2.KeyLaboratoryofCognitiveRadioandInformationProcessingofMinistryofEducation,GuilinGuangxi541004,China)

In the low-cost aircraft attitude detection system, the complementary filter was widely used because of simple principle and low computational complexity. Aiming at the problem that the accelerometer can not distinguish between the acceleration of gravity and motion may cause attitude calculation error by complementary filtering, an attitude calculation algorithm of complementary and adaptive limited filter was proposed for low cost aircraft, and the design method of adaptive limited filter gate valve was given. The angular velocity output by a gyroscope and the acceleration output by an accelerometer were fused to obtain the gate valve of the limited filter, and then the normalized output increment of the accelerometer by limiting filter replaced the accelerometer input of the original complementary filter. The accuracy of attitude determination under uniform motion was improved. The actual system test shows that the proposed algorithm has high accuracy and low cost, and is easy to be realized in low cost aircraft control system.

attitude calculation; complementary filter; limiting filtering; quaternion method; adaptive

2016-10-28;

2016-12-13。 基金項目:國家自然科學基金資助項目(61362005);“認知無線電與信息處理”教育部重點實驗室2015年主任基金資助項目(CRKL150113); 桂林電子科技大學研究生教育創新計劃項目(2016YJCX90)。

王守華(1975—),男,山東濱州人,副教授,碩士,主要研究方向:信息處理、導航定位; 鄧桂輝(1989—),男,廣東茂名人,碩士研究生,主要研究方向:組合導航、室內導航; 紀元法(1975—)，男，山東莘縣人，教授，博士，主要研究方向:衛星通信、衛星導航、數字信號處理; 孫希延(1973—)，女，山東濰坊人，研究員，博士，主要研究方向:衛星導航、電子對抗。

1001-9081(2017)05-1507-05

10.11772/j.issn.1001-9081.2017.05.1507

TP391.9

A