基于Arduino 的四旋翼飞行器控制系统设计

万云霞，许伦豹，胡 龙，吴佳鑫，王言章

(吉林大学a.地球信息探测仪器教育部重点实验室;b.仪器科学与电气工程学院，长春130026)

基于Arduino 的四旋翼飞行器控制系统设计

万云霞a，b，许伦豹b，胡 龙b，吴佳鑫b，王言章a，b

(吉林大学a.地球信息探测仪器教育部重点实验室;b.仪器科学与电气工程学院，长春130026)

为改善滤波效果，针对四旋翼飞行器滤波算法计算量大的问题，采用基于 Kalman与 DMP (Digital Motion Processing)滤波相结合的姿态数据处理算法及 PID(Proportion-Integration-Differentiation)姿态控制算法，设计了四旋翼飞行器控制系统。系统硬件由Arduino控制板及四旋翼飞行器平台组成，在此平台基础上建立了飞行器动力学模型并对Kalman滤波器及PID控制器参数进行调试。实际飞行结果表明，该系统能对飞行姿态的偏移进行快速调整，调整灵敏度和稳态时间得到明显改善，有效地完成对四旋翼飞行器的稳定控制。

四旋翼飞行器;Kalman滤波;DMP滤波;PID控制器;姿态控制

0 引 言

四旋翼飞行器是由4个转子推动飞行的直升机，其方向和速度的控制是通过改变每个转子的相对速度实现的。从Breguet-Richet发明世界上第1架四旋翼飞行器至今，四旋翼飞行器发展了一个多世纪，但由于结构和操作技术的限制，大型四旋翼飞行器却没有得到快速发展。近年来随着飞行控制理论、微惯导(MIMU:Micro Inertial Measurement Unit)、微机电(MEMS:Micro Electro Mechanical System)以及新型材料等技术的进步，微小型四旋翼飞行器的发展愈发迅速［1］。针对四旋翼的姿态数据处理及控制问题出现了许多滤波算法和控制理论算法，例如高斯滤波，互补滤波，卡尔曼滤波等。但对于软件滤波仍存在降低控制速度和数据稳定度不理想的问题;同时反步(Backstepping)控制，线性二次型最优控制(LQR:Linear Quadratic Regulator)，滑模(Sliding Mode)控制等控制算法也可应用于飞行器控制。但因计算量大且较为繁琐，对处理器要求较高，故笔者提出了简易Kalman算法与硬件滤波相结合的滤波措施和简单的PID(Proportion-Integration-Differentiation)控制算法，设计了基于Arduino的四旋翼飞行器控制系统。

1 飞行器姿态控制原理

以飞机中心为原点建立X-Y-Z坐标系(见图1)，机身绕X轴转过的角度称为翻滚角(Roll)，绕Y轴转过的角度称为俯仰角(Pitch)，绕Z轴转过的角度称为偏航角(Yaw)，这3个角度统称为欧拉角(φ，φ，θ)。当同一轴上两电机的转速不同时，就会产生倾角，其大小取决于同轴电机的转速差。四旋翼姿态控制是通过对3个角度的调节实现的，增加高度控制后，就可完成简单的飞行任务。

四旋翼有“+”和“x”两种飞行方式，为了操作简便，笔者采用十字飞行的方式。图2中1、3电机顺时针转动，同时会产生逆时针的扭矩;2、4电机逆时针转动，同时产生顺时针的扭矩，当两轴的扭矩相同而抵消时，四旋翼将不会绕Z轴旋转。

如图2f所示，当增大电机1的转速而减小电机3的转速时，飞行器将会向后飞行(沿箭头所指方向运动)。如果4个电机的转速同时增大(或减小)相同的转速，四旋翼将会爬升(或下降);当飞行器转速所产生的升力与其自身重力相同时，飞行器将会处于悬停状态。通过控制4个电机的转速可实现对四旋翼飞行方向的控制，图2给出了各种飞行方向的转速控制图。

图1 四旋翼飞行器原理图Fig.1 Four-rotor aircraft flight principle photo

图2 四旋翼各种飞行图示Fig.2 Illustration of the variousmovements of a quad-rotor

2 飞行器结构组成

四旋翼飞行器的飞行控制系统通常由传感器测量装置、主控制器、电机驱动装置及电机和螺旋桨等部分组成。传感器用来测量飞行器的姿态数据，主控制器根据设定姿态对该数据进行处理，经过控制算法最终以4路PWM(Pulse-Width-Modulation)信号控制电机转速并校正飞行角度，以实现自动调节旋转力矩稳定飞行姿态。图3、图4分别为四旋翼飞行器控制构架和飞行器实物图。

