基于四元数的四轴飞行器姿态控制

荆学东 潘翔

摘 要： 为了实现四轴飞行器的姿态控制，建立四轴飞行器四元数运动学方程，给出了四元数微分方程的解析解和數值解，在此基础上求解出欧拉角。四轴飞行器采用串级PID控制算法，以欧拉角作为系统外环，补偿后的角速度作为系统内环。通过Matlab/Simulink仿真，对比单级PID控制效果，验证了算法的可靠性。最后，搭建了飞行器试验平台，在STM32飞控板上编程实现算法。实验证明，该控制系统较单级PID具有响应速度快，超调量小等优点，基本满足室外飞行要求。

关键词： 四轴飞行器； 四元数法； PID控制算法； 串级PID； 姿态控制； 微分方程

中图分类号： TN967.6?34； TP273 文献标识码： A 文章编号： 1004?373X（2018）16?0116?04

Abstract： A quaternion kinematic equation for the quadrotor aircraft is established， the analytical solution and numerical solution of the quaternion differential equation are given， and the Euler angle is solved on this basis， so as to realize attitude control of the quadrotor aircraft. The cascade PID control algorithm is used for the quadrotor aircraft， taking the Euler angle as the outer ring of the system and angular velocity after compensation as the inner ring of the system. The reliability of the algorithm is verified by using the Matlab/Simulink simulation to compare the effects of single?level PID control and cascade PID control. The aircraft test platform is established. The algorithm is implemented by means of programming on the STM32 flight control board. The experimental results show that in comparison with the single?level PID， the control system has the advantages of faster response speed and less overshoot， which can basically meet the requirements of outdoor flight.

Keywords： quadrotor aircraft； quaternion method； PID control algorithm； cascade PID； attitude control； differential equation

四轴飞行器与普通的飞行器相比具有结构简单、故障率低及单位体积能够产生更大升力等优点，在军事、民用和科技领域发挥着越来越重要的作用，非常适合在狭小空间内执行任务。因此四旋翼飞行器具有广阔的应用前景，吸引了众多科研人员，成为国内外新的研究热点。

本文通过建立四轴飞行器四元数运动学方程，得到该方程的数值解，再将四元数转换为欧拉角，避免了欧拉角法运算量大、存在奇异点的问题。在控制算法方面，采用串级PID控制，增加了系统的可靠性和鲁棒性。通过Matlab/Simulink仿真，并搭建四轴飞行器实验平台，验证了算法的正确性。

1 姿态描述与姿态角表示

1.1 四轴飞行器姿态描述

为了更好地描述四轴飞行器的姿态，建立如图1所示的坐标系，分别为参考坐标系n系、机体坐标系b系。参考坐标系的原点为机体的旋转中心，机体坐标系的原点为机体的中心。

从仿真图可以看出，俯仰通道阶跃响应上升时间为0.7 s，超调量为22%，在1.2 s达到稳定，在调节过程中出现震荡，震荡的幅度较小频率较低，曲线稳定后没有震荡。单级PID自动控制效果不理想，为达到更理想的控制效果，本文采用串级PID控制，增强系统的稳定性和鲁棒性。

3.2 串级PID控制

控制系统在只用角度单环的情况下，系统很难稳定运行，本文加入加速度作为内环，角速度由陀螺仪和加速度计互补滤波得出。四轴飞行器串级PID控制流程图如图3所示，其中角度作为外环，角速度作为内环。

从仿真图可以看出，俯仰通道阶跃响应上升时间为0.25 s，超调量为2%，在0.35 s达到稳定。通过仿真图对比可知，在串级PID控制下，控制系统响应速度快，超调量明显减小，且没有出现震荡，比单级PID控制更加稳定可靠，达到了良好的控制效果。

4 实 验

考虑到仿真结果与实际飞行情况的差异，为验证双闭环PID控制算法的可行性，搭建了基于STM32微控制器的四轴飞行器，调试确定了各个参数见表1，并进行了室外飞行实验，飞行效果良好，如图5所示。

5 结 论

本文通过建立四轴飞行器四元数运动学微分方程，解得微分方程的数值解，从而得到四轴飞行器的欧拉角。在控制方面，对比了单级PID和串级PID控制的仿真效果，单级PID存在明显的超调，经过较长时间才能稳定；而串级PID响应速度快，超调量小，稳定所需时间短，满足四轴飞行器控制的基本要求。

参考文献

[1] 何瑜.四轴飞行器控制系统设计及其姿态解算和控制算法研究[D].成都：电子科技大学，2015.

HE Yu. The design of quadrotor aircraft control system and research of its attitude solving & control algorithm [D]. Chengdu： University of Electronic Science and Technology of China， 2015.

[2] 杨成顺.多旋翼飞行器建模与飞行控制技术研究[D].南京：南京航空航天大学，2013.

YANG Chengshun. Research on modeling and flight control technology of multi?rotor aircraft [D]. Nanjing： Nanjing University of Aeronautics and Astronautics， 2013.

[3] 刘豹，唐万生.现代控制理论[M].3版.北京：机械工业出版社，2006.

LIU Bao， TANG Wansheng. Modern control theory [M]. 3rd ed. Beijing： China Machine Press， 2006.

[4] 秦永元.慣性导航[M].2版.北京：科学出版社，2014.

QIN Yongyuan. Inertial navigation [M]. 2nd ed. Beijing： Science Press， 2014.

[5] 张承岫，李铁鹰，王耀力.基于MPU6050和互补滤波的四旋翼飞控系统设计[J].传感技术学报，2016，29（7）：1011?1015.

ZHANG Chengxiu， LI Tieying， WANG Yaoli. Design of quadrotor aircraft flight control system based on MPU6050 and adaptive complementary filter [J]. Chinese journal of sensors and actuators， 2016， 29（7）： 1011?1015.

[6] 陈航科，张东升，盛晓超，等.四旋翼飞行器悬停状态姿态控制建模与仿真[J].计算机仿真，2013，30（11）：41?45.

CHEN Hangke， ZHANG Dongsheng， SHENGXiaochao， et al. Modeling and attitude control simulation for quadrotor aircraft in hover [J]. Computer simulation， 2013， 30（11）： 41?45.

[7] 龚瑞.微型四旋翼飞行器飞行控制系统设计与验证[D].南京：南京航空航天大学，2015.

GONG Rui. Design and verification of flight control system for miniature quadrotor helicopter [D]. Nanjing： Nanjing University of Aeronautics and Astronautics， 2015.

[8] 李世光，王文文，申梦茜，等.基于STM32的姿态测量系统设计[J].现代电子技术，2016，39（9）：12?14.

LI Shiguang， WANG Wenwen， SHEN Mengxi， et al. Design of attitude measurement system based on STM32 [J]. Modern electronics technique， 2016， 39（9）： 12?14.

[9] DERAFA L， BENALLEGUE A， FRIDMAN L. Super twisting control algorithm for the attitude tracking of a four rotors UAV [J]. Journal of the Franklin Institute， 2012， 349（2）： 685?699.

[10] ZHAN Q， WANG J， XI X. Control system design and experiments of a quadrotor [C]// Proceedings of IEEE International Conference on Robotics and Biomimetics. Guangzhou： IEEE， 2012： 1152?1157.