四旋翼飞行器的双回路PID控制器的设计与实现

卢敬元，曾祥烨，王静宜，王 杨

（河北工业大学 电子信息工程学院，天津 300401）

四旋翼飞行器的双回路PID控制器的设计与实现

卢敬元，曾祥烨，王静宜，王 杨

（河北工业大学 电子信息工程学院，天津 300401）

四旋翼飞行器是当今市场的一个新兴且关注度高的研究方向．PID算法当属四旋翼飞行器的核心部分，优秀的PID算法可以实现稳定的悬停和快速的打舵响应．针对传统的单回路PID控制器的不足设计了一种新的双回路 PID控制器，结构上分为主回路和副回路，角速度作为主回路，角度作为副回路，副回路的输出量作为主回路的期望值．并在角度姿态的获取上创新的加入了滤波系统，从而使副回路获得的角度曲线更加稳定和平滑，降低了电机的工作温度，提高了控制精度．控制器采用了STM 32F103作为控制核心．该款处理器有配套的DSP库，可以实现FIR滤波器,滤波参数确定于MATLAB的FIR设计工具．经过上机实验说明了新的滤波双回路PID系统不仅悬停十分稳定，对于打舵响应十分迅速，而且具备一定的抗外力干扰能力，满足设计需求．

双回路PID算法;有限长单位冲激响应滤波器;MATLAB；四旋翼；STM 32

0 引言

四旋翼飞行器具有体积小、重量轻、姿态灵活、扩展性强等优势，能够很好的进行侦查、航拍、运送轻型货物等任务，是当前小型无人飞行器的热门研究方向．本文的控制系统使用的处理器为STM 32F103ZET6，该款内核的特点就是高性能、低功耗，适合用于四旋翼这类对功耗要求敏感的系统．

近些年随着各种自动控制技术的发展与成熟，出现了很多在概念上更加先进的算法，但是由于四旋翼的结构决定着其搭载的处理器的高复杂度的浮点、矩阵运算的能力较弱，所以当前的四旋翼控制算法还是以PID和各种改进型PID算法为主．目前大部分四旋翼的开源项目采用的都是原始的单回路PID控制[1]，本文设计的双回路PID算法辅以滤波器在稳定性和可移植性上都强于传统的单环PID算法．

1 四旋翼结构模型和基本原理

1.1 机体物理模型

四旋翼由4个电机和螺旋桨构成，从结构上主要分为两大类：“X”型和“十”型，主要构成部件为飞行控制器、遥控接收机、电机、电子调速器等等．本文主要介绍“X”型[2]，其结构图如图1所示，延顺时针方向分别是1、2、3、4号电机，为克服机体自转采用了不同转向的旋桨，其中对角旋浆转向相同，相邻旋浆转向相反，正前方为机头方向．其中绕X轴旋转的为俯仰角，绕Y轴旋转的为横滚角，绕 Z轴旋转的为偏航角[3]．

图1 四旋翼基本结构Fig.1 The structure of the four-rotor aircraft

4个电机分为2组，控制机体的3个基本姿态角：Roll，Pitch，Yaw．机体的所有姿态由这3种姿态角线性组合而成．姿态角、机体姿态和电机输出对应如表1，因此在编写飞行控制程序对机体进行姿态控制时，需要3组参数分别控制Roll、Pitch、Yaw这3个姿态角．表1中的1～4分别代表1～4号电机，“+”为电机的等量转速提高；“ ”为电机的等量转速降低．

表1 姿态电机输出对应表Tab.1 The results of motor output

1.2 飞行控制系统模型

飞行控制系统模型[4]可以简单表示为图2，其中Moto1～Moto4表示1～4号电机的PWM控制信号[5]．在姿态融合时，主要依靠陀螺仪的角加速度来计算姿态角，由于陀螺仪长时间工作会产生漂移，故用加速度计辅以重力加速度对陀螺仪进行校正．但是在对偏航角（Yaw）计算时，由于Z轴垂直于水平面，加速度计无法校正，所以选择使用地磁传感器对Yaw进行专门校准．

