改進(jìn)互補(bǔ)濾波在六旋翼飛行器中的應(yīng)用*

許晶晶， 郭培源， 董小棟， 徐 盼

(北京工商大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，北京 100048)

0 引 言

近年來，低成本的多旋翼無人飛行器因結(jié)構(gòu)簡(jiǎn)單、操作靈活、具有垂直起降和懸停能力等眾多優(yōu)點(diǎn)，在民用與工業(yè)領(lǐng)域的應(yīng)用越來越廣泛。

飛行器的姿態(tài)估計(jì)是飛行器實(shí)現(xiàn)穩(wěn)定飛行的基礎(chǔ)，而姿態(tài)解算的精度和速度將直接影響到飛控算法的穩(wěn)定性和可靠性。而高精度的慣性導(dǎo)航器件并不適用于消費(fèi)級(jí)無人機(jī)領(lǐng)域，因此,市面上應(yīng)用最廣泛的是較為低廉的微機(jī)電系統(tǒng) (micro-electro-mechanical system，MEMS)傳感器。但由于MEMS傳感器中的陀螺儀角度是由積分得到的，長(zhǎng)時(shí)間會(huì)出現(xiàn)零點(diǎn)漂移與溫度漂移，隨著時(shí)間的積累，角度值存在很大誤差；MEMS傳感器中的加速度計(jì)對(duì)干擾非常敏感，在飛行過程中，易受噪聲和振動(dòng)的影響，動(dòng)態(tài)特性較差。因此，需要在軟件上改進(jìn)，引入信息融合技術(shù)將多種傳感器的數(shù)據(jù)進(jìn)行融合處理，以提高姿態(tài)解算的精度。Hoffmann F等人[1～4]提出了卡爾曼濾波算法對(duì)數(shù)據(jù)進(jìn)行融合；吳濤等人[5～10]提出了用擴(kuò)展卡爾曼濾波 (extended Kalman filtering,EKF) 算法來解決系統(tǒng)的非線性問題，但其計(jì)算量大，對(duì)處理器要求高，計(jì)算時(shí)間長(zhǎng)，不適用于微型飛行器；劉洲等人[11～13]提出了一種自適應(yīng)擴(kuò)展互補(bǔ)濾波方法(adaptive extended complementary filtering,AECF)減少了系統(tǒng)的動(dòng)態(tài)誤差，但其對(duì)于陀螺儀的解算精度很低。綜上所述，針對(duì)單個(gè)慣性傳感器不可信，復(fù)雜算法對(duì)硬件要求高等問題，提出了一種改進(jìn)的一階互補(bǔ)濾波姿態(tài)估計(jì)算法,利用加速度計(jì)補(bǔ)償陀螺儀偏差，并加入比例—積分(proportional-integral,PI)控制環(huán)節(jié)，用于實(shí)現(xiàn)姿態(tài)誤差的動(dòng)態(tài)權(quán)重補(bǔ)償。實(shí)驗(yàn)表明:算法計(jì)算量小，實(shí)時(shí)性好，精度高，能夠很好的應(yīng)用于六旋翼飛行器中。

1 飛行器系統(tǒng)建模與姿態(tài)估計(jì)

1.1 坐標(biāo)系定義

