自適應混合濾波算法在微型飛行器姿態估計中的應用*

傅忠云，劉文波，孫金秋，徐貴力(.南京航空航天大學金城學院，江蘇南京56;.南京航空航天大學自動化學院，江蘇南京006)

自適應混合濾波算法在微型飛行器姿態估計中的應用*

傅忠云1*，劉文波2，孫金秋1，徐貴力2
(1.南京航空航天大學金城學院，江蘇南京211156;2.南京航空航天大學自動化學院，江蘇南京210016)

針對低成本慣性測量單元(IMU)存在漂移和噪聲干擾等問題，提出了一種具有自適應參數調節的混合濾波算法。采用四元數法進行系統模型的描述，用梯度下降法對加速度計測得的數據進行處理，再通過互補濾波器將其與陀螺儀測量值進行融合，形成混合濾波算法。同時，考慮到飛行姿態的復雜性，進行參數λ的自適應調節，因而改進后的混合濾波算法，能保證各種飛行姿態變化情況下實時姿態的最優估算。實際系統在線實時性能測試表明，提出的算法簡單，估計精度高，易于在嵌入式系統中實現，具有較高推廣應用價值。

姿態估計;四元數;梯度下降法;互補濾波;自適應混合濾波算法

隨著微小型無人飛行器在地質勘測，災害監測及軍事偵察等領域的廣泛應用，準確的飛行姿態測量顯得越來越重要。然而，由于微型無人機具有自身有效載荷小等缺點，使得一些高精度姿態測量裝置無法應用。由陀螺儀和加速度計組成的低成本慣性測量單元(IMU)，具有體積小，重量輕，功耗低，系統實現簡單，性價比高等特點，在很多領域得到了廣泛的應用。而低成本陀螺儀存在積分累計誤差，加速度計在動態姿態測量時易受高頻噪聲影響，測量精度較低，如何利用好的濾波算法對兩者進行融合濾波，進而獲得最優的姿態估計，成為解決此類問題的關鍵［1-3］。

針對以上問題，國內外很多學者進行了大量的研究工作。文獻［4］中提出了利用卡爾曼濾波對陀螺儀隨機漂移誤差的補償，但沒有考慮加速度計的高頻干擾問題。文獻［5-10］等分別利用擴展卡爾曼濾波，無跡卡爾曼濾波，粒子濾波等濾波算法，使姿態估計精度有了一定程度的提高。但是卡爾曼濾波算法計算量較大，難以應用于嵌入式微控制器中。文獻［11］利用顯性互補濾波算法進行了固定旋翼無人機的姿態估計，避免了傳統互補濾波器對姿態估計進行重構的缺點。文獻［12］提出了一種梯度下降法對加速度計和磁強計的輸出數據進行處理，利用互補濾波算法，將其和陀螺儀的積分結果進行融合，實現了在線實時姿態估計，但是由于融合時參數為定值，因此在飛行姿態變化劇烈等情況發生時估計誤差較大，算法適應性差。本文采用嵌入式微處理器和低成本的IMU，利用四元數進行系統和姿態描述，提出一種基于自適應參數調節的梯度下降法和互補濾波的混合濾波算法，實現了高精度微型飛行器全姿態估計。

1 算法思想

本文設計的混合濾波算法總體思想為:首先，利用陀螺儀測得的角速度和四元數微分方程式得出角速度微分四元數;然后運用梯度下降法對加速度計測得數據進行處理，得到最小的誤差四元數的微分值;再將兩者進行互補融合，盡可能的減小陀螺儀的漂移誤差和加速度計的高頻干擾引起的姿態估計誤差;最后對互補濾波后的姿態微分四元數進行積分，估算出最優的姿態值。同時，利用最優重力估計結果，對梯度下降法中可調參數λ的取值進行了優化。該混合濾波算法運算過程不涉及三角和反三角函數運算，全部使用簡單的四元數加減乘除運算，運算效率高，計算工作量很小。

2 飛行姿態的四元數描述