图2 飞控系统框图Fig.2 Flight control system

1.3 飞行器的力学模型

飞行器在飞行过程中的总升力[6]来自于4个桨产生的升力之和，故升力Fi可以表示为

式中：i为第i个螺旋桨；Fi为升力，单位为牛顿（N）； 为当前空气密度，单位为kg/m3；CT为阻力系数；A为螺旋桨旋转之后的面积，单位为m2；为电机转速，单位为rad/s；R为螺旋桨的半径，单位为m．在本次试验中采用的是4套相同规格的电机和桨，因此Ai、 、Ri近似相等，可简化为用常量ki表示，故Fi可以表示为

由表1可知，在X形的四轴飞行器中，飞行姿态是由3个基本角决定的．将其在力学模型中加以分析，姿态角产生的实质就是由于2组桨的不同转速而产生的力矩差．以横滚角（Roll）为例，横滚力矩T为

式中：l为电机到飞行器重心的距离，单位为m．

飞行姿态的控制是在油门转速的基础上对4个电机进行转速的二次调整，所以每个电机的实际转速可以表示为油门转速0和调整转速，姿态角的控制实际是在一个连续的时刻对不同的角加速度的控制，以横滚角为例，使横滚角的旋转轴为X轴，绕X轴转动的角加速度可以表示为

式中：ax表示绕X轴转动的角加速度，单位为rad/s2；Ixx表示绕X轴的转动惯量，单位为kg/m2．在离散控制系统中，某一特定时刻的四旋翼飞行器的姿态变化取决于a在时间T上的二重积分．

2 双回路PID算法

飞行控制器的软件方面核心是PID控制器．PID具有结构简单、原理易懂、不需精确的系统模型等优点．由于飞行器的结构多变且不规则，所以不能得到准确的机体数学模型，这也导致了对每一机体的结构和参数需要根据经验来选取，这时PID技术即是最优选择，PID控制器的机制就根据系统的期望值和测量值之间的误差值通过比例、积分、微分的运算得到一个控制量来修正误差．

在PID控制器中，使用最为广泛和经典的当属单回路PID控制系统，其3个字母分别代表比例（proportion）、积分（integration）、微分（differentiation）．在四旋翼系统下期望值为遥控器输出的期望角度，测量值为姿态传感器输出的角度数据，输出值为控制电机转速的PWM信号．

传统的单回路PID在对于大型四旋翼飞行器的控制能力不足，例如在角度控制的PID系统在进行微小变化时并不敏感，而如果加大参数又会使在处理较大误差时极易产生超调．此时角速度的变化最能反映瞬间的机体的姿态变化，在此设计一种针对于传统单回路PID算法的改良算法：将2个单回路PID进行串联，使用角速度来处理姿态变化，但是输入量仍为角度的双回路PID控制算法．

2.1算法基本原理

双回路PID算法流程[7]如图3所示，双回路PID输出量为电机的转速信号，输入量为实时的角度和角速度．双回路PID主要分为主回路和副回路，主回路为角速度控制，副回路为角度控制．副回路主要负责划定大概的调节范围，然后由主回路来进行精确的角度实现．

图3 双回路PID算法流程图Fig.3 Double-loop PID controller

2.2 参数调试

双回路PID算法由2套PID参数构成，其调试方法和顺序与普通PID有较大差异，参数意义和调试顺序如下：

1）主回路P：修正误差的主要参数，直接决定打舵相应．

2）主回路D：加快打舵相应，抑制P产生的回中震荡．

3）主回路I：在一定范围内抑制PD产生的超调效应．