根據(jù)文獻(xiàn)[14]對(duì)六旋翼飛行器的姿態(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系中分別用橫滾角φ、俯仰角θ和偏航角ψ描述。

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

(X,Y,Z)=(x,y,z)

(1)

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

(2)

式中φ,ψ,θ分別為偏航角、橫滾角、俯仰角。

1.2 姿態(tài)更新四元數(shù)算法

Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k

(3)

(4)

(5)

N系與b系基本旋轉(zhuǎn)對(duì)應(yīng)的坐標(biāo)變換矩陣為

(6)

比較式(5)和式(6)可得飛行器的姿態(tài)角為

(7)

四元數(shù)的微分方程為

(8)

式中ωx,ωy,ωz為機(jī)體坐標(biāo)系下的角速度。在已知初始四元數(shù)的情況下，通過三軸陀螺儀測(cè)量3個(gè)軸的角速度即可實(shí)時(shí)更新四元數(shù)的值，進(jìn)而更新姿態(tài)角獲得姿態(tài)信息。

1.3 互補(bǔ)濾波算法

MEMS傳感器中的陀螺儀能夠測(cè)量機(jī)體旋轉(zhuǎn)角速度，其動(dòng)態(tài)性能較好，但由于其角度由積分得到，隨著時(shí)間的積累，角度值存在很大誤差。加速度計(jì)的靜態(tài)特性非常好，但由于其對(duì)干擾非常敏感，在飛行過程中，易受噪聲和振動(dòng)的影響，動(dòng)態(tài)特性較差。因此，在低頻段一般可以信賴加速度計(jì)的數(shù)據(jù)，在高頻段信賴陀螺儀的數(shù)據(jù)，互補(bǔ)濾波算法就是依據(jù)兩者在頻域上的動(dòng)態(tài)響應(yīng)特性的互補(bǔ)關(guān)系，在頻域中加入低通和高通濾波器，對(duì)兩者的姿態(tài)數(shù)據(jù)進(jìn)行融合，利用加速度計(jì)的測(cè)量值來修正陀螺儀的誤差，提高系統(tǒng)的解算精度。設(shè)加速度計(jì)的測(cè)量值為αx,αy,αz，陀螺儀積分后的姿態(tài)解算值為νx,νy,νz，有

(9)

陀螺儀與加速度計(jì)之間的誤差為

(10)

但由于在噪聲較大時(shí)，低通濾波器的阻帶衰減較慢，固定的參數(shù)必然難以達(dá)到在所有情況下的最優(yōu)估計(jì)值，濾波效果難以達(dá)到預(yù)期。因此，本文在互補(bǔ)濾波的原理基礎(chǔ)上，在計(jì)算出陀螺儀與加速度計(jì)之間的誤差之后，引入了PI控制器，構(gòu)成自適應(yīng)互補(bǔ)濾波器，動(dòng)態(tài)補(bǔ)償系數(shù)Kp，Ki，噪聲干擾較小時(shí)，適當(dāng)提高加速度計(jì)的權(quán)重，在噪聲干擾較大時(shí)，適當(dāng)降低加速度計(jì)的權(quán)重，主要依靠陀螺儀進(jìn)行姿態(tài)測(cè)量，從而提高姿態(tài)數(shù)據(jù)的融合精度，即使在噪聲大的情況下，也能較為準(zhǔn)確地解算出姿態(tài)角。原理如圖2所示。姿態(tài)更新算法軟件設(shè)計(jì)流程如圖3所示。

圖2 加入PI控制器的互補(bǔ)濾波算法原理框圖

誤差積分調(diào)節(jié)

誤差比例調(diào)節(jié)

(11)

由此，獲得更新的四元數(shù)并代入式(7)中，即可得出較為準(zhǔn)確的姿態(tài)角度。

圖3 濾波算法程序流程

2 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證算法的可行性與效果，搭建了基于匿名科創(chuàng)制作的開源六旋翼飛行器平臺(tái)進(jìn)行實(shí)驗(yàn)，如圖4所示。其主控芯片采用STM32F103，主頻72MHz，具有豐富的外設(shè)接口。姿態(tài)芯片采用MPU6050，包含3軸陀螺儀和3軸加速度計(jì)。通信方式為WiFi模式，可以實(shí)時(shí)回傳數(shù)據(jù)，無需保存到內(nèi)存卡，因此，在其提供的地面站上非常方便進(jìn)行濾波算法調(diào)試。本文以X軸的實(shí)時(shí)輸出為例，提供實(shí)時(shí)三軸慣性原始數(shù)據(jù)和參考姿態(tài)角。

圖4 匿名科創(chuàng)實(shí)驗(yàn)平臺(tái)

采用雙閉環(huán)控制PID算法控制6個(gè)電機(jī)的轉(zhuǎn)速，軟件上控制系統(tǒng)的周期為3ms，脈寬調(diào)制(pulse width modulation,PWM)頻率為10kHz，同時(shí)進(jìn)行限幅處理，避免油門突然增大或減小引起飛行器震蕩，導(dǎo)致系統(tǒng)不穩(wěn)定。陀螺儀輸出信號(hào)的低通濾波器的截止頻率ωc=100Hz，陀螺儀的輸出量程為±1000°/s，更新周期為0.0025s，在此不考慮偏航角對(duì)飛行器偏航情況的影響。PID參數(shù)為0.85,32,0.05，一階濾波、卡爾曼濾波、二階互補(bǔ)濾波算法的實(shí)時(shí)濾波效果波形如圖5所示。由圖5可以看出二階互補(bǔ)濾波與卡爾曼濾波效果相近。但可以看出，二階互補(bǔ)濾波與卡爾曼濾波效果相較于一階濾波有一定的滯后，由于其計(jì)算量相對(duì)較大，實(shí)時(shí)跟蹤性較一階濾波效果差。表1為時(shí)間消耗與角度估計(jì)的標(biāo)準(zhǔn)差比較。

圖5 X軸3種濾波對(duì)比

由圖6實(shí)時(shí)數(shù)據(jù)波形的結(jié)果可以得出：傳統(tǒng)一階互補(bǔ)濾波的姿態(tài)解算誤差角為12.5°，改進(jìn)一階互補(bǔ)濾波算法的姿態(tài)解算誤差角為3.5°。改進(jìn)一階互補(bǔ)濾波算法的實(shí)時(shí)濾波動(dòng)態(tài)曲線過渡平滑，實(shí)時(shí)跟蹤效果較傳統(tǒng)互補(bǔ)濾波更為理想。在震動(dòng)較大的環(huán)境下，其實(shí)時(shí)解算精度可控制在0°～5°之間。由此，驗(yàn)證了算法的可行性，且改進(jìn)后的一階濾波效果更好，有效地減弱了干擾帶來的影響，提高了姿態(tài)解算的精度。

表1 時(shí)間消耗與角度估計(jì)誤差的標(biāo)準(zhǔn)差比較

圖6 一階濾波與加入PI控制的改進(jìn)一階互補(bǔ)濾波

3 結(jié) 論

分析了基于一階互補(bǔ)濾波算法的姿態(tài)解算原理與實(shí)現(xiàn)方法，指出了運(yùn)動(dòng)中的加速度對(duì)姿態(tài)的影響，并提出了震動(dòng)較大時(shí)的改進(jìn)互補(bǔ)濾波算法，將加速度計(jì)解算出的角度加入一個(gè)PI控制。實(shí)驗(yàn)結(jié)果表明：算法提高了姿態(tài)解算的精度，且實(shí)時(shí)性較好，適用于六旋翼姿態(tài)解算。

[1] 劉 戀,向鳳紅,毛劍琳.修正的UKF濾波時(shí)差定位算法[J].傳感器與微系統(tǒng),2017,36(4):138-142.

[2] Hoffmann F,Goddemeier N,Bertram T.Attitude estimation and control of a quadrocopter[C]∥IEEE/RSJ International Confe-rence on Intelligent Robots and Systems,IEEE,2010:1072-1077.

[3] Mahmood A,Wallace J W,Jensen M A.Radio frequency UAV attitude estimation using direction of arrival and polarization[C]∥European Conference on Antennas and Propagation,IEEE,2017:1857-1859.

[4] Chang J,Cieslak J,Dávila J,et al.A two-step approach for an enhanced quadrotor attitude estimation via IMU data[J].IEEE Transactions on Control Systems Technology, 2017,99:1-9.

[5] Tarhan M,Altugˇ E.EKF-based attitude estimation and stabilization of a quadrotor UAV using vanishing points in catadioptric images[J].Journal of Intelligent & Robotic Systems,2011,62(3-4):587-607.

[6] 閆保芳,毛慶洲.一種基于卡爾曼濾波的超寬帶室內(nèi)定位算法[J].傳感器與微系統(tǒng),2017,36(10)：137-140,143.

[7] 吳 濤,白 茹,朱禮堯,等.基于卡爾曼濾波的航姿參考系統(tǒng)設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào),2016,29(4):531-535.

[8] Wang S,Yang Y.Quadrotor aircraft attitude estimation and control based on Kalman filter[C]∥Control Conference,IEEE,2012:5634-5639.

[9] 賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法[J].傳感技術(shù)學(xué)報(bào),2014(1):90-95.

[10] Mahony R,Hamel T,Pflimlin J M.Nonlinear complementary filters on the special orthogonal group[J].IEEE Transactions on Automatic Control,2008,53(5):1203-1218.

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

[12] 劉 洲,單修洋,譚 芳.自適應(yīng)顯式互補(bǔ)濾波在六旋翼飛行器中的應(yīng)用[J].傳感器與微系統(tǒng),2017,36(5):157-160.

[13] 趙 陽,婁小平,劉 鋒,等.自適應(yīng)MEMS加速度計(jì)濾波算法[J].傳感器與微系統(tǒng),2016,35(11):120-122.

[14] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社,2013:5-7.