四元數是英國數學家Hamilton W R在1843年首先提出的［13］。四元數是一個四維的復合數，可以用來表示剛體旋轉或者三維空間的坐標系。隨著剛體運動力學的發展，人們發現采用單位四元數來描述剛體的旋轉運動十分方便，而且可以避免歐拉法的“奇點”現象，以及方向余弦法計算量大、實時性差等問題。近年來，隨著慣性導航技術的發展，四元數在捷聯式慣性導航系統中有了較為廣泛的應用。

假設參考坐標系為E(xE，yE，zE)，機體坐標系為S(xS，yS，zS)，參考系到機體系的姿態旋轉四元數為SEq=［q1，q2，q3，q4］。由四元數SEq表示的旋轉可以用三維坐標系中的旋轉矩陣SER(q)代替［2］:

則參考系和機體系之間的轉換關系可以表示為:

在空間三維坐標系下，無人飛行器的姿態通常由θ、γ和ψ描述。其中，θ為繞xE軸的旋轉角度，稱為俯仰角;γ為繞yE軸的旋轉角度，稱為橫滾角; ψ為繞zE軸的旋轉角度，稱為偏航角。根據四元數代數學和歐拉旋轉矩陣，可得姿態角［2］:

俯仰角:

橫滾角:

偏航角:

因此，當姿態四元數確定后，即可獲得對應的旋轉矩陣，進而得到四元數表示的參考坐標系下的姿態角。

3 混合濾波算法

3.1 利用陀螺儀獲取姿態

在載體坐標系下，一個三軸陀螺儀可以測得繞x，y，z三個軸的角速度，分別用ωx、ωy和ωz表示，將這3個量和0一起定義為陀螺儀角速度四元數向量Sω=［0ωxωyωz］。對參考系到機體系的姿態旋轉四元數SEq進行標準化得到SE^q，則旋轉四元數的微分方程式為:

即:

其中，SE˙q為SEq的導數，?為向量叉乘運算。

實際計算時，將式(6)離散化后，可利用式(7)進行迭代計算:

其中，Δt為系統采樣周期。利用陀螺儀測得三軸加速度及式(6)和式(7)，求取更新的旋轉四元數，再利用式(3)～式(5)即可解算出實時姿態角。

3.2 梯度下降法

梯度下降法，就是利用函數對應的負梯度方向來更新每次迭代的新的搜索方向，使得每次迭代能使待優化的目標函數逐步減小，通常用來求解函數的最小值。因此目標函數的確定是梯度下降法的前提，下面簡述本文研究問題的目標函數的確定過程，及梯度下降法在姿態估算中的應用原理。

根據四元數坐標變換，可以將地球重力場的參考方向通過四元數的叉乘，轉換到載體坐標系下進行表示，因為向量的四元數表示法有4個元素，所以，將0作為第1個元素放入向量的已有的3個元素之前。則在參考系下，地球重力場中地球的重力加速度始終垂直向下平行于該坐標系的z軸，且為一常值，因此可以得到標準化的重力場的絕對參考方向E^g=［0 0 0 1］。加速度計測得的重力加速度在載體坐標系中的各軸向分量分別為ax、ay和az，則加速度計四元數向量Sa=［0 axayaz］，標準化為S^a=［0^ax^ay^az］。

則重力場的參考方向在載體坐標系下可表示為:

式中，SE^q*=［q1-q2-q3-q4］是SE^q共軛四元數。SE^q*和SE^q分別是SEq*和SEq標準化形式。將E^g=［0

0 0 1］代入式(8)，即可得載體坐標系下的重力加速度各軸向分量值標準化值S^g=［0^gx^gy^gz］。將其與加速度傳感器測得的加速度向量相減，得:

當該函數的2范數值‖fg(ES^q，S^a)‖=0時，測量值與實際值相同，即為正確的姿態。但是由于實際系統測量，坐標對準及數值運算和迭代時都會產生誤差，因此該值不一定為0。本文應用梯度下降法求取最小值，就可以得到載體的最優姿態。

通常用目標函數的雅克比矩陣求取其梯度，其雅克比矩陣為:

迭代公式為:

