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

自適應(yīng)混合濾波算法在微型飛行器姿態(tài)估計中的應(yīng)用*

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

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

姿態(tài)估計;四元數(shù);梯度下降法;互補濾波;自適應(yīng)混合濾波算法

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

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

1 算法思想

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

2 飛行姿態(tài)的四元數(shù)描述

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

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

則參考系和機體系之間的轉(zhuǎn)換關(guān)系可以表示為:

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

俯仰角:

橫滾角:

偏航角:

因此，當(dāng)姿態(tài)四元數(shù)確定后，即可獲得對應(yīng)的旋轉(zhuǎn)矩陣，進而得到四元數(shù)表示的參考坐標系下的姿態(tài)角。

3 混合濾波算法

3.1 利用陀螺儀獲取姿態(tài)

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

即:

其中，SE˙q為SEq的導(dǎo)數(shù)，?為向量叉乘運算。

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

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

3.2 梯度下降法

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

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

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

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

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

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

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

迭代公式為:

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

3.3 混合濾波器融合算法

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

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

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

其中，λ為可調(diào)參數(shù)。

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

式中需要調(diào)整的參數(shù)有兩個α和λ，且兩參數(shù)之間無明顯直接聯(lián)系，應(yīng)用時難以調(diào)節(jié)?？紤]到系統(tǒng)實際要求SEq▽(k)的收斂率大于或等于載體的實際變化率，則應(yīng)取較大的梯度方向的搜索步長μk，相應(yīng)的α應(yīng)取值很大，式(11)和式(14)可簡化為:

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

式中:

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

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

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

圖1 自適應(yīng)混合濾波算法總體框圖

4 實驗驗證及結(jié)果

4.1 測試平臺

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

圖2 上位機監(jiān)控界面

4.2 試驗結(jié)果及分析

靜態(tài)試驗:

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

正常飛行試驗:

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

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

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

表1 運動加速度濾除誤差

圖3 靜止時姿態(tài)角

圖4 正常飛行時姿態(tài)角

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

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

圖6 姿態(tài)快速變化時各算法估算俯仰角曲線

5 結(jié)論

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

［1］高鐘毓.慣性導(dǎo)航系統(tǒng)技術(shù)［M］.北京:清華大學(xué)出版社，2012.

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

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

［4］葉锃峰，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法［J］.傳感技術(shù)學(xué)報，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］.傳感技術(shù)學(xué)報，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，F(xiàn)OAM and Low-Cost MEMS Sensors［J］.Sensors，2012，12(7):9566-9585.

［9］朱豐超，姚敏立，賈維敏.基于低成本陀螺和傾角儀的姿態(tài)估計［J］.宇航學(xué)報，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］程國采.四元數(shù)法及其應(yīng)用［M］.長沙:國防科技大學(xué)出版社，1991.

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

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

劉文波(1968-)，女，教授，博導(dǎo)，主要從事信號處理、計算機測控技術(shù)研究，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

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

2014-02-18

2014-04-29