自適應顯式互補濾波在六旋翼飛行器中的應用*

劉 洲， 單修洋， 譚 芳

(中南大學 機電工程學院，湖南 長沙 410083)

自適應顯式互補濾波在六旋翼飛行器中的應用*

劉 洲， 單修洋， 譚 芳

(中南大學 機電工程學院，湖南 長沙 410083)

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

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

0 引 言

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

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

1.1 坐標系定義

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

圖1 六旋翼飛行器的坐標系

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

(1)

(2)

1.2 姿態(tài)更新

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

(3)

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

(4)

(5)

2 基于AECF的融合算法

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

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

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

(6)

2.1AECF穩(wě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為初始補償系數(shù)；KP1為臨界補償系數(shù)。

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

3 實驗測試

3.1 實驗平臺搭建

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

圖3 六旋翼飛行器的實驗平臺

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

(11)

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

3.2 靜態(tài)測試

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

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

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

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

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

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

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

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

3.3 動態(tài)測試

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

圖7 機體動態(tài)實驗測試裝置

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

圖8 步進電機輸出橫滾角速度ωr

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

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

4 結束語

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

[1] 戴定成，蔡宗平，牛 創(chuàng).基于容積卡爾曼濾波的自適應IMM算法[J].現(xiàn)代雷達，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] 夏 楠,邱天爽,李景春,等.一種卡爾曼濾波與粒子濾波相結合的非線性濾波算法[J].電子學報,2013, 41(1)：148-152.

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

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

[7] 付勇杰,丁艷紅,梁義維,等.動態(tài)傾角傳感器及其傳遞特性的研究[J].儀表技術與傳感器,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] 王 立，章 政，孫 平.一種自適應互補濾波姿態(tài)估計算法[J].控制工程，2015(5):881-886.

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

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

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

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

劉 洲(1993- )，男，碩士研究生，主要研究方向為慣性傳感器的姿態(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

國家“973”計劃基金資助項目(2011CB013104)；中南大學中央高?；究蒲袠I(yè)務費專項資金資助項目

V 249

A

1000—9787(2017)05—0157—04