基于Mahony濾波算法的姿態(tài)解算與應用研究

徐恩松 陸文華 劉云飛 李寶磊 馮旺

摘 要：針對多旋翼無人機自駕儀在某一特定的飛行路徑中，飛行控制系統(tǒng)中存在姿態(tài)信息的實時獲取和精確解算誤差較大問題，文章介紹了四旋翼飛行器姿態(tài)解算常用四元數(shù)法姿態(tài)解算方法與互補濾波融合算法原理，并研究了影響姿態(tài)解算的重要參數(shù)（比例系數(shù)Kp）的最佳取值。通過對Mahony算法給予改進，最后在Matlab仿真試驗和Mission Planner地面站軟件的測試下，比較不同的姿態(tài)解算結(jié)果，調(diào)試得出最佳的比例系數(shù)Kp值，得到了更好的姿態(tài)解算結(jié)果。四旋翼飛行器姿態(tài)信息的準確性直接影響飛行器的飛行控制效果，實驗結(jié)果表明，Mahony自適應互補濾波算法濾波效果良好，可以有效降低低成本IUM系統(tǒng)的誤差累加，可以在實際應用中得到較高的姿態(tài)精度，對飛行器的控制研究具有一定的參考價值與指導意義。

關鍵詞： 姿態(tài)解算;比例系數(shù)Kp;Mahony算法;Matlab仿真

【Abstract】 Aiming at the problem of real-time acquisition and accurate solution of attitude information in flight control system of multi-rotor UAV self-driving instrument in a specific flight path， in this paper， the quaternion method and the principle of complementary filtering fusion algorithm are introduced， and the optimum values of the important parameters （proportional coefficient Kp） affecting attitude calculation are studied. The Mahony algorithm is improved. Finally， under the test of Matlab simulation and Mission Planner ground station software， different attitude calculation results are compared， and the optimal proportional coefficient Kp value is debugged. The better attitude calculation results are obtained. The accuracy of attitude information of Four-rotor aircraft directly affects the flight control effect of the aircraft. The experimental results show that the filtering effect of Mahony adaptive complementary filtering algorithm is good， which can effectively suppress the error accumulation of low-cost IUM system. It can obtain high attitude accuracy in practical application. It has certain reference value and guiding significance for the control research of the aircraft.

【Key words】 ?attitude solution; proportional coefficient Kp; Mahony algorithm; Matlab simulation

0 引 言

0.1 飛行姿態(tài)解算的概念

姿態(tài)解算的原理：用不同的坐標系表示同一個確定的向量時，2個坐標系表示出的結(jié)果大小和方向一定相同。但是因為2個坐標系的旋轉(zhuǎn)矩陣存在著誤差，即使得當一個向量經(jīng)過一個帶有誤差的旋轉(zhuǎn)矩陣后，在另一個坐標系中距理論值將出現(xiàn)一定偏差。因此可通過此偏差來修正這個旋轉(zhuǎn)矩陣。目前，常見的姿態(tài)表示方法有歐拉角法、方向余弦法和四元數(shù)法。其中，歐拉角法在求解姿態(tài)時存在奇點（萬向節(jié)死鎖），不能用于全姿態(tài)解算;方向余弦可用于全姿態(tài)的解算，但計算量大，也無法滿足實時性要求;四元數(shù)法，計算量小，無奇點且可以滿足飛行器運動過程中姿態(tài)的實時解算要求。

0.2 姿態(tài)解算算法

四旋翼無人機的飛行控制系統(tǒng)中，姿態(tài)信息的實時獲取和精確解算尤為關鍵[1]。 目前常見的姿態(tài)解算算法有梯度下降算法[2]、互補濾波算法[3]和卡爾曼濾波算法[4]等。不同算法在不同情況下的解算效果不同，本文主要研究基于Mahony互補濾波算法的姿態(tài)解算[3]，對2個坐標系相互轉(zhuǎn)化時旋轉(zhuǎn)矩陣存在偏差進行修正，旋轉(zhuǎn)矩陣的元素是四元數(shù)，并于修正后給出了濾波的仿真結(jié)果分析。

1 四元數(shù)法姿態(tài)解算原理

