基于Mahony滤波器和PID控制器的四旋翼飞行器姿态控制

黄坡　马艳　杨万扣

摘要：四旋翼飞行器由于其简单的气动布局和复杂的动力学模型在控制领域引起了研究热潮，姿态估计与控制器设计一直是实现四旋翼飞行器稳定飞行的难点。为实现精确的四旋翼飞行器姿态估计，首先分析了IMU传感器示值组成和误差存在的原因，然后在方向余弦矩阵（DCM）和重正交化的基础上，具体给出了Mahony滤波器的实现流程。通过与扩展卡尔曼滤波器对比表明，该算法不仅能保证很高的姿态估计精度，而且计算时间小于扩展卡尔曼滤波器，有助于提高系统姿态估计的实时性。结合Mahony滤波后的姿态信息，采用嵌套PI-PID控制策略设计了控制器。最后，将姿态估计算法和控制算法应用到实验平台上，可以实现四旋翼飞行器悬停和角度跟踪功能。

关键词：四旋翼飞行器；Mahony滤波器；姿态估计；DCM；嵌套PI-PID控制器

中图分类号：TP273 文献标识码：A 文章编号：1009-3044（2014）07-1611-07

近年来，随着MEMS传感器，嵌入式微处理器以及无刷直流电机技术上的突破，四旋翼飞行器研究获得了飞速发展[1]。四旋翼飞行器在民用和军用方面具有广阔的应用前景，在一些人类无法涉及或应该避免的环境中，比如自然灾害和工业灾害，可以由它来代替人类完成工作；另外它的低空预警和轻物运输功能，具备很大商业潜力和商业价值，目前正被逐渐地投入使用。

虽然四旋翼飞行器的结构和原理并不复杂，但是传感器信息获取和控制方法选用却带来了较大的难度[2]。四旋翼飞行器作为一个非线性强耦合系统，有四个输入六个输出。独立输入数量小于输出自由度数量，因此它属于欠驱动系统。输出的6个自由度包括位置和姿态角度（俯仰角，滚转角，偏航角）。实现四旋翼飞行器的自主飞行需要实现位置控制与姿态控制。姿态控制是四旋翼飞行器实现各项工作的核心，位置控制建立在完善的姿态控制的基础之上。因此本文关心的主要是姿态控制。

获取准确的姿态角度值是研究四旋翼飞行器的一个重要方面。对陀螺仪积分或者依据加速度计得到姿态角尽管是一种直观的想法，但是陀螺仪工作一段时间会出现零点漂移，积分带来的误差随时间将越来越大，而加速度计则更容易受到噪声或者震动的干扰，因此依据两种方式获取的姿态角必定与真实的姿态角相差很大。如上所述，单用传感器无法获得准确的姿态角信息，必须结合滤波算法对姿态角进行估计。

为整体上解决四旋翼飞行器的姿态控制，该文提出了基于Mahony滤波器[3-4]的姿态估计和嵌套PI-PID[5]控制器。首先对各传感器的示值组成和特性进行了分析，在此基础上，根据Mahony滤波器数学原理，给出了基于方向余弦矩阵（DCM）的滤波器详细实现。Mahony滤波器运用了互补滤波器的思想，但增加了旋转角速度修正环节，在本文中，为了得到更加准确的结果，增加了对每次计算得到的DCM重新正交化这一环節。对比卡尔曼滤波器[6-7]，由于卡尔曼滤波器基于复杂矩阵运算，计算时间过长将导致姿态角更新时间变长，容易出现延时，Mahony滤波器在良好的姿态估计基础上耗费时间更少，因此实时性更好。接着，姿态控制器采用了嵌套PI-PID控制器，尽管有很多非线性的方法被提出运用到四旋翼飞行器，但是由于模型的不确定性，它们实际表现反而不如PID控制器好。嵌套PI-PID控制器由一个速率控制内环和一个位置控制外环组成。

1 四旋翼飞行器的气动原理

四旋翼飞行器结构一般呈十字型，为获得较轻机体质量，宜采用碳纤维作为材料。十字型端点处各有一个用电机驱动的旋翼，相对旋翼旋转方向一致，如图1所示，若前后旋翼顺时针（逆时针）旋转，则左右旋翼逆时针（顺时针）旋转。

2 基于Mahony滤波器的姿态估计

5 总结与展望

本文提出基于Mahony滤波器的姿态估计和嵌套PI-PID控制器设计。首先设计了Mahony滤波器，不仅可以得到准确的姿态估计，而且计算耗费时间对比卡尔曼滤波器更少，对实时迅速地控制飞行器姿态变化非常有利。在良好的姿态估计基础上，控制器设计部分采用嵌套PI-PID控制，最终，四旋翼飞行器实现悬停和姿态跟踪。后续的工作包括在姿态控制的基础上，实现位置速度控制和路点飞行。

参考文献：

[1] Mahony R，Kumar V，Corke P.Multirotor Aerial Vehicles Modeling， Estimation and Control of Quadrotor[J]. IEEE Robotics & Automation Magazine，2012，19（3）： 20-32.

[2] 王伟，马浩，孙长银.四旋翼姿态控制系统设计[J].科学技术与工程，2013，13（19）：5513-5519.

[3] Mahony R，Hamel T，Pflimlin Jean-Michel.Complementary Filters on the Special Orthogonal Group[C].IEEE Transactions on Automatic Control，Europe，2008，53（5）：1203-1218.

[4] Mahony R，Hamel T，Pflimlin Jean-Michel.Complementary filter design on the Special orthogonal group SO（3）[C].2005 The 44th IEEE Conference on Decision and Control. Spain， Serville，2005：1477-1484.

[5] BouabdallaH S，Noth A，Siegwart R.Techniques Applied to an Indoor Micro Quadrotor[C].Proceedings of 2004 IEEE International Conference on Intelligent Robots and Systems，Japan， Sendal，2004.3：2451-2456.

[6] Hoffmann F，Goddemeier N，Bertram T.Attitude estimation and control of a quadrotor[C].2010 IEEE International Conference on Intelligent Robots and Systems，Taiwan， Taipei，2010：1072-1077.

[7] Chan A L，Tan S L，Kwek C L.Sensor data fusion for attitude stabilization in a low cost Quadrotor system[C]. 2011 IEEE15th International Symposium on Consumer Electronics.Singapore，2011： 34-39.

[8] Hamel T，Mahony R.Attitude estimation on SO（3）based on direct inertial measurements[C]. Proceedings of 2006 IEEE International Conference on Robotics and Automation. Florida， Orlando，2006：2170-2175.

[9] Diebel J.Representing Attitude： Euler Angles， Unit Quaternions， and Rotation Vectors[EB/OL].[2006-10-20]. http：//citeseerx.ist.psu.edu/viewdoc/download？doi=10.1.1.110.5134&rep=rep1&type=pdf.