四旋翼飞行器力学模型与控制系统设计

王史春

(1.台州职业技术学院电气信息学院，浙江台州318000;2.工业控制技术国家重点实验室浙江大学智能系统与控制研究所，浙江杭州310027)

0 引言

四旋翼飞行器能够自由悬停和垂直起降，结构简单，易于控制，这些优势决定了它具有广泛的应用领域［1］，在民用、医疗、军事等领域都有着无限的潜力.在民用领域，它可以进行航拍，以得到在地面难以测量和计算的数据.在医疗领域，四旋翼直升机可以进入普通地面机器人难以到达的地区进行搜救等活动，最大程度地避免了人员财产的损失.在军用方面，四旋翼直升机可以作为侦察使用，它飞行灵活、稳定;同时，若在四旋翼直升机上增加其他机械装置，则可以利用它完成更加复杂和重要的任务.然而，作为一个MIMO非线性系统［2］，四旋翼飞行器输入变量与输出变量之间的耦合作用，时变非线性的动力学特征，系统本身的不确定性及外部的干扰等的引入，使得系统的控制问题变得十分复杂.如何能够设计出有足够的飞行动力并且具有良好稳定性的控制系统［3-4］，是四旋翼飞行器当今面临的主要问题;如何协调电动机的控制和飞行器的控制，成为四旋翼飞行器设计的关键.

近几年来，国外一些知名研究机构扩展了四旋翼飞行器的研究领域，希望其在无GPS信号的室内环境中可以利用一些特定的传感器数据进行导航.在特定环境中，拥有特定功能的飞行器，对四旋翼飞行器系统有特殊的要求，所以开发一个稳定的飞行控制系统是非常必要的.而国内对于四旋翼飞行器飞行控制系统的研究起步较晚，一些稳定的飞行控制系统都被商品化，无法对其系统进行改进，这给人们的研究带来很多不便，因此需要开发一款属于自己的飞行控制系统.首先对飞行器控制系统进行动力学建模［5］，再根据四旋翼飞行器的性能要求和后续应用性开发的要求，对四旋翼飞行器进行系统设计，包括软硬件的设计，软件的设计和上位机的开发，最后通过实际的飞行测试飞行器的飞行效果.

1 飞行动力学建模

1.1 建立坐标系

为了确切描述四旋翼飞行器的飞行状态，需要选取合适的坐标系，本文选择了以下两个坐标系，机体坐标系B(Oxyz)，地面坐标系E(OXYZ)［6］.

为了描述两个坐标系的转换关系，定义了3个欧拉角:横滚角φ，俯仰角θ和偏航角ψ，分别表示机体绕 x，y，z轴旋转到X，Y，Z轴的角度，这3个角构成飞行器的姿态角，如图1所示.

图1 姿态角示意图Fig.1 Schematic diagram of attitude angle

从图1中可以得到机体坐标系到地面坐标系每个轴的转换矩阵.

此时机体坐标系B到地面坐标系E的转换矩阵为

将式(1)～(3)代入式(4)中可得到式(5).

1.1.1 模型假设

1)飞行器是刚体且对称;

2)地面坐标系为惯性坐标系，重力加速度不随飞行高度的变化而变化;

3)不计地球自转和公转运动的影响;

4)4个螺旋桨轴与Z轴平行排列;

5)机体坐标系原点与质心一致;

6)忽略空气阻力.

1.1.2 建模过程

根据模型假设，4个螺旋桨轴都能很好地与Z轴平行排列，定义4个螺旋桨提供的升力分别为F1，F2，F3，F4.在机体坐标系下，飞行器的受力为

将坐标转换矩阵(5)代入，可得到在地面坐标系中飞行器的受力

在地面坐标系下，飞行器的质心运动方程为

对于飞行器的角位移运动方程，采用1.2中提到的拉格朗日力学方法来推导，具体推导过程参见文献［7］.飞行器的动能表示为

飞行器的势能表示为

由 Lagrange方程［8］，有 L=T －V，取广义坐标 q=(φ，θ，ψ)，其对应的广义力为

分别代入到Lagrange方程中，可得飞行器的角位移运动方程

定义四旋翼飞行器子系统的4个输入量为

式中:U1为垂直速度控制量;U2为横滚控制量;U3为俯仰控制量;U4为偏航控制量.此时将式(8)和(11)结合起来，可得到飞行器的动力学模型