在确定一个合适的P之后加入D，此时易产生震荡，需要适当提高P以匹配D，合理的PD值中的P往往要大于初始P值．主回路参数调试时要去掉副回路，使期望欧拉角直接作为主回路的期望值，调试摇杆手感和打舵响应速度．调整到合适打舵响应之后重新接入副回路：

4）副回路P：期望角的放大倍数，使传入主回路的期望值能与主回路的PID算法能较好匹配．

相对于成熟的单回路PID算法，双回路PID算法在参数调试方面的复杂性主要存在于主、副回路的参数匹配，在两回路综合调试效果不佳时，不易确定问题回路，且在多姿态角协同调试时复杂度较高．双回路PID目前还没有比较成熟确定参数的数学模型，主要使用经验试凑法．

2.3 油门信号计算

本文采用位置式控制，即输出量直接为最终的电机转速，位置式的表达式[8]

其中：e(t)为在t时刻的期望值和测量值之差，称之为误差；kp为比例系数；ki为积分系数；kd为微分系数；u(t)为t时刻的输出量．在实际使用中，可表示为时间段的累加，可表示为2次误差值之差，将副回路输出命名为ANGELout(t)，此时副回路的输出可表示为

根据图3可知，主回路的输入量为eRate(t)=ANGELout(t)当前角速度，由此主回路的输出可表示为

其中kpRate、kiRate、kdRate为主回路的比例、积分、微分系数．双回路PID相应的也有两套PID参数需要调整．

需注意的是由ANGELRateout(t)求出的是电机的调整转速，电机输出时还需加上基础的油门转速．最终进行油门信号输出之前需要对油门信号进行限幅，防止过冲失控．电机的实际输出可表示为式 (4)

式中POWER是基础的油门信号，由遥控器拨杆直接给出．由式(3)可以分别求出俯仰角的调整值PITCHout、横滚角的调整值ROLLout、偏航角的调整值YAWout．

2.4算法测试

实验平台[9]为对角电机孔距450mm的机架，姿态角和角速度的更新频率约为200Hz，PID算法的控制频率约为50 Hz，期望角恒为0，即让机体处于悬停自稳状态，将机体的3个姿态角中的2个固定住，单独调试一个姿态角，图4所示．

将姿态角通过蓝牙模块上传到上位机进行精确的角度分析，如图5所示，上位机软件下半部分为机体实时角度，上半部分为电机实时转速，可见图5在自稳状态下，机体的稳定度较高，超调量很小，系统能很快进入自稳状态．但是由于双回路的电机转速的变化是取决于角速度变化，在高转速时，电机的震动会使角速度产生大幅震荡，导致电机转速变化非常不线性，经常会在转速高频率、大幅度改变的情况下工作，电机在短时间内大量发热，这种高温工作状态下容易导致电机失磁损坏．解决方案是在姿态获取时设计FIR滤波器消除震动干扰，平滑电机的控制曲线．

图4 测试机架Fig.4 Structural testing

图5 双回路PID图像Fig.5 The image of double-loop PID controller

3 FIR滤波器

FIR的设计主要有窗函数法、频率采样法、等波纹最佳逼近法等，本文采用窗函数设计法．在滤波器的实际实现中，由于是用一个有限长的序列h(n)代替理想滤波器中的h_d(n)，肯定会引起误差，产生吉布斯（Gibbs）效应，也称为截断效应，使阻带的衰减小，从而满足不了技术上的要求．此时就要根据各种窗函数的特性和应用场景来选择合适的窗函数来减小截断效应，设计一个能满足技术要求的FIR线性相位滤波器．

典型的窗函数有：矩形窗、三角形窗、汉宁窗、哈明窗、布莱克曼窗等．本次根据未滤波数据中的陀螺仪取值计算了机身最大晃动频率、主要干扰来源，并在MATLAB根据各个窗的h(n)选择了矩形窗．

3.1 基于STM 32F10x DSP库的FIR实现

