自適應(yīng)顯式互補(bǔ)濾波在六旋翼飛行器中的應(yīng)用*

劉 洲， 單修洋， 譚 芳

(中南大學(xué) 機(jī)電工程學(xué)院，湖南 長(zhǎng)沙 410083)

自適應(yīng)顯式互補(bǔ)濾波在六旋翼飛行器中的應(yīng)用*

劉 洲， 單修洋， 譚 芳

(中南大學(xué) 機(jī)電工程學(xué)院，湖南 長(zhǎng)沙 410083)

針對(duì)六旋翼飛行器的慣性傳感器在測(cè)量過程中存在漂移的問題，提出了基于自適應(yīng)顯式互補(bǔ)濾波的姿態(tài)解算算法，并對(duì)該算法的原理和穩(wěn)定性進(jìn)行分析；搭建了六旋翼飛行器的實(shí)驗(yàn)測(cè)試裝置，并進(jìn)行機(jī)體的動(dòng)、靜態(tài)測(cè)試實(shí)驗(yàn)。在實(shí)驗(yàn)中，對(duì)自適應(yīng)顯式互補(bǔ)濾波算法、顯式互補(bǔ)濾波算法以及陀螺儀傳感器測(cè)量方法進(jìn)行比較。實(shí)驗(yàn)結(jié)果表明：自適應(yīng)顯式互補(bǔ)濾波算法能夠?qū)崿F(xiàn)慣性傳感器的解算姿態(tài)角誤差收斂，且動(dòng)態(tài)誤差最小。

自適應(yīng)顯式互補(bǔ)濾波； 六旋翼飛行器； 姿態(tài)解算； 慣性傳感器； 陀螺儀

0 引 言

六旋翼飛行器慣性測(cè)量單元IMU使用的是微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)類型傳感器。該類型傳感器中陀螺儀能夠測(cè)量機(jī)體旋轉(zhuǎn)角速度，且動(dòng)態(tài)跟蹤性能好。但由于測(cè)量角度是通過角速度積分得到，且陀螺儀本身存在漂移，隨時(shí)間的積累，角度值存在很大誤差。因此，需采用信息融合策略將多種傳感器的數(shù)據(jù)進(jìn)行處理，以提高姿態(tài)解算的精度。國(guó)內(nèi)外學(xué)者提出了一系列的算法如卡爾曼濾波[1～3]、粒子濾波[4,5]和互補(bǔ)濾波[6,7]。經(jīng)典互補(bǔ)濾波在頻域角度對(duì)多個(gè)傳感器數(shù)據(jù)進(jìn)行融合，消除噪聲干擾。該算法計(jì)算量小，相比粒子濾波不需要計(jì)算系統(tǒng)狀態(tài)變量的概率密度[4,5]；相比卡爾曼濾波不需要計(jì)算噪聲序列的方差矩陣[6]。Mahony提出了一種顯式的非線性互補(bǔ)濾波(explicit complementary filtering，ECF)并應(yīng)用于垂直起落飛行器中[8,9]；在ECF的基礎(chǔ)上，許多學(xué)者對(duì)ECF中的增益進(jìn)行在線調(diào)整[9～12]。本文針對(duì)陀螺儀存在漂移及六旋翼飛行器機(jī)體姿態(tài)變化劇烈(如雜技動(dòng)作)時(shí)，加速度計(jì)修正滯后的問題，提出了自適應(yīng)顯式互補(bǔ)濾波算法(AECF)。該算法相比ECF可根據(jù)機(jī)體角速度實(shí)時(shí)動(dòng)態(tài)調(diào)節(jié)補(bǔ)償系數(shù)，從而加快速度計(jì)對(duì)陀螺儀的修正。實(shí)驗(yàn)結(jié)果表明：AECF融合算法能更好減少系統(tǒng)動(dòng)態(tài)誤差，滿足六旋翼飛行器的姿態(tài)解算要求。

1 姿態(tài)解算原理

1.1 坐標(biāo)系定義

六旋翼飛行器的姿態(tài)描述需要定義導(dǎo)航坐標(biāo)系(n系)和機(jī)體坐標(biāo)系(b系)。如圖1所示，n系采用東北天的原則選擇坐標(biāo)軸(X，Y，Z)的正方向;b系的坐標(biāo)軸為x，y，z。飛行器的姿態(tài)角度在n系中分別用橫滾角φ、俯仰角θ和偏航角ψ描述。

圖1 六旋翼飛行器的坐標(biāo)系

捷聯(lián)慣性導(dǎo)航系統(tǒng)將慣性傳感器直接固定在飛行器上，慣性傳感器測(cè)得的數(shù)據(jù)基于b系，所以姿態(tài)解算結(jié)果的坐標(biāo)需從b系轉(zhuǎn)換到n系。轉(zhuǎn)換過程如式(1)[13]