图3 四旋翼飞行器控制架构Fig.3 Control architecture of four-rotor aircraft

图4 四旋翼飞行器实物图Fig.4 Quad-rotor

2.1 传感器测量装置

为获得四旋翼姿态数据，系统选用MPU-6050作为姿态传感器，获取3轴的角速率和线性加速度。该传感器为全球首例6轴运动处理传感器，将一个3轴MEMS陀螺、一个3轴MEMS加速度计以及一个数字运动处理器DMP(Digital Motion Processor)集成一体，其中DMP自带硬件滤波器，通过编程得到稳定的姿态数据［2］。

2.2 主控制器

图3所示的控制算法是在基于Arduino硬件平台上开发完成的。系统选用Arduino Uno R3作为主控制器，该平台核心处理器采用ATmega328，具有14路数字输入/输出口(其中6路作为PWM输出)，6路模拟输入，串口、I2C和SPI通信接口以及10位内置A/D，能满足系统设计要求［3］。

2.3 电源及电机驱动

四旋翼飞行器的电机需要满足长时间高速旋转的要求，一般选择无刷直流电机(BLDC:Brushless Direct Current)，可运用数字变频控制技术进行控制并具有较大的可调转速范围(1～10 000 rad/s)，噪音小且维护方便［4］。无刷直流电调(ESC:Electronic Speed Control)用来驱动无刷直流电机的驱动器件，并根据控制信号(PWM波占空比)的变化调节电机的转速。飞行器动力电源选用输出电压为11.1 V的3 s锂铁电池，能满足电调对大输出电流的要求且携带方便。另外螺旋桨选用5045规格的正反桨。

3 数学模型

3.1 模型建立

四旋翼飞行器共有4个固定的推力角度转子，代表4个由相应螺旋桨产生的输入力量。对四旋翼的控制实际是对欧拉角(φ，φ，θ)以及空间坐标系中的3个方向上的位移(x、y、z)控制。可以看出，系统只有4个输入输出力而却有6个输出控制状态(x、y、z轴，φ，φ，θ)，因此，四旋翼飞行器是个六自由度的欠稳定系统［4］。

为了建立不失一般性的动力学模型，对四旋翼飞行器做出以下4点假设:1)把四旋翼飞行器当做均匀、对称的刚体;2)惯性坐标系E的原点位置与四旋翼飞行器的几何中心、质心重合;3)四旋翼飞行器所受的阻力与重力不受高度、区域的影响而保持不变;4)螺旋桨所产生的力与电机转速的平方成正比。此外为了对四旋翼飞行器的空间位置进行精确控制建立方向余弦转换矩阵［5］

地面坐标为笛卡尔直角坐标系［5］。其中φ为翻滚角、φ为偏航角、θ为俯仰角。

利用力与力矩平衡关系的运动方程以及拉格朗日方法可将飞行器的动力学模型简化为如下形式［6］

其中g为重力加速度，m为飞行器的总质量，K1、K2与K3为飞行器各方向上的阻力系数，当转速较低时可忽略飞行器所受的阻力。

假定飞行器的重心在设定原点处向上(或向下)移动了d个单位，从而降低了角加速度对力敏感程度，因而使系统的稳定性得到提高。为方便运算，用以下方程式表达飞行器各个方向上的输入力矩

其中U1为4个电机总共产生的垂直升力，U2为俯仰通道所受力矩，U3为偏航通道所受力矩，U4为滚动通道所受力矩，Hi(i=1，2，3，4)为飞行器上相应电机产生的推力，Ii(i=1，2，3)为以飞行器自身建立的坐标系的3个轴上所受的转动惯量。飞行器的欧拉角表示如下［6］

其中l为飞行器支架的半轴长度，Ki(i=4，5，6)代表阻力系数，分别以X，Y，Z和φ为一组控制输出。飞行器控制系统数学模型框如图5所示。

图5 四旋翼飞行器控制系统数学模型框图Fig.5 Block diagram modeling of four rotor-craft control system

3．2 PID系统状态控制方程

由于对飞行器进行仿真需要整体的系统传递函数，通过对四旋翼进行数学建模已得到相关输入、输出的变量关系，设状态空间方程的形式如下

其中U=(U1，U2，U3，U4)T为输入矢量，X=(x，y，z，˙x，˙y，˙z，θ，φ，φ，˙θ，˙φ，˙φ，g)T为状态矢量，Y=(x，y，z，φ，θ，φ)T为输出量，则由数学模型可得系统的状态空间方程的各矩阵如下［6］