式中，SEq▽(k)是梯度下降法求解出的姿態四元數，SEqest(k-1)為迭代上一次估計值，▽fg(·)為目標函數fg(·)的梯度值，μk稱為梯度方向的搜索步長，其值可由μk=α‖SEq˙ω(k)‖Δt確定，α與加速度計的噪聲有關，一般取α＞1。這樣就可以采用梯度下降法根據式(11)進行迭代求解SEq▽(k)。實際應用中，每次采樣只需迭代一次即可，姿態的收斂速度由μk決定。同樣利用式(3)～式(5)可解算出實時姿態角。

3.3 混合濾波器融合算法

上述3.1和3.2節中分別闡述了利用兩種單獨傳感器解算出姿態角方法。但是，利用單獨陀螺儀積分進行姿態角估計，受積分漂移，低頻噪聲等影響將不可避免地導致其估計值隨時間發散，誤差較大。而由于受到高頻干擾的影響，加速度計在應用梯度下降法時難以收斂到最優解。下面將利用互補濾波器將兩者進行融合，充分發揮兩種傳感器的優點，使陀螺儀估算的SEqω(k)的發散率和梯度下降法的SEq▽(k)的收斂率相等，進行互補融合，實現最優估計。

基本互補濾波算法的融合公式為:

式中:SEq▽(k)是梯度下降法求解出的姿態四元數，SEqω(k)為陀螺儀求解出的姿態四元數，ξ和1-ξ分別為兩種單獨姿態估計的權值。ξ的取值要使SEq▽(k)的收斂加權率和積分漂移產生的SEqω(k)發散的加權率相等，即:

其中，λ為可調參數。

將式μk=α‖SE˙qω(k)‖Δt及式(14)代入式(13)可得:

式中需要調整的參數有兩個α和λ，且兩參數之間無明顯直接聯系，應用時難以調節。考慮到系統實際要求SEq▽(k)的收斂率大于或等于載體的實際變化率，則應取較大的梯度方向的搜索步長μk，相應的α應取值很大，式(11)和式(14)可簡化為:

同時，由于μk很大，ξ也可以近似為0，將式(7)、式(16)和式(17)代入式(13)，則有:

式中:

簡化后的融合公式只有一個可變參數λ，易于調節，從式(18)和式(19)也可以看出，該融合算法是將陀螺儀的角速度四元數和由梯度下降法得到的加速度解算出的角速度先進行了融合，然后進行積分得出姿態角。

通常梯度下降法的參數λ取為一定值(根據具體系統試驗結果確定)，但是當系統姿態變化劇烈時，將會出現明顯的過沖或震蕩現象。本文將該參數的取值增加了自適應調節能力，進行動態的調整。例如:當系統姿態不變向前沖或作水平滑動時，由于運動加速度將被引入加速度計測量值中，此時，加速度計測量值與上次估計結果的誤差將很大，因此必須將參數λ調小;而當系統姿態快速變化時，則可能超出陀螺儀量程，造成測量誤差較大，而此時加速度計測量值較為準確，則λ取較大值，因此對λ做了如下優化:

式中，|·|為取絕對值運算，ea(k-1)為上一個采樣周期加速度計誤差值。誤差取e=［e0exeyez］T=S^g?S^a。λ取值上限為2，可以保證陀螺儀測量值的融入。另外，當λ＜0.2，則取下限值0.2，以保證加速度計的測量值對系統輸出的作用。而且λ計算公式中融入了低通濾波，可減小參數變化速率，改善系統性能。改進后的自適應混合濾波AHF(A-daptive Hybrid Filter)算法總體框圖如圖1所示。

圖1 自適應混合濾波算法總體框圖

4 實驗驗證及結果

4.1 測試平臺

系統傳感器選用美國體感技術公司InvenSense的MPU-6050。它是全球首例整合三軸加速度計、三軸陀螺儀的MEMS傳感器，有效避免了陀螺儀與加速度計的軸間差問題。雖然該模塊本身具有“以數字輸出6軸或9軸的旋轉矩陣、四元數以及歐拉角格式的融合演算數據”功能，但是，由于代碼封閉，具體濾波算法未知，參數不可調節，使用不方便。經測試，與本文提出算法比較，模塊自帶算法具有運算速度慢，加速度引入誤差大等缺點。因此，該模塊自帶濾波算法未能得到廣泛應用。本次實驗平臺處理器選用意法半導體生產的使用ARM-CortexM3內核的STM32F103C8 32位微處理器，通過SPI協議將數據使用2.4 GHz無線數據傳輸模塊發送到上位機進行觀測分析。2 Mbit/s的空中速率結合電腦的USB 1.1全速接口，使得上位機能夠以100 Hz的頻率實時繪制并導出數據波形。系統測試平臺為自制四旋翼微型飛行器。圖2為上位機監控界面，可直觀的觀測飛行姿態，方便程序的調試，同時添加了數據導出功能，可實時將數據導入到Excel表格中，方便數據分析。