ST公司对于STM 32F10x有配套的DSP库[10]实现丰富功能，本文采用的是其2.0版本，可以进行PID、FFT、FIR等多种操作．其中：FIR库函数的函数体、输入输出参数如表2所示．

表2 fir_16by16_stm32函数说明Tab.2 The function description of fir_16by16_stm32

在使用该FIR函数时应注意以下几点[11]：

1）为了提高芯片效率，根据直接性FIR结构特性，在定义输入输出数组长度时应满足输出数组的长度应为输出数组的长度与滤波器系数的个数之差．

2）根据表2可知，此函数是由2个16位（short型）数卷积得到32位（int型）结果．在选取输入函数是要注意16位数的范围，即使输入数组内的元素满足．

3）由于STM 32系列只支持整型运算，而本文中无论是MATLAB得到的滤波器参数还是待滤波的陀螺仪角度都为浮点型，若强行运算会极大的降低运算效率增加时延，所以要在误差可接受的前提下选取适当的方法对2组参数进行整型转化．

3.2 滤波器设计

滤波器参数特点是中间部分的参数权重较高，当输入陀螺仪角度发生较大变化，在经过大概（阶数/2）个周期过后输出值才会有明显的变化．本系统的运行频率大概为50 Hz，故时延主要来自于每周期的参数传递而非卷积计算时间．

针对以上系统特性，选取滤波器时就更加倾向低阶，在MATLAB中用FDA Tool对滤波器进行设计[12]，计算了机体摆动频率后得到设计截止频率为4 Hz，选取了10阶矩形窗滤波器，其衰减特性比较符合使用需要．衰减特性如图6所示．

图6 矩形窗衰减特性Fig.6 The attenuation characteristics of rectangular window

3.3 实验结果分析

FIR滤波器实验与PID实验采用相同的机架平台，陀螺仪采用MPU6050，配合2.4 GHz的遥控装置．数据波形采集于油门开度50%阶段，将飞行器固定在平地上，此时飞行器位于离地临界状态．在当前油门开度下，由于飞行器被固定在水平地面上，所以现在的角度变化都为电机震动叠加产生的，将角度变化图像放大，如图7，可见角度数据以一个小且快的速率变化，此时的角速度也会高频大幅度的震荡，从而导致了电机转速的震荡．

图7 未滤波角度Fig.7 Unfiltered angle

在系统中加入刚才设计好的FIR滤波器，相同的油门开度下将角度曲线放大，如图8，可见之前因为电机震动产生的抖动已经基本消除．此时的陀螺仪采样延时实测可接受．

图8 滤波后角度Fig.8 Filtered angle

4 双回路PID和FIR滤波器结合调试

将调试好的FIR滤波器加入到测量欧拉角和测量0角速度之后，依旧用图4的调试方法，在上位机得到图9．

图9 滤波后图像Fig.9 Filtered image

将图9与图5对比，可见在双回路PID算法中加入FIR滤波器效果十分显著，在自稳性影响不大的情况下大幅度的削减了电机震荡，电机发热也大大减小，由此证明FIR滤波器对于双回路PID的优化十分有效．

将机体初始横滚角倾斜20°，期望角为0°，迅速推起油门至50%，得到姿态曲线如图10，可见横滚角立刻能回归零点，而且几乎没有多余震荡，超调量极小，稳定且快速．在期望角设置为20°时对机体进行快速打舵，姿态曲线如11图，可见机体能很好的执行打舵指令，在快速连续变相打舵时也不会产生超调而失控．在机体悬停稳定时人工引入外部干扰，姿态曲线如图12，在外部产生干扰时机体能迅速反应而且迅速恢复悬停状态，几乎没有超调，稳定性较好．

图10 自稳姿态图像Fig.10 The image of automatic stability

图11 打舵姿态图像Fig.11 The image of helm response

图12 引入外部干扰图像Fig.12 The image of introducing external interference

5 结束语