1.1 四元數(shù)基本概念

2 Mahony互補濾波融合算法

互補濾波融合算法可根據(jù)加速度計測量數(shù)據(jù)利用梯度下降法得到最小誤差四元數(shù)的微分，再通過互補濾波算法與陀螺儀得出角速度微分四元數(shù)進行融合，對融合后的姿態(tài)微分四元數(shù)進行積分，估算出最優(yōu)四元數(shù)。

經(jīng)典互補濾波[8]利用陀螺儀和加速度計各自在動態(tài)和靜態(tài)上的優(yōu)勢，在頻域上用加速度計測得的準確瞬時值來彌補陀螺儀隨時間積累所產(chǎn)生的漂移誤差，即分別加入低通和高通濾波器提高姿態(tài)解算的精度[9]。互補濾波的關鍵在于系數(shù) Kp的選擇，經(jīng)典低通環(huán)節(jié)阻帶衰減較慢，不能滿足四旋翼飛行器在飛行中的高動態(tài)性，通過對Mahony算法做出改進，最后在Matlab仿真試驗和Mission Planner地面站軟件[10]的測試下，比較不同的姿態(tài)解算結(jié)果，調(diào)試得出最佳的比例系數(shù)Kp值，得到了更好的姿態(tài)解算結(jié)果。

3 Matlab姿態(tài)解算仿真實驗

在實驗前，研究將進行試驗數(shù)據(jù)的獲取，獲取飛行所需要的傳感器的姿態(tài)角數(shù)據(jù)。首先QGC與Pixhawk進行連接[12]，并設置了多旋翼無人機的自主飛行路徑，然后在一段時間的飛行后，提取其飛行數(shù)據(jù)，主要涉及其未修正前的傳感器所測量的姿態(tài)角信息，再將其保存在Matlab能夠調(diào)用的dat數(shù)據(jù)文件夾中[13]。

這里，首先將特定路徑Pixhawk的飛行數(shù)據(jù)從日志中提取出來，提取的數(shù)據(jù)是飛行器未經(jīng)過Mahony算法解算[14]得到的的姿態(tài)角數(shù)據(jù)，Φ、θ、ψ分別為機體的俯仰角、偏航角和翻滾角，然后將這些數(shù)據(jù)以dat的格式保存起來。將數(shù)據(jù)加載到已經(jīng)封裝好的Mahony源程序中，傳入Matlab中進行仿真實驗[15]。

實驗中，先后更改了3次Kp值，依次為0.6、1.1、2，得到了3張效果圖。分別如圖1～圖3所示。

4 仿真實驗與結(jié)果分析

比例系數(shù)反映了姿態(tài)結(jié)果中加速計測量值對姿態(tài)結(jié)果的修正情況。Kp值越大說明加速度計的結(jié)果對姿態(tài)解算的結(jié)果影響越大，又因為對高頻震蕩非常敏感，這就有了結(jié)果中滾動角和俯仰角震蕩頻率[16]隨著Kp值的增大而增大的情況。所以要不斷地進行試驗，根據(jù)特定的場景確定最適合的Kp值，Kp值并不是越小越好，當Kp值越小時，陀螺儀對濾波結(jié)果的影響就較大，就會產(chǎn)生一些低頻的錯誤信息。所以要綜合考慮Kp與Ki的值的大小。

由圖1～圖3中可以得到，當Kp值小于1.1時，雖然濾波滾動角和俯仰角有波動，但波動的頻率并不是特別大;當Kp值大于等于1.1時，濾波的航向角波動很大;當Kp值為0.6～1.1之間，無人機自主導航的精確度更佳。

5 結(jié)束語

本文闡述了四旋翼飛行器姿態(tài)解算常用四元數(shù)法姿態(tài)解算方法與互補濾波融合算法原理，并研究了影響姿態(tài)解算的重要參數(shù)（比例系數(shù)Kp）的最佳取值。通過對Mahony算法做出改進，最后在Matlab仿真試驗和Mission Planner地面站軟件的測試下，比較不同的姿態(tài)解算結(jié)果，調(diào)試得出最佳的比例系數(shù)Kp值，得到了更好的姿態(tài)解算結(jié)果。四旋翼飛行器姿態(tài)信息的準確性直接影響飛行器的飛行控制效果。實驗結(jié)果表明，Mahony自適應互補濾波算法濾波效果良好，可以有效降低低成本IUM系統(tǒng)的誤差累加，可以在實際應用中得到較高的姿態(tài)精度，對飛行器的控制研究具有一定的參考價值與指導意義。