圖2 上位機監控界面

4.2 試驗結果及分析

靜態試驗:

為了測試本算法系統性能，首先進行了靜態試驗，因為系統沒有磁強計，因此只進行了俯仰角和橫滾角的靜態數據測試及比對。同時，因為靜止時加速度計測得的角度非常準確，所以，將本文提出參數自適應調節的混合濾波(AHF)算法估算出的姿態角和加速度計(ACC)測量值進行比較。如圖3所示為靜止時俯仰角和橫滾角加速度計測量值及估算值比較圖。從圖3可以看出，本算法靜態誤差非常小，系統進行了加速度計和陀螺儀零偏校準，經測試靜止放置5分鐘測得的靜態誤差絕對值的平均值為0.05°，有效地避免了陀螺儀的漂移問題。

正常飛行試驗:

正常飛行姿態仍然進行俯仰角和橫滾角的測量及估算，將本文算法估算角度值和高精度光纖陀螺儀及加速度測量值進行比較。圖4所示為俯仰角和橫滾角曲線圖，由圖可見該算法估計值與光纖陀螺儀FOG(Fibre-Optic Gyroscope)測量值曲線吻合度很高，曲線平滑，能有效濾除加速度計的毛刺和突變現象。將本文算法估算值與光纖陀螺儀測算結果的差值的絕對值作為估算誤差，俯仰角估算的誤差平均值為0.523°，橫滾角估算的誤差平均值為0.498°。以上測試結果表明該系統在靜止和正常飛行狀態時的姿態角估計精度完全滿足實際要求。

為測試本算法的性能，進行了水平滑動和姿態快速變化兩種極端飛行姿態試驗，以俯仰角為例進行測試，并將本文提出的自適應參數調節混合濾波算法和參數不變混合濾波算法以及幾種常用的濾波算法進行了比較。

如圖5所示為飛行器做水平滑動運動時各算法對運動加速度的濾除效果圖。水平滑動時俯仰角的理論真值為0°，則對幾種算法估算值的絕對值取平均值，即可得運動加速度濾除誤差，結果如表1所示。可見改進后的混合濾波算法在運動加速度特性濾除時表現出了良好的性能。

表1 運動加速度濾除誤差

圖3 靜止時姿態角

圖4 正常飛行時姿態角

圖5 水平滑動時加速度濾除效果圖

當控制器姿態角度變化過快時，加速度計則會發生突變，同時也可能超出陀螺儀量程，造成陀螺儀測量失效，積分誤差大。如圖6所示，通過幾種算法比較可見，本文算法系統快速性好，無超調，無穩態誤差;卡爾曼濾波(Kalman)算法［14］對未精確建模系統，收斂速度慢，同時還存在超調;顯性互補濾波(ECF)和參數不變混合濾波(HF)算法恢復速度都較慢。

圖6 姿態快速變化時各算法估算俯仰角曲線

5 結論

本文將梯度下降法和互補濾波器結合起來，并對參數λ的選擇進行了優化和改進，形成自適應參數可調的混合濾波算法，通過實時系統在線測試，該算法能對飛行器各種姿態進行高精度姿態角度估算。同時，通過和其他常用濾波算法進行比較，該算法具有姿態估算精度高，快速性好，無超調及靜差等優點，完全能滿足實際工程需求。

［1］高鐘毓.慣性導航系統技術［M］.北京:清華大學出版社，2012.

［2］曹娟娟，房建成，盛蔚，等.低成本多傳感器組合導航系統在小型無人機自主飛行中的研究與應用［J］.航空學報，2009，30 (10):1923-1929.