(1)

(2)

1.2 姿態(tài)更新

根據(jù)機(jī)體角速度與地理坐標(biāo)下的姿態(tài)之間的關(guān)系[14]，姿態(tài)更新表達(dá)式如下

(3)

式中Ωb為測(cè)量的機(jī)體角速度。設(shè)固定在載體上的陀螺儀三軸輸出角速度為ωb,則

(4)

(5)

2 基于AECF的融合算法

自適應(yīng)顯式互補(bǔ)濾波姿態(tài)算法原理如圖2所示。

圖2 六旋翼姿態(tài)解算算法框圖

AECF的姿態(tài)觀測(cè)狀態(tài)方程如下

(6)

2.1AECF穩(wěn)定性驗(yàn)證

根據(jù)系統(tǒng)誤差定義Lyapunov函數(shù)V[13]為

(7)

(8)

(9)

2.2 參數(shù)整定

圖2中參數(shù)整定模塊的KP，KI取值規(guī)則如下

(10)

式中 ωmax為陀螺儀的最大量程；ωc為陀螺儀的截止角速度(ωc<ωmax)；KP0為初始補(bǔ)償系數(shù)；KP1為臨界補(bǔ)償系數(shù)。

KP0根據(jù)陀螺儀的ωc以及陀螺儀和加速度計(jì)動(dòng)態(tài)響應(yīng)特性進(jìn)行選取。當(dāng)機(jī)體角速度低于陀螺儀的截至頻率ωc時(shí)，陀螺儀的測(cè)量不滿足信號(hào)不失真的條件，所以，選擇恒定的KP0。機(jī)體角速度越大，相應(yīng)地提高KP使得加速度計(jì)更快地修正陀螺儀的偏差。然而，KP過大會(huì)導(dǎo)致高頻濾波效果差，信號(hào)毛刺增加。當(dāng)機(jī)體角速度超過陀螺儀的量程ωmax時(shí)，KP取最大值，即KP1。積分系數(shù)KI的加入可以消除陀螺儀漂移常值誤差。為了保證實(shí)時(shí)性，選擇KI為常值。文獻(xiàn)[8]給出了SO(3)下ECF算法的相關(guān)理論證明，文獻(xiàn)[10]和文獻(xiàn)[14]給出了AECF算法中KP，KI的整定規(guī)則的證明。

3 實(shí)驗(yàn)測(cè)試

3.1 實(shí)驗(yàn)平臺(tái)搭建

針對(duì)軸距為550mm的六旋翼飛行器搭建系統(tǒng)實(shí)驗(yàn)平臺(tái)。選擇意法公司生產(chǎn)的主頻168MHz，STM32F427作為核心控制器,InvenSense公司的MPU6050傳感器作為姿態(tài)的解算的硬件平臺(tái),實(shí)驗(yàn)平臺(tái)如圖3所示。

圖3 六旋翼飛行器的實(shí)驗(yàn)平臺(tái)

令陀螺儀輸出信號(hào)的低通濾波器的截至頻率ωc=100Hz，陀螺儀的輸出量程為±1 000(°)/s，因此，ωmax=1 000(°)/s，更新周期為0.002 5s，取KP0=0.2，KI0=0.008。

(11)

六旋翼飛行器的姿態(tài)解算準(zhǔn)確性，分為靜態(tài)誤差和動(dòng)態(tài)誤差兩部分。靜態(tài)誤差保證了六旋翼飛行器起飛前的姿態(tài)以及起飛后姿態(tài)保持不變的情況下的準(zhǔn)確性；動(dòng)態(tài)誤差保證了六旋翼飛行器飛行過程中受外界擾動(dòng)影響導(dǎo)致速度變化的情況下姿態(tài)解算的準(zhǔn)確性。

3.2 靜態(tài)測(cè)試

靜態(tài)測(cè)試下，由于六旋翼飛行器速度為0，AECF和ECF中的KP，KI取值相同，即控制效果一樣。取KP=KP0=0.2，KI=0.008。六旋翼飛行器在靜止時(shí)刻經(jīng)自適應(yīng)互補(bǔ)濾波和陀螺儀的姿態(tài)角對(duì)比曲線如圖4所示。

圖4 靜態(tài)情況下輸出的姿態(tài)角