将对飞行器进行测量得到的参数带入式(11)并用Matlab求出相应状态空间方程的传递函数，由于笔者仅针对四旋翼飞行器的悬停状态进行仿真，故θ=φ≈0，传递函数可简化为

4 姿态数据处理及系统控制

4．1 滤波处理

飞行器的姿态数据质量直接影响系统控制精度，故采用硬件与软件相结合的方案实现对噪声较大且不稳定的姿态数据进行滤波处理。硬件滤波采用MPU-6050自带的数字运动处理器(DMP:Digital Motion Processing)，它可接收并处理来自陀螺仪、加速度计的数据，处理结果可直接从DMP寄存器读出，且比从一般的加速度计与陀螺仪读出的数据更加稳定，减少了飞行过程中因震动和外部环境所产生的噪声。

为了进一步提高数据的可靠性，对数据进行软件滤波。卡尔曼滤波算法能在一系列受噪声干扰的数据中准确估计动态系统的状态。卡尔曼滤波算法具有编程简单、能对现场采集的数据进行实时更新和处理的优点，因而采用卡尔曼滤波算法对采集的数据进行融合得到正确的姿态角，从而更有利于对飞行器进行控制。

卡尔曼滤波算法具体的实现流程如图6所示［7］。

图6 卡尔曼滤波算法流程图Fig.6 Flowchart of Kalman filter algorithm

图7 为滤波后的示意图，在静态时测得，X轴角速率漂移较大，加速度受噪声干扰强;经过DMP与卡尔曼算法滤波后，角度数据稳定在-0.01°附近，信噪比与稳定性得到较大改善。

图7 数据处理对比图Fig.7 Data processing comparison chart

4.2 姿态控制算法

经过硬件、软件滤波后的姿态数据，其信噪比与数据质量有明显改善，有利于进行姿态控制。该系统的姿态控制算法采用PID控制。PID控制器是一种线性控制器。它由被控量的期望值r(t)与实际输出值y(t)构成控制偏差e(t)，然后将偏差e(t)按比例(P)、积分(I)、微分(D)的关系通过线性组合构成控制量u(t)去控制被控对象［8，9］，其输出和输入之间关系可描述为

其中Kp为比例系数，Ti为积分时间常数，Td为微分时间常数。

系统控制算法以角速率为内环反馈和角度为外环反馈组成的内外环嵌套控制实现对飞行姿态调整［10］。在对PID参数进行调试实验中，采用PI(Proportion-Integration)控制器比PID控制器的控制效果更好。PID姿态控制器原理框图如图8所示。

控制器外环是角度的PI控制，内环是角速率的PI控制。期望角度与传感器测得的实际角度经比较后，误差经控制器处理并以此输出量作为内环的输入量，该输出量与传感器测得的角速率进行比较后得到最终输出值。经过转换后以PWM信号作为控制量控制电机的转速。其中内环的稳定是实现四旋翼稳定飞行的关键因素。在实际调试中只用P时外环往往也能实现较好的角度控制。

在实现高度控制时可以高度进行粗略控制和精确锁定，在高度控制的基础上可实现飞行器的悬停控制。精确PID高度控制器框图如图9所示。

图8 PID姿态控制器原理框图Fig.8 PID Controller of attitude

图9 精确高度PID控制器Fig.9 Precise PID controller of height

精确高度PID控制系统中，飞行器高度、垂直方向的速度和Z轴方向的加速度为需要的实测值。其中加速度控制器执行速率为100 Hz，速率控制器执行速率为50 Hz。而在一般飞行中不需要对高度进行精确控制，只需四旋翼维持在设定的高度，所以可采用图10所示的简单控制方式。

图10 简单高度PID控制器Fig.10 Simple PID controller of height

5 系统仿真

图11 单通道PID控制流程图Fig.11 Flow of signal-channel of PID control

由图12可见，当参数值较合适时系统响应时间较短、超调较低、延迟时间较短，从而能保证四旋翼飞行器的姿态角与期望角有偏差时能通过PID调节迅速地达到期望姿态。若需要更加快速地调节则需更细致地改变参数观察响应曲线。

6 结 语

图12 俯仰通道阶跃响应Fig.12 The step response of pitch channel

笔者完成了四旋翼飞行器控制结构的设计，通过DMP与Kalman滤波算法提高了PID处理数据的信噪比，并在系统建模的基础上对飞行器的姿态控制与高度调节进行了设计。系统仿真证明了PID控制的实效性，实际飞行结果表明了该控制系统在四旋翼飞行器姿态控制方面的有效性。笔者所进行的理论研究与实验结果为四旋翼飞行器的自稳定控制提供了有价值的参考。