［3］吳永亮，王田苗，梁建宏.微小型無人機三軸磁強計現場誤差校正方法［J］.航空學報，2011，32(2):330-336.

［4］葉锃峰，馮恩信.基于四元數和卡爾曼濾波的兩輪車姿態穩定方法［J］.傳感技術學報，2012，25(4):524-528.

［5］Wu Y L，Wang T M，Liang J H，etal.Attitude Estimation for Small Helicopter Using Extended Kalman Filter［C］∥Proceedings of the 2008 IEEE Conference on Robotics，Automation and Mechatronics. Piscataway:IEEE Press，2008:577-581.

［6］朱文杰，王廣龍，高鳳岐，等.基于MIMU和磁強計的在線實時定姿方法［J］.傳感技術學報，2013，26(4):536-540.

［7］Tarhan M，Altug E.EKF Based Attitude Estimation and Stabilization of a Quadrotor UAV Using Vanishing Points in Catadioptric Images［J］.Journal of Intelligent and Robotic Systems，2011，62(3-4): 587-607.

［8］Marina H G，Espinosa F，Santos C.Adaptive UAV Attitude Estimation Employing Unscented Kalman Filter，FOAM and Low-Cost MEMS Sensors［J］.Sensors，2012，12(7):9566-9585.

［9］朱豐超，姚敏立，賈維敏.基于低成本陀螺和傾角儀的姿態估計［J］.宇航學報，2011，32(8):1728-1733.

［10］Won S P，Melek W W，Golnaraghi F.A Kalman/Particle Filter Based Position and Orientation Estimation Method Using a Position Sensor/Inertial Measurement Unit Hybrid System［J］.IEEE Transactions on Industrial Electronics，2010，57(5):1787-1798.

［11］Euston M，Coote P，Mahony R，et al.A Complementary Filter for Attitude Estimation ofa Fixed-Wing UAV［C］∥Proceedings ofthe 2008 IEEE/RSJ International Conference on Robots and System. Piscataway:IEEE Press，2008:340-345.

［12］Sebastian O H，Madgwick，Andrew J L，et al.Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm［C］∥IEEE International Conference on Rehabilitation Robotics，2011.

［13］程國采.四元數法及其應用［M］.長沙:國防科技大學出版社，1991.

［14］劉興川，張盛，李麗哲，等.基于四元數的MARG傳感器姿態測量算法［J］.清華大學學報(自然科學版)，2012，52(5):627-631.

傅忠云(1980-)，女，碩士，講師，主要從事測控技術及嵌入式系統的教學及應用研究，358328647@qq.com;

劉文波(1968-)，女，教授，博導，主要從事信號處理、計算機測控技術研究，wenboliu@nuaa.edu.cn。

Application of Adaptive Hybrid Filter Algorithm in the Estimation of the Micro Air Vehicle Attitude*

FU Zhongyun1*，LIU Wenbo2，SUN Jinqiu1，2，XU Guili2

(1.Nanhang Jincheng College，Nanjing 211156，China; 2.College of Automation Engineering，University of Aeronautics and Astronautics，Nanjing 210016，China)

Concerning the drift and noise interference of low cost inertial measurement unit，a hybrid filtering algorithm with adaptive adjustment of parameters was proposed.With the quaternion for describing the attitudes，the accelerometer data is processed using gradient descent algorithm.And then the results are fused with Gyro measurements through the complementary filter，which is called the mixed filter algorithm.At the same time，considering the complexity of flight attitude，the parameters can be adaptively adjusted.So the improved hybrid filter algorithm can guarantee the optimal attitude estimation in real time for various flight attitudes.The online test results of the realtime system show that the proposed algorithm simple to realize and has high estimation accuracy.It is especially suitable for implementation on embedded hardware which has high application value.

attitude estimation;quaternion;gradient descent algorithm;complementary filter;adaptive hybrid filter algorithm

V249.3

A

1004－1699(2014)05－0698－06

10.3969/j.issn.1004－1699.2014.05.024

項目來源:國家自然科學基金項目(60974105);航空科學基金項目(20100152003)

2014-02-18

2014-04-29