由圖4(a)和4(b)可知，實(shí)際的初始位置不能保證絕對(duì)水平。經(jīng)過互補(bǔ)濾波后，在靜止時(shí)刻，飛行器的俯仰角橫滾角都收斂。但是，經(jīng)過一段時(shí)間后，僅靠陀螺儀輸出的姿態(tài)角由于陀螺儀的漂移特性而呈現(xiàn)發(fā)散趨勢(shì)。所以無法滿足姿態(tài)解算的可靠性要求。定義姿態(tài)解算的誤差為圖2中誤差向量e的模，靜態(tài)誤差實(shí)驗(yàn)數(shù)據(jù)如圖5所示。

圖5 靜態(tài)測(cè)試下輸出誤差

由圖5(a)可知，AECF算法姿態(tài)解算的誤差基本保持在小于0.2°的范圍內(nèi)。由圖5(b)可知，陀螺儀姿態(tài)誤差逐漸增加，不滿足系統(tǒng)的穩(wěn)定性。

考慮電機(jī)振動(dòng)帶來的噪聲影響，選擇六旋翼飛行器的飛行模式為懸停自穩(wěn)定模式，在姿態(tài)閉環(huán)控制的情況下，六旋翼飛行器在一定范圍內(nèi)保持水平，受電池續(xù)航能力的限制，測(cè)試時(shí)間為400 s。靜態(tài)測(cè)試結(jié)果如圖6所示。

由圖6(a)可知，在懸停模式下六旋翼飛行器的橫滾角φ對(duì)期望值的跟蹤性能良好。由圖6(b)可知，在初始狀態(tài)下，受機(jī)體姿態(tài)期望值的影響，機(jī)體姿態(tài)處于擺動(dòng)狀態(tài)。當(dāng)機(jī)體達(dá)到穩(wěn)定狀態(tài)后橫滾角誤差由初始轉(zhuǎn)態(tài)的4°收斂到低于0.5°范圍內(nèi)。結(jié)果表明：在振動(dòng)噪聲下AECF算法姿態(tài)解算的準(zhǔn)確性。

圖6 懸停模式的靜態(tài)測(cè)試

3.3 動(dòng)態(tài)測(cè)試

六旋翼飛行器在飛行過程中，尤其是雜技等動(dòng)作存在姿態(tài)角劇烈變化的情況。本文搭建了一個(gè)簡(jiǎn)易的實(shí)驗(yàn)測(cè)試裝置用于模擬飛行器的姿態(tài)變化環(huán)境，如圖7所示。

圖7 機(jī)體動(dòng)態(tài)實(shí)驗(yàn)測(cè)試裝置

該裝置的工作原理是由計(jì)算機(jī)給驅(qū)動(dòng)器發(fā)送脈寬調(diào)制(PWM)信號(hào)，通過驅(qū)動(dòng)器驅(qū)動(dòng)步進(jìn)電機(jī)，輸出恒定的旋轉(zhuǎn)速度，且飛控與電機(jī)輸出軸之間剛性連接。該裝置可用于動(dòng)態(tài)測(cè)試下AECF和ECF算法的性能對(duì)比實(shí)驗(yàn)。動(dòng)態(tài)測(cè)試下取AECF和ECF的初始Kp=0.2，KI=0.008。由于機(jī)體的橫滾角和俯仰角具有對(duì)稱性，僅選擇橫滾角速度ωx作為研究對(duì)象。測(cè)試輸出的機(jī)體速度曲線如圖8所示，不同算法輸出的誤差曲線如圖9所示。

圖8 步進(jìn)電機(jī)輸出橫滾角速度ωr

圖9 姿態(tài)誤差|e|

由圖8，圖9(a)可知：當(dāng)機(jī)體橫滾角速度為-450°/s時(shí)采用ECF算法輸出動(dòng)態(tài)誤差最大4°～6°。采用AECF算法，其輸出橫滾角的動(dòng)態(tài)誤差為30°～40°。由此可得，AECF算法比ECF算法動(dòng)態(tài)誤差小，但是信號(hào)的毛刺比ECF嚴(yán)重。在300～800s之間，機(jī)體角速度為0°，AECF和ECF算法中的橫滾角都達(dá)到穩(wěn)態(tài)且穩(wěn)態(tài)誤差低于0.2°。

4 結(jié)束語

本文搭建以STM32F427為核心控制器的六旋翼飛行器系統(tǒng)以及靜、動(dòng)態(tài)測(cè)試平臺(tái)。靜、動(dòng)態(tài)測(cè)試結(jié)果如下：1)在靜態(tài)測(cè)試中AECF和ECF的補(bǔ)償系數(shù)KP，KI參數(shù)相同。實(shí)驗(yàn)表明：AECF能保證姿態(tài)解算收斂，并且誤差向量低于0.2°，而陀螺儀傳感器解算效果發(fā)散；六旋翼飛行器存在機(jī)體噪聲的情況下，AECF算法下的姿態(tài)解算結(jié)果依然能保持穩(wěn)定狀態(tài)。2)在動(dòng)態(tài)測(cè)試中，AECF比ECF動(dòng)態(tài)精度更高。

