基于Mahony滤波算法的姿态解算与应用研究

徐恩松 陆文华 刘云飞 李宝磊 冯旺

摘 要：针对多旋翼无人机自驾仪在某一特定的飞行路径中，飞行控制系统中存在姿态信息的实时获取和精确解算误差较大问题，文章介绍了四旋翼飞行器姿态解算常用四元数法姿态解算方法与互补滤波融合算法原理，并研究了影响姿态解算的重要参数（比例系数Kp）的最佳取值。通过对Mahony算法给予改进，最后在Matlab仿真试验和Mission Planner地面站软件的测试下，比较不同的姿态解算结果，调试得出最佳的比例系数Kp值，得到了更好的姿态解算结果。四旋翼飞行器姿态信息的准确性直接影响飞行器的飞行控制效果，实验结果表明，Mahony自适应互补滤波算法滤波效果良好，可以有效降低低成本IUM系统的误差累加，可以在实际应用中得到较高的姿态精度，对飞行器的控制研究具有一定的参考价值与指导意义。

关键词： 姿态解算;比例系数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 飛行姿态解算的概念

姿态解算的原理：用不同的坐标系表示同一个确定的向量时，2个坐标系表示出的结果大小和方向一定相同。但是因为2个坐标系的旋转矩阵存在着误差，即使得当一个向量经过一个带有误差的旋转矩阵后，在另一个坐标系中距理论值将出现一定偏差。因此可通过此偏差来修正这个旋转矩阵。目前，常见的姿态表示方法有欧拉角法、方向余弦法和四元数法。其中，欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态解算;方向余弦可用于全姿态的解算，但计算量大，也无法满足实时性要求;四元数法，计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算要求。

0.2 姿态解算算法

四旋翼无人机的飞行控制系统中，姿态信息的实时获取和精确解算尤为关键[1]。 目前常见的姿态解算算法有梯度下降算法[2]、互补滤波算法[3]和卡尔曼滤波算法[4]等。不同算法在不同情况下的解算效果不同，本文主要研究基于Mahony互补滤波算法的姿态解算[3]，对2个坐标系相互转化时旋转矩阵存在偏差进行修正，旋转矩阵的元素是四元数，并于修正后给出了滤波的仿真结果分析。

1 四元数法姿态解算原理

1.1 四元数基本概念

2 Mahony互补滤波融合算法

互补滤波融合算法可根据加速度计测量数据利用梯度下降法得到最小误差四元数的微分，再通过互补滤波算法与陀螺仪得出角速度微分四元数进行融合，对融合后的姿态微分四元数进行积分，估算出最优四元数。

经典互补滤波[8]利用陀螺仪和加速度计各自在动态和静态上的优势，在频域上用加速度计测得的准确瞬时值来弥补陀螺仪随时间积累所产生的漂移误差，即分别加入低通和高通滤波器提高姿态解算的精度[9]。互补滤波的关键在于系数 Kp的选择，经典低通环节阻带衰减较慢，不能满足四旋翼飞行器在飞行中的高动态性，通过对Mahony算法做出改进，最后在Matlab仿真试验和Mission Planner地面站软件[10]的测试下，比较不同的姿态解算结果，调试得出最佳的比例系数Kp值，得到了更好的姿态解算结果。

3 Matlab姿态解算仿真实验

在实验前，研究将进行试验数据的获取，获取飞行所需要的传感器的姿态角数据。首先QGC与Pixhawk进行连接[12]，并设置了多旋翼无人机的自主飞行路径，然后在一段时间的飞行后，提取其飞行数据，主要涉及其未修正前的传感器所测量的姿态角信息，再将其保存在Matlab能够调用的dat数据文件夹中[13]。

这里，首先将特定路径Pixhawk的飞行数据从日志中提取出来，提取的数据是飞行器未经过Mahony算法解算[14]得到的的姿态角数据，Φ、θ、ψ分别为机体的俯仰角、偏航角和翻滚角，然后将这些数据以dat的格式保存起来。将数据加载到已经封装好的Mahony源程序中，传入Matlab中进行仿真实验[15]。

实验中，先后更改了3次Kp值，依次为0.6、1.1、2，得到了3张效果图。分别如图1～图3所示。

4 仿真实验与结果分析

比例系数反映了姿态结果中加速计测量值对姿态结果的修正情况。Kp值越大说明加速度计的结果对姿态解算的结果影响越大，又因为对高频震荡非常敏感，这就有了结果中滚动角和俯仰角震荡频率[16]随着Kp值的增大而增大的情况。所以要不断地进行试验，根据特定的场景确定最适合的Kp值，Kp值并不是越小越好，当Kp值越小时，陀螺仪对滤波结果的影响就较大，就会产生一些低频的错误信息。所以要综合考虑Kp与Ki的值的大小。

由图1～图3中可以得到，当Kp值小于1.1时，虽然滤波滚动角和俯仰角有波动，但波动的频率并不是特别大;当Kp值大于等于1.1时，滤波的航向角波动很大;当Kp值为0.6～1.1之间，无人机自主导航的精确度更佳。

5 结束语

本文阐述了四旋翼飞行器姿态解算常用四元数法姿态解算方法与互补滤波融合算法原理，并研究了影响姿态解算的重要参数（比例系数Kp）的最佳取值。通过对Mahony算法做出改进，最后在Matlab仿真试验和Mission Planner地面站软件的测试下，比较不同的姿态解算结果，调试得出最佳的比例系数Kp值，得到了更好的姿态解算结果。四旋翼飞行器姿态信息的准确性直接影响飞行器的飞行控制效果。实验结果表明，Mahony自适应互补滤波算法滤波效果良好，可以有效降低低成本IUM系统的误差累加，可以在实际应用中得到较高的姿态精度，对飞行器的控制研究具有一定的参考价值与指导意义。

参考文献

[1]赵伟，王志华，周兵. 基于.NET技术和MVC的新架构模式[J]. 计算机工程与设计，2012，33（7）：2646-2651.

[2]高京都. 四旋翼飞行器控制系统的设计与实现[D]. 大连：大连理工大学，2015.

[3]李玉玲. 人工智能在计算机视觉及网络领域中的应用[J]. 电脑编程技巧与维护，2018（8）：156-157，161.

[4]段曉军，吴成富. 嵌入式实时控制系统的快速原型化及其在无人机飞控系统设计与实时仿真中的应用[C]//中国航空学会控制与应用第十一届学术年会.烟台：中国航空学会，2004：530-534.

[5]张锦. 三余度飞控计算机系统软件的研究与设计[D].西安：西北工业大学，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]陈华胄，谌海云. 基于自适应互补滤波的四旋翼飞行器姿态解算[J]. 测控技术，2018，37（6）：81-84，89.

[13]江杰，王康，李刚. 一种四旋翼姿态解算与控制优化方法设计分析[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]洪世杰，李嘉欣，侯佳木，等. 自主巡线四旋翼无人机建模及姿态控制器设计[J]. 现代计算机（专业版），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组合导航系统自适应滤波算法与仿真研究[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.