本文设计了基于STM 32F103平台的四旋翼飞行控制器，使用双回路PID算法，并在其中加入FIR滤波器，有效提高了四旋翼机体的自稳性能和打舵响应速度，而且在FIR滤波器的优化下显著减小了电机负荷，有效的保护电机，同时也提高了平台的可移植性，增强了系统的鲁棒性．在经过对两回路PID参数整定仔细的后，本文设计的飞行控制系统可以很好的实现飞行器响应遥控信号进行飞行动作．

[1]Atheer L Salihl，Moghavvemi M，Haider AF Mohamed．Flight PID Controller Design for a UAVQuadrotor[C]//Scientific Research and Essays，2010．

[2]赵世荣，孙运强，石喜玲．基于信息融合四旋翼飞行器稳定性改进设计 [J]．电子器件，2015，38（6）：1417-1420．

[3]刘峰，吕强，王国胜，等．四轴飞行器姿态控制系统设计 [J]．计算机测量与控制，2011，（3）：583-585．

[4]叶树球，詹林．基于PID的四旋翼飞行器姿态控制系统 [J]．计算机与现代化，2015（5）：117-120

[5]李秀英，刘彦博．基于PWM的四旋翼飞行器控制方法 [J]．吉林大学学报（信息科学版），2011，29（5）：464-472．

[6]刘浩蓬，龙长江，万鹏，等．植保四轴飞行器的模糊PID控制 [J]．农业工程学报，2015，31（1）：71-77

[7]李俊，李运堂．四旋翼飞行器的动力学建模及PID控制 [J]．辽宁工程技术大学学报（自然科学版），2012，31（1）：114-117．

[8]郭晓鸿，杨忠，杨成顺，等．一种基于STM 32的四旋翼飞行器控制器 [J]．应用科技，2011，38（7）：35-40．

[9]韩志凤，李荣冰，刘建业，等．小型四旋翼飞行器试验平台设计 [J]．测控技术，2013，32（10）：121-124．

[10]STMicroelectronics．UM 0585User manual[Z]．Swiss：STMicroelectronics，2010．

[11]龙吉．一种在STM 32应用FIR滤波器的设计与实现 [J]．吉林化工学院，2015（10）：262-262．

[12]王春兴，张彬．节点文献基于Matlab的FIR数字滤波器设计 [J]．现代电子技术，2011，34（18）：83-85．

[责任编辑 代俊秋]

Design and implementation of double-loop PID controller used in four-rotor aircraft

LU Jingyuan，ZENG Xiangye，WANG Jingyi，WANG Yang

(School of Electronics and Information Engineering,Hebei University of Technology,Tianjin 300401,China)

Four-rotor aircraft is an emerging research and drawing more interests in the modern market.The core of the PID algorithm is undoubtedly the four-rotor aircraft,excellent PID algorithm can achieve stable hover and rapid response helm.Aiming at the shortcomings of traditional single-loop PID controller,we design anew double-loop PID controller. It is divided into main loop and secondary loop structure,the angular velocity as the main loop,the angle as the secondary loop,the output of the secondary loop as the main loop of expectations.A FIR filter is designed to reduce high-frequency interference and motor temperature and improve the control precision.The STM 32F103 is adopted to be the control core of the Four-rotor aircraft.Before we use the matched DSP library of the STM 32F103 to achieve FIR filter,we debug the related parameters of the FIR filter by means of MATLAB.Machine experiment illustrates the new double-loop PID filtering system not only makes the hover stable and the helm res ponse rapid,but also under control,to meet the design requirements．

double-loop PID;FIR;MATLAB;four-rotor aircraft;STM 32

TP13

A

1007-2373(2016)05-0016-07

10.14081/j.cnki.hgdxb.2016.05.003

2016-10-25

河北省自然科学基金（F2015202331）

卢敬元（1990-），男（汉族），硕士生．通讯作者：曾祥烨（1979-），男（汉族），讲师，博士．