[1] 戴定成，蔡宗平，牛 創(chuàng).基于容積卡爾曼濾波的自適應(yīng)IMM算法[J].現(xiàn)代雷達(dá)，2015，37(3)：27-30.

[2]YunX,AparicioC,BachmannER.Implementationandexperimentalresultsofaquaternion-basedKalmanfilterforhumanbodymotiontracking[J].IEEETransactionsonRobotics,2006,22(6):1216-1227.

[3]AbasN,LegowoA,IbrahimZ.Modelingandsystemidentification

usingextendedKalmanfilterforaquadrotorsystem[J].AppliedMechanics&Materials,2013,313-314:976-981.

[4] 夏 楠,邱天爽,李景春,等.一種卡爾曼濾波與粒子濾波相結(jié)合的非線性濾波算法[J].電子學(xué)報(bào),2013, 41(1)：148-152.

[5] 曲仕茹,馬志強(qiáng).改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用[J].飛行力學(xué),2013,31(5):458-461.

[6] 郭曉鴻，楊 忠，陳 喆,等.EKF和互補(bǔ)濾波器在飛行姿態(tài)確定中的應(yīng)用[J].傳感器與微系統(tǒng),2011,30(11):149-152.

[7] 付勇杰,丁艷紅,梁義維,等.動(dòng)態(tài)傾角傳感器及其傳遞特性的研究[J].儀表技術(shù)與傳感器,2012(9):6-8.

[8]MahonyR,HamelT,PflimlinJ.Nonlinearcomplementaryfiltersonthespecialorthogonalgroup[J].IEEETransactionsonAutomaticControl,2008,53(5):1203-1218.

[9]EustonM,CooteP,MahonyR,etal.Acomplementaryfilterforattitudeestimationofafixed-wingUAV[C]∥IEEEInternationalConferenceonIntelligentRobotsandSystems,Nice,France:2008:340-345.

[10] 王 立，章 政，孫 平.一種自適應(yīng)互補(bǔ)濾波姿態(tài)估計(jì)算法[J].控制工程，2015(5):881-886.

[11] 張承岫,李鐵鷹,王耀力,等.基于MPU6050和互補(bǔ)濾波的四旋翼飛控系統(tǒng)設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào),2016,29(7):1011-1015.

[12] 傅忠云,劉文波,孫金秋,等.自適應(yīng)混合濾波算法在微型飛行器姿態(tài)估計(jì)中的應(yīng)用[J].傳感技術(shù)學(xué)報(bào),2014(5):698-703.

[13]WuZM,SunZG,ZhangWZ,etal.Anovelapproachforattitudeestimationbasedonmemsinertialsensorsusingnonlinearcomplementaryfilters[J].IEEESensorsJournal,2016,16(10):1.

[14] 孫金秋,游有鵬,傅忠云,等.基于自適應(yīng)顯式互補(bǔ)濾波的姿態(tài)解算方法[J].測(cè)控技術(shù),2015, 4(4):24-27.

劉 洲(1993- )，男，碩士研究生，主要研究方向?yàn)閼T性傳感器的姿態(tài)解算以及六旋翼飛行器的控制算法，E—mail：liuzhou2015@csu.edu.cn。

Application of adaptive explicit complementary filtering in six-rotor aircraft*

LIU Zhou, SHAN Xiu-yang, TAN Fang

(School of Mechanical and Electrical Engineering,Central South University,Changsha 410083,China)

To solve the drift problem of inertial sensor of six-rotor aircraft during the measurement process,a attitude estimation algorithm based on adaptive explicit complementary filtering(AECF)is proposed.The principle and stability of the proposed algorithm are analyzed.A six-rotor aircraft experimental test device is set up to carry out static and dynamic experiment of the aircraft.Adaptive explicit complementary filtering algorithm,explicit complementary filtering algorithm and gyroscope sensor measurement are compared in the experiment.The experimental results show that the adaptive explicit complementary filtering algorithm can make the solution attitude error of sensor gradually converge and its dynamic error is the smallest.

adaptive explicit complementary filtering; six-rotor aircraft; attitude estimation; inertia sensor; gyroscope

10.13873/J.1000—9787(2017)05—0157—03

2017—07—17

國(guó)家“973”計(jì)劃基金資助項(xiàng)目(2011CB013104)；中南大學(xué)中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金資助項(xiàng)目

V 249

A

1000—9787(2017)05—0157—04