从式(13)可以看出，系统的模型是由线运动和角运动两个子系统构成的，而角运动影响线运动，但是线运动不影响角运动，还可以通过分别调节控制作用U1，U2，U3，U4来单独控制 ¨z，φ¨，θ¨，ψ¨.

2 系统设计

本实验室所设计的四旋翼飞行控制系统［9］的目标是设计出一套完整的四轴飞行器飞行控制系统，通过硬件和软件实现控制系统，从而实现我们的目标.期望通过该飞行控制系统，可以让四旋翼飞行器在近地环境下能够完成垂直起降、悬停、任意旋转一定的角度等动作，并在对其施以外界干扰时能自动调节并迅速恢复到平稳状态，最后在此基础上通过测试.

2.1 硬件设计

整个系统硬件［10］部分根据模块化思想进行设计，结构框图如图2所示.

图2 系统硬件结构框图Fig.2 The hardware structure diagram of the system

从图2可以看出，整个系统主要包含的模块有遥控器信号捕捉模块，IMU(姿态测量模块)，PID控制器模块，PWM输出模块，IIC模块和USART(无线串口)模块.下面简要介绍每个模块的功能:

1)遥控器信号捕捉模块.这个模块主要用来捕捉遥控器的PWM波信号.

2)IMU(姿态测量模块).该模块包括3个传感器，共有9组数据.这个模块和STM32连接的接口是IIC接口，9组数据通过基于互补滤波的姿态解算算法，算出当前飞行器的姿态，然后和上一个模块给出的姿态设定值做差，得到姿态误差.

3)PID控制器模块［11］.该模块由软件生成，采用分段PID串级控制.使用串级控制的好处是可以很好地消除外部扰动，之所以使用分段PID是因为误差较小时，所需要的控制作用较小，而误差较大的时候所需要的控制作用也较大.该模块的输入是姿态误差，输出是控制量对应的4个电机的 PWM［12］输出.

4)PWM输出模块.利用STM32自带的PWM生成器直接产生需要的PWM波，PWM波的参数主要是周期与占空比.

5)IIC模块.之所以加上这个模块，是因为STM32［13］这块芯片的硬件IIC使用时不能满足使用要求，笔者开发的时候是根据时序自己编写软件模拟IIC来解决这个问题的.

6)USART(无线串口)模块.这个模块主要是用于调试PID参数和姿态数据实时地回传给上位机，里面主要涉及的是通信协议的编写.

2.2 软件设计

四旋翼飞行器飞行控制系统软件任务划分为以下几部分:

1)初始化任务.完成系统、外设及相关数据变量的初始化工作.

2)数据通讯任务.通过点对点线线模块完成数据的下传、上传.

3)遥控器信号捕捉任务.通过对遥控器信号的解析以及一定的时序完成对遥控器信号的捕捉.

4)数据采集任务.完成陀螺仪、加速度计及磁力计的数据采集工作(超声波).

5)定时器定时及PWM输出.需要利用一个定时器对控制周期进行定时，同时利用4个定时器来产生PWM波对电调进行控制，继而控制电机.

6)姿态解算任务.对采集到的9组数据利用一定的滤波算法融合在一起，进而实现对飞行器姿态的解算.

7)控制律解算任务.采用PID控制律，在每个控制周期内对PWM波的输出进行更新，即对电机的控制量更新.本飞行控制系统是一个控制姿态的闭环系统，而不是控制电机转速的闭环系统.

2.2.1 飞行控制系统软件流程

本系统的软件开发环境是IAR Embedded Workbench IDE for ARM［14］，此开发环境具有编译速度快，直观和简单等特性，所以选择IAR开发环境.整个系统的软件流程图如图3所示.

1)硬件模块初始化.初始化各硬件模块，完毕以后需要读出保存在Flash的控制参数，读取完参数后，把这些参数赋值给相应的运算变量，以备之后的PID运算;紧接着依据捕捉的遥控器2通道的信号上升沿是否大于1 700，判断是否需要对电子调速器进行行程的校准.

2)姿态解算模块.对姿态数据进行更新，然后对发送给上位机的缓冲区数据进行更新，最后就是判断系统是否处于解锁和关闭状态，这些都是出于安全防护的考虑.如果对整个系统没有锁定这一功能，而操作者在对系统上电的时候系统就处于运行状态，假如这个时候遥控器信号不是正常值，很容易出现危险的情况.同理，为什么要判断遥控器是否关闭，原因是遥控器关闭的时候信号处于异常状态，如果不加这个判断，即遥控器关闭状态下对系统上电，且解锁完毕，飞行器会出现异常的工作状况.接着就是等待控制周期标志置位，对控制量进行更新，本系统的控制周期是2.5 ms，之所以采用这个控制周期是因为控制电调的PWM波周期是2.5 ms.

