0 引言

    四旋翼飛行器因為其結構簡單對稱、相對容易控制的特點，在監測、檢查、救援、電子情報等很多重要的領域被廣泛使用^[1-2]，最近幾年在學術界和科研領域引起很大興趣。四軸飛行器姿態角解算的速度和精度直接影響飛行器是否能獲得實時、穩定的飛行姿態，所以姿態解算是四旋翼飛行器研究的核心之一。

    對于四旋翼飛行器的姿態測量，采用集成了三軸陀螺儀和三軸加速度計的MPU6050進行姿態數據的采集，在較簡單的硬件平臺下，應該怎樣濾除外部干擾，得到精確度比較高的數據變得十分重要。近幾年姿態估計算法隨著電子技術的發展相繼出現。文獻[3]應用互補濾波^[3]對四旋翼的數據進行融合，該算法可以有效抑制因電機轉動而帶來的周期性干擾和因溫度影響給MPU6050帶來的漂移誤差。但是應用互補濾波導致飛控系統有滯后現象，截止頻率附近衰減較慢，處理后的數據存在誤差，精確度較低。文獻[4]提出基于歐拉角的迭代擴展卡爾曼^[4]（IEKF）姿態測量方法，該方法使用歐拉角方法描述姿態角，基于EKF濾波算法，在計算觀測矩陣H時進行多次迭代，該算法估計出的姿態比普通EKF估計的姿態精度高。但該算法復雜、計算量大、循環周期長，僅適用于仿真，在實際工程上應用較少。文獻[5-6]提出粒子濾波的算法，粒子濾波算法是在狀態空間中尋找一組隨機樣本，近似地表示出概率密度函數，獲得系統狀態的最小方差，當樣本數量趨于無窮時可以逼近任何形式的概率密度函數，擺脫了解決非線性濾波問題時隨機量必須滿足高斯分布的制約，因此粒子濾波能夠比較精準地表達基于觀測量和控制量的后驗分布。但是粒子濾波需要大量的樣本數量才能較準確地近似系統的后驗概率密度，使得算法復雜度較高，而且重采樣階段會造成樣本有效性和多樣性的損失，導致樣本出現貧化現象。

    利用MPU6050采集姿態信息，為了獲得較精確的姿態角，對EKF算法進行了改進，MPU6050在每一個采樣時刻可以同時測得當前時刻飛行器的姿態角和姿態角變化率的信息，結合改進EKF算法對數據進行處理能夠準確地估算飛行器當前的位置和姿態。飛行器在飛行過程中受非高斯白噪聲的影響，本文在進行姿態解算時，為有效模擬飛行器測量過程的噪聲，用兩個加權高斯概率密度函數表示非高斯白噪聲函數，選擇合理高斯函數的權重、均值和方差。通過改進型EKF有效地濾除該噪聲，減小了飛行器姿態偏差。

1 系統建模

    為了獲得四旋翼飛行器的姿態數據信息，首先選定兩個基本坐標系：慣性坐標系E(XYZ)和機體坐標系B(X′Y′Z′)^[7]，如圖1所示。

式中c代表cos，s代表sin，轉換矩陣是非線性矩陣，為避免該矩陣出現奇點問題，飛行器在測試飛行過程中不會做出大幅度機動動作，因而可以認為系統滿足以下假設：

    (1)四旋翼飛行器橫滾角與俯仰角滿足以下不等式：

    (2)在實際飛行中飛行器角速度信號由陀螺儀測量，因此可以假設飛行器橫滾角速度與偏航角速度為有界信號L_∞，即：w_x∈L_∞，w_z∈L_∞。

    MPU6050可以快速、穩定地采集飛行器靜止時的姿態信息，但當飛行器開始運動時，機體抖動及螺旋槳高頻率的運動使得MPU6050測得的飛行器位姿存在明顯的偏差。利用EKF將MPU6050采集的數據進行融合。EKF濾波原理是將非線性系統進行線性化處理，利用泰勒級數展開系統函數，取一次項作為EKF的系數矩陣，忽略了高階項噪聲。因此采用EKF進行姿態解算存在較大的濾波偏差。針對以上問題，本文提出改進型EKF，對四旋翼飛行器的姿態數據進行解算。

2 改進型擴展卡爾曼濾波原理及算法

    為有效濾除噪聲，測得精確的姿態數據，對EKF濾波算法進行了改進，采用兩個EKF并行地對四旋翼飛行器的姿態數據進行預測和更新，將它們輸出的狀態變量加權整合后輸出飛行器的最優狀態估計，因此經改進EKF處理后的飛行器姿態數據達到最小均方誤差。實現對飛行器位姿的有效操控，從理論上分析經過改進EKF濾波后的效果會更加接近真實值，有效地濾除噪聲，且算法復雜度低、運算量小，不需要時頻域的頻繁轉換，更不會因為樣本數量的選擇而導致樣本重采樣現象，融合后飛行器姿態數據精度高。具體步驟如下：

    (1)EKF1 與EKF2 對四旋翼飛行器姿態數據并行地進行處理，輸出對應的狀態變量X_1，k、X_2，k和協方差P_1，k、P_2，k；

    (2)計算EKF1 和EKF2的似然函數，求出對應的權重a₁、a₂；

    (3)加權整合后更新四旋翼飛行器狀態X_k和協方差P_k，輸出飛行器姿態的最優估計；

    (4)實時調整飛行器姿態，待飛行器下一個狀態到達前返回步驟(1)，繼續下一次循環。

    具體過程如圖2所示。

    對于一個隨機系統，具體的估計過程如下：

3 仿真與實驗

    在改進型EKF理論的基礎上進行仿真，在NI my RIO-1900的實驗平臺上對飛行器進行實驗。

    為保證飛行器初始時刻的穩定性及減小初始時刻姿態誤差，對初始時刻四旋翼飛行器姿態數據進行濾波處理，以橫滾角為例，采集飛行器靜止時刻姿態數據，改進EKF濾波后姿態與普通EKF濾波后姿態進行對比，如圖3、圖4所示。普通EKF濾波后飛行器姿態有約1°左右的偏差，改進EKF濾波后飛行器姿態穩定性較好，偏差較小，幾乎可以忽略。二者相比較，改進型EKF的濾波效果較好，魯棒性強。

    給飛行器橫滾角一個操控命令，使其傾斜14°，飛行器在執行操控命令時，機體坐標系繞慣性坐標系旋轉會產生一定旋轉角，橫滾角初始狀態不是絕對的0°，經測試飛行器初始時刻橫滾角為0.41°。如圖5和圖6所示，飛行器接收到操控命令后，普通EKF濾波后的飛行器經40 ms達到期望姿態，在140 ms 內有波動，140 ms 之后飛行器波動范圍減小，但存在約0.5°左右的偏移。改進EKF濾波后的飛行器經38 ms后達到期望姿態，僅用時80 ms飛行器就達到理想狀態，且偏移較小，可以忽略。從二者的對比圖可得出，改進型EKF濾波后飛行器姿態角較穩定，提高了系統的準確性和可靠性。

    通過MPU6050采集四旋翼飛行器0～100 s內的姿態數據，采集的姿態經改進EKF濾波后的動態測試如圖7，在4 s～16 s、48 s～54 s、60 s～70 s、72 s～86 s、91 s～96 s時間段內，改進EKF估計值完全跟蹤到理想值，在16 s～18 s、23 s～26 s、56 s～59 s、96 s～99 s時間段內，由于四旋翼飛行器姿態角度變化幅度較大，改進EKF估計值與理想值之間出現偏差，但在下一刻，即19 s、27 s、60 s、100 s時，改進EKF估計值快速恢復實時追蹤。

    經計算，改進EKF估計值均方誤差為3.306，普通EKF估計值均方誤差為5.884 3。改進EKF估計值精度明顯高于普通EKF估計值精度。精度提高的效率為：

    經過多次測試可知：該算法設計合理，姿態精度有較明顯的改善，改進的算法已成功應用在四旋翼飛行器上，得到了理想的效果。

4 結論

    本文改進的擴展卡爾曼了濾波算法，克服了非高斯白噪聲對傳感器姿態解算的影響，且該算法復雜度低、精度高、計算量小。實驗結果表明，改進的算法可以準確地估計出姿態角，與經過普通EKF濾波后的姿態測量值對比，改進EKF在飛行器姿態精度上有較明顯的改善，驗證改進方法是有效的，為四旋翼無人機實時調整姿態提供了一種科學手段。該算法只對MPU6050采集的數據進行融合處理，進一步可將該算法應用至地磁計模塊，對飛行器數據進一步進行融合處理，以快速實現飛行器的垂直起飛和懸停的控制操作。

參考文獻

[1] ROSALES C，GANDOLFO D，SCAGLIA G，et al.Trajectory tracking of a mini four-rotor helicopter in dynamic environments-a linear algebra approach[J].Robotica，2015，33(8)：1628-1652.

[2] LUPASHIN S，HEHN M，MUELLER M W，et al.A platform for aerial robotics research and demonstration：the flying machine arena[J].Mechatronics，2014，24(1)：41-54.

[3] LU X，LI J，LIU W.A study of complementary filter algorithm for four-rotor helicopters attitude control system[J].Sensors & Transducers，2014，174(7)：298-306.

[4] 賀海鵬，閻妍，馬良，等.基于IEKF的四旋翼無人機姿態測量方法研究[J].計算機仿真，2015(4)：56-60.

[5] MERCADO D A，CASTILLO P，LOZANO R.Quadrotors data fusion using a particle filter[C].Unmanned Aircraft Systems(ICUAS)，2014 International Conference on.IEEE，2014.

[6] 曲仕茹，馬志強.改進的粒子濾波在四旋翼姿態估計中的應用[J].飛行力學，2013(5)：458-461，466.

[7] ZHAO S，AN H，LIU J，et al.Identification of aerodynamic parameter for quadrotor model[J].Journal of Electronic Measurement & Instrument，2013，27(8)：744-749.

作者信息：

侯玉涵，王耀力

(太原理工大學 信息工程學院，山西 太原030024)