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

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

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

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

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

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

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

1 算法思想

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

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

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

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

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

在空間三維坐標(biāo)系下，無(wú)人飛行器的姿態(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ù)確定后，即可獲得對(duì)應(yīng)的旋轉(zhuǎn)矩陣，進(jìn)而得到四元數(shù)表示的參考坐標(biāo)系下的姿態(tài)角。

3 混合濾波算法

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

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

即:

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

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

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

3.2 梯度下降法

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

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

則重力場(chǎng)的參考方向在載體坐標(biāo)系下可表示為:

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

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

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

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

迭代公式為:

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

3.3 混合濾波器融合算法

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

基本互補(bǔ)濾波算法的融合公式為:

式中:SEq▽(k)是梯度下降法求解出的姿態(tài)四元數(shù)，SEqω(k)為陀螺儀求解出的姿態(tài)四元數(shù)，ξ和1-ξ分別為兩種單獨(dú)姿態(tài)估計(jì)的權(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ù)有兩個(gè)α和λ，且兩參數(shù)之間無(wú)明顯直接聯(lián)系，應(yīng)用時(shí)難以調(diào)節(jié)?？紤]到系統(tǒng)實(shí)際要求SEq▽(k)的收斂率大于或等于載體的實(shí)際變化率，則應(yīng)取較大的梯度方向的搜索步長(zhǎng)μk，相應(yīng)的α應(yīng)取值很大，式(11)和式(14)可簡(jiǎn)化為:

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

式中:

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

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

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

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

4 實(shí)驗(yàn)驗(yàn)證及結(jié)果

4.1 測(cè)試平臺(tái)

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

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

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

靜態(tài)試驗(yàn):

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

正常飛行試驗(yàn):

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

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

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

表1 運(yùn)動(dòng)加速度濾除誤差

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

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

圖5 水平滑動(dòng)時(shí)加速度濾除效果圖

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

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

5 結(jié)論

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

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

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

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

［4］葉锃峰，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法［J］.傳感技術(shù)學(xué)報(bào)，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和磁強(qiáng)計(jì)的在線實(shí)時(shí)定姿方法［J］.傳感技術(shù)學(xué)報(bào)，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ì)［J］.宇航學(xué)報(bào)，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］程國(guó)采.四元數(shù)法及其應(yīng)用［M］.長(zhǎng)沙:國(guó)防科技大學(xué)出版社，1991.

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

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

劉文波(1968-)，女，教授，博導(dǎo)，主要從事信號(hào)處理、計(jì)算機(jī)測(cè)控技術(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

項(xiàng)目來(lái)源:國(guó)家自然科學(xué)基金項(xiàng)目(60974105);航空科學(xué)基金項(xiàng)目(20100152003)

2014-02-18

2014-04-29