图3 飞行控制系统整体软件流程图Fig.3 Software flow chart of the flight control system

3)遥控器信号捕捉模块.主要应用的是中断子程序;控制周期定时子模块，主要是控制周期标志位;数据发送子程序，应用DMA发送;姿态传感器数据接收子模块，通信协议是IIC;上位机数据接收子程序，应用的是中断子程序;PWM控制量输出子程序;还有“X”模式4个电机控制量耦合关系子程序和Flash读取存储子程序.限于篇幅，子程序不详细介绍.

2.2.2 上位机软件设计

前面已经提到整套控制系统的核心就是PID控制策略，所以需要对PID参数进行调节才能获得好的控制效果.上位机软件设计就是基于对PID参数更好的调节，有以下功能:

1)实时调节PID参数.STM32程序烧写需要Jlink连接电脑，如果没有一个上位机可以实时地调节PID参数，就需要调节一次参数烧写一次程序，这给调试整个系统带来很大的不便.

2)观察模块运行情况.在系统设计时依据的是模块化的设计方案，即对整个系统分成各个模块，先把每个模块的功能实现，最后再把这些模块串联起来组成一个完整的系统.设计模块时需要对模块本身的功能进行校验，比如遥控器信号的捕捉，如果能有一个界面显示捕捉的数值，可以很清晰地看出模块是不是能够正常工作，还可以在模块设计时验证功能是否正确和完善(前提当然是先完成无线串口模块).再次，整个系统组装完毕以后，需要对整个系统先进行功能测试，然后才是装桨试飞，所以试飞之前也需要上位机的数据显示，特别是控制量的显示，这样给系统调试带来很大的方便.

3)飞行信息监控.在四旋翼飞行器成功飞行的过程中也需要对过程信息进行监控，以免意外情况的发生.即使有意外情况发生，比如存在系统设计漏洞，就可以通过保存的数据进行分析，找出漏洞和造成意外的原因.飞行控制系统设计后期，希望让飞行器做出一些复杂的动作，如果还是依靠遥控器给出设定信号，肯定是完成不了预期任务的，还是需要上位机给出一系列的指令.总之，设计一款属于本系统的上位机非常重要.图4是目前设计的上位机版本，随着开发的深入，会对其进行必要的修改.

从图4中可以看到，上位机根据不同的功能，整个界面分成了8个工作区，分别是串口操作区，控制量图像显示区，遥控器信号捕捉区，PWM控制信号输出区，水平仪显示区，PID参数调节区，文件操作区和姿态传感器数据区.该上位机基本实现了上述需要的功能，但是可以看出该上位机只是用于飞行控制数据的显示和PID参数的调试，所以之后还需要对上位机增加其他的功能，比如飞行器轨迹的规划，飞行器机动动作指令的发送等.

图4 上位机运行界面Fig.4 PC operation interface

该上位机是在VC++6.0环境下［15］开发的，而且需要用到一些控件，实际开发若是在Windows 7的操作系统下进行，会加载不了控件，所以需要先在Windows xp的操作系统下加载控件，编程完毕才能在Windows 7的电脑下使用，这给开发带来一定的不便，所以之后计划将该上位机移植到Visual studio上.如果将来还需要将上位机在Windows系统的设备上使用，还需要将该上位机移植到一些不依赖于操作系统的开发环境中.

3 飞行实验

飞行之前，要作以下准备:先把程序烧到STM32芯片中，测试输出的PWM波形是否正常.然后在上位机测试，先设置端口和波特率(根据计算机的端口以及无线模块的波特率设置).本实验设置端口:COM5，波特率19 200.把飞行器放在水平地面上(用水平仪测试)，标零多次设置，直到俯仰角和横滚角为0.设置磁标定，使飞行器进行旋转，直到界面的数据不变为止，保存标定.稳定性测试:飞行器接上电池，通电，遥控器油门打到最大，上位机通过3D显示，观察到几乎不动的效果，说明稳定性好;否则，就说明可能硬件或软件出了问题，要经过排除，直到稳定状态.PID参数的设置，通过对飞行器参数分析，设定PID参数，如表1所示.

表1 PID参数表Tab.1 PID parameters table

图5 飞行测试Fig.5 Fly test