［1］聂博文，马宏绪，王剑，等.微小型四旋翼飞行器的研究现状与关键技术［J］.电光与控制，2007，14(6):113-117. NIE Bowen，MA Hongxu，WANG Jian，etal.Small Quad-Totor Aircraft Present Research Situation and Key Technology［J］. Electronics Optics＆Control，2007，14(6):113-117.

［2］赖义汉，王凯.基于 MPU6050的双轮平衡车控制系统设计［J］.河南工程学院学报:自然科学版，2014，26(1):53-57. LAIYihan，WANG Kai.Two-Wheel Balancing Car Control System Based on MPU6050［J］.Journal of Henan Institute ofEngineering:Natural Science Edition，2014，26(1):53-57.

［3］蔡睿妍.Arduino的原理及应用［J］.电子设计工程，2012，20(16):155-157. CAIRuiyan.The Principle and Application of Arduino［J］.Electronic Design Engineering，2012，20(16):155-157.

［4］李秀英，刘彦博.基于PWM的四旋翼飞行器控制方法［J］.吉林大学学报:信息科学版，2011，29(5):464-472. LIXiuying，LIU Yanbo.Control Method of Quad-Totor Based on PWM［J］.Journal of Jilin University:Information Science Edition，2011，29(5):464-472.

［5］国倩倩.微型四旋翼飞行器控制系统设计及控制方法研究［D］.长春:吉林大学通信工程学院，2013. GUO Qianqian.Research of Micro Quad in Design and Control Method［D］.Changchun:College of Communication Engineering，Jilin University，2013.

［6］MIAN A A，DAOBOW.Modeling and Backstepping-Based Nonlinear Control Strategy for a 6 DOFQuadrotor Helicopter［J］. Chinese Journal of Aeronautics，2008，21(3):261-268.

［7］王帅，魏国.卡尔曼滤波在四旋翼飞行器姿态测量中的应用［J］.兵工自动化，2011，30(1):73-80. WANG Shuai，WEIGuo.Application of Kalman Filter in the Attitude Measurement of Quad-Rotor［J］.Ordnance Industry Automation，2011，30(1):73-80.

［8］王素青，姜维福.基于MATLAB/Simulink的PID参数整定［J］.自动化技术与应用，2009，28(3):24-25. WANG Suqing，JIANGWeifu.PID Parameters Tuning Based on MATLAB/Simulink［J］.Industrial Control and Application，2009，28(3):24-25.

［9］段世华.四旋翼飞行器控制系统的设计和实现［D］.成都:成都电子科技大学自动化工程学院，2012. DUAN Shihua.Design and Implementation of Quad Control System［D］.Chengdu:School of Automation Engineering，Chengdu Electronic Technology University，2012.

［10］LIM H，PARK J，LEE D，et al.Build Your Own Quadrotor:Open-Source Projects on Unmanned Aerial Vehicles［J］. Robotics＆Automation Magazine，IEEE，2012，19(3):33-45.

［11］PATEL V K R S，PANDEY A K.Modeling and Performance Analysis of PID Controlled BLDCMotor and Different Schemes of PWM Controlled BLDCMotor［J］.International Journal of Scientific and Research Publications，2013，3(4):1-14.

(责任编辑:刘俏亮)

Design of Quad-Rotor Control System Based on Arduino

WAN Yunxiaa，b，XU Lunbaob，HU Longb，WU Jiaxinb，WANG Yanzhanga，b

(a.Key Laboratory for Geophysical Instrumentation of Ministry of Education; b.College of Instrumentation and Electrical Engineering，Jilin University，Changchun 130026，China)

In order to adjust the flightattitude of the quad-rotor，an attitude data processing jointalgorithm based on Kalman and DMP(Digital Motion Processing) filter was adopted，and PID(Proportion-Integration-Differentiation)algorithm was used to control its flight attitude.Based on the hardware platform，the dynamic model of the aircraftwas established and the parameters of Kalman filter and PID controller were debugged.The flying experiment shows that after using the control algorithm，the control system can quickly adjust the flight attitude offset to control the quad-rotor effectively.

quad-rotor; Kalman filtering; digital motion processing(DMP); proportion-integrationdifferentiation(PID)controller;attitude control

TP273

A

1671-5896(2015)04-0389-08

2015-03-11

吉林省重大科技攻关基金资助项目(20140203015GX);吉林大学“大学生创新创业训练计划”基金资助项目(2014B65250)

万云霞(1980— )，女，山东烟台人，吉林大学工程师，主要从事地球探测仪器、数字信号处理研究，(Tel)86-1894361464 (E-mail)wangyx@jlu.edu.cn。