參考文獻

[1]趙偉，王志華，周兵. 基于.NET技術和MVC的新架構模式[J]. 計算機工程與設計，2012，33（7）：2646-2651.

[2]高京都. 四旋翼飛行器控制系統(tǒng)的設計與實現(xiàn)[D]. 大連：大連理工大學，2015.

[3]李玉玲. 人工智能在計算機視覺及網(wǎng)絡領域中的應用[J]. 電腦編程技巧與維護，2018（8）：156-157，161.

[4]段曉軍，吳成富. 嵌入式實時控制系統(tǒng)的快速原型化及其在無人機飛控系統(tǒng)設計與實時仿真中的應用[C]//中國航空學會控制與應用第十一屆學術年會.煙臺：中國航空學會，2004：530-534.

[5]張錦. 三余度飛控計算機系統(tǒng)軟件的研究與設計[D].西安：西北工業(yè)大學，2006.

[6]劉耀欽，袁承芬. MVC設計模式在Web開中的應用與研究[J]. 信息安全與技術，2013，4（11）：78-80.

[7]LUO R C， LIN M H， SCHERP R S. Dynamic multi-sensor data fusion system for intelligent robots[J]. IEEE Journal of Robotics and Automation， 1988，15（4）： 386-396.

[8]KAMBEROVA G， MANDELBAUM R， MINTZ M， et al. Decision-theoretic approach to robust fusion of location data[J]. Journal of the Franklin Institute， 1999， 336（2）： 269-284.

[9]XU Lijia， CHEN Yangzhou， CUI Pingyuan. Improvement of D-S evidential theory in multisensor data fusion system[C]// Proceedings of the World Congress on Intelligent Control and Automation. Hangzhou：Zhejiang University， 2004： 3124-3128.

[10]MOORE J B. Direct Kalman filtering approach for GPS/INS integration[J]. IEEE Transactions on aerospace and electronic systems， 2002， 38（2）：687-693.

[11]SASIADEKT J Z， WANG Q， ZEREMBA M B. Fuzzy adaptive Kalman filtering for INS/GPS data fusion[C]// the 15th IEEE International Symposium on Intelligent Control （ISIC 2000）.RIO， Patras，Greece：IEEE，2000：181-186.

[12]陳華胄，諶海云. 基于自適應互補濾波的四旋翼飛行器姿態(tài)解算[J]. 測控技術，2018，37（6）：81-84，89.

[13]江杰，王康，李剛. 一種四旋翼姿態(tài)解算與控制優(yōu)化方法設計分析[J].計算機仿真，2016，33（11）：42-47.

[14]CARLSON N A. Federal filter for fault-tolerant integrated navigation system[c]// IEEE. PLANS'88.， Position Location and Navigation Symposium， Reard， Navigation into the 21st Century'.Orlando， FL， USA：IEEE， 1988：110-119.

[15]洪世杰，李嘉欣，侯佳木，等. 自主巡線四旋翼無人機建模及姿態(tài)控制器設計[J]. 現(xiàn)代計算機（專業(yè)版），2018（16）：66-71.

[16]CUI Yongjun， LIU Jun， ZHANG Wendong. Research on adaptive Kalman filtering algorithm for integrated GPS/INS navigation system[C]// 6th International symposium on Test and Measurement. Dalian：[s.n.]，2005，8.

[17]黃曉瑞， 崔平遠， 崔祜濤. GPS/INS組合導航系統(tǒng)自適應濾波算法與仿真研究[J]. 飛行力學，2001， 19（2）：69-72，77.

[18]XIA Q， RAOAND M， YING Y. Adaptive fading Kalman filter with an application[J]. Automatica，1994， 30（8）：1333-1338.