多次测试实践表明，在室内或室外有风干扰的情况下，对PID的三个参数PK，IK，DK在表 1的基础上进行微调.飞行的稳定性主要决定于横滚、俯仰和航向，一开始飞行时，可能会出现三个角度的偏移，那就微调上面的参数.本实验室设置的参数，在飞行过程中达到了非常稳定的效果(如图5和表2所示)，飞行器能稳定地飞行，能进行垂直升降、悬空，任意旋转一定的角度等姿态控制，达到了非常理想的效果.

表2 飞行测试数据Tab.2 Flight test data(°)

4 结论

飞行系统设计已完成得非常成功，但在建模控制时，需根据飞行器的具体参数，对PID参数进行反复调节才能达到稳定控制的目的.其次，本实验室开发的飞行器是为后续的开发作准备，包括飞行器的环境感知能力，轨迹控制等智能化，以达到应用效果，可作为本项目以后努力的方向.

［1］姬江涛，扈菲菲，贺智涛，等.四旋翼无人机在农田信息获取中的应用［J］.农机化研究，2013，35(2):1-4.Ji Jiangtao，Hu Feifei，He Zhitao，et al.The application of quacl-rotor UAV in farmland infarmation acquisition［J］.Journal of Agricultural Mechanization Research，2013，35(2):1-4.(in Chinese)

［2］宿敬亚，樊鹏辉，蔡开元.四旋翼飞行器的非线性PID姿态控制［J］.北京航空航天大学学报，2011，37(9):1054-1058.Su Jingya，Fan Penghui，Cai Kaiyuan.Attitude control of quadrotor aircraft via nonlinear PID［J］.Journal of Beijing University of Aeronautics and Astronautics，2011，37(9):1054-1058.(in Chinese)

［3］黄溪流.一种四旋翼无人直升机飞行控制器的设计［D］.南京:南京理工大学，2010.

［4］齐书浩.微型四旋翼飞行器总体设计及其运动控制［D］.上海:上海交通大学，2013.

［5］李俊，李运堂.四旋翼飞行器的动力学建模及PID控制［J］.辽宁工程技术大学学报(自然科学版)，2012，31(1):114-117.Li Jun，Li Yuntang.Modeling and PID control for a quadrotor［J］.Journal of Liaoning Technical University(Natural Science)，2012，31(1):114-147.(in Chinese)

［6］从梦苑.基于线性二次调节器的四旋翼飞行器控制系统的设计与研究［D］.哈尔滨:哈尔滨工程大学，2011.

［7］Bouabdallah S.Design and control of quadrotors with application to autonomous flying［D］.Swiss Federal Institute of Technology，2007.

［8］左宗玉.SE(3)上一般力学控制系统的运动描述与飞行器建模［J］.航空学报，2012，33(8):1491-1497.Zuo Zongyu.Kinematic description of a general mechanical control system on SE(3)and aircraft modeling［J］.Acta Aeronautica et Astronautica Sinica，2012，33(8):1491-1497.(in Chinese)

［9］李秀英，刘彦博.基于PWM的四旋翼飞行器的控制方法［J］.吉林大学学报(信息科学版)，2011，29(5):464-471.Li Xiuying，Liu Yanbo.Control of four-rotor aircraft based on PWM［J］.Journal of Jinlin University(Information Science Edition)，2011，29(5):464-471.(in Chinese)

［10］江斌.小型型四旋翼低空无人飞行器综合设计［D］.杭州:浙江大学，2013.

［11］刘金琨.先进PID控制MATLAB仿真［M］.北京:电子工业出版社，2012.

［12］宿敬亚，张瑞峰，王新华，等.基于滤噪微分器的四旋翼飞行器控制［J］.控制理论与应用，2009，26(8):827-832.Su Jingya，Zhang Ruifeng，Wang Xinhua，et al.Controlling a four-rotor aircraft basic on noise-attenuation differentiator［J］.Control Theory and Applications，2009，26(8):827-832.(in Chinese)

［13］陈城.基于STM32的温湿度检测系统［D］.武汉:武汉科技大学，2012.

［14］沈建华.STR71X系列ARM微控制器原理与实践［M］.北京:北京航空航天大学出版社，2006.

［15］王育剑，刘国华，战传娜，等.基于VC6.0的SR830数据采集系统的设计与实现［J］.南开大学学报(自然科学版)，2013，46(1):22-26.Wang Yujian，Liu Guohua，Zhan Chuanna，et al.Design and implementation of SR830 data a cquisition system based on VC6.0［J］.Journal of Nankai University(Natural Science)，2013，46(1):22-26.(in Chinese )