改進互補濾波在六旋翼飛行器中的應用*

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

(北京工商大學 計算機與信息工程學院，北京 100048)

0 引 言

近年來，低成本的多旋翼無人飛行器因結構簡單、操作靈活、具有垂直起降和懸停能力等眾多優點，在民用與工業領域的應用越來越廣泛。

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

1 飛行器系統建模與姿態估計

1.1 坐標系定義

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

捷聯慣性導航系統將慣性傳感器直接固定在飛行器上，慣性傳感器測得的數據基于b系。姿態解算結果的坐標需從b系轉換到N系,如式(1)

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

(1)

圖1 六旋翼飛行器坐標系

(2)

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

1.2 姿態更新四元數算法

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

(3)

(4)

(5)

N系與b系基本旋轉對應的坐標變換矩陣為

(6)

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

(7)

四元數的微分方程為

(8)

式中ωx,ωy,ωz為機體坐標系下的角速度。在已知初始四元數的情況下，通過三軸陀螺儀測量3個軸的角速度即可實時更新四元數的值，進而更新姿態角獲得姿態信息。

1.3 互補濾波算法

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

(9)

陀螺儀與加速度計之間的誤差為

(10)

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

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

誤差積分調節

誤差比例調節

(11)

由此，獲得更新的四元數并代入式(7)中，即可得出較為準確的姿態角度。

圖3 濾波算法程序流程

2 實驗結果與分析

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

圖4 匿名科創實驗平臺

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

圖5 X軸3種濾波對比

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

表1 時間消耗與角度估計誤差的標準差比較

圖6 一階濾波與加入PI控制的改進一階互補濾波

3 結 論

分析了基于一階互補濾波算法的姿態解算原理與實現方法，指出了運動中的加速度對姿態的影響，并提出了震動較大時的改進互補濾波算法，將加速度計解算出的角度加入一個PI控制。實驗結果表明：算法提高了姿態解算的精度，且實時性較好，適用于六旋翼姿態解算。

[1] 劉 戀,向鳳紅,毛劍琳.修正的UKF濾波時差定位算法[J].傳感器與微系統,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] 閆保芳,毛慶洲.一種基于卡爾曼濾波的超寬帶室內定位算法[J].傳感器與微系統,2017,36(10)：137-140,143.

[7] 吳 濤,白 茹,朱禮堯,等.基于卡爾曼濾波的航姿參考系統設計[J].傳感技術學報,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] 賈瑞才.基于四元數EKF的低成本MEMS姿態估計算法[J].傳感技術學報,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] 傅忠云,劉文波,孫金秋,等.自適應混合濾波算法在微型飛行器姿態估計中的應用[J].傳感技術學報,2014(5):698-703.

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

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

[14] 秦永元.慣性導航[M].北京：科學出版社,2013:5-7.