四旋翼飞行器PID优化控制

王史春

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

四旋翼飞行器是一个具有六自由度和4个控制输入的非线性欠驱动系统，具有对外界和自身抗干扰敏感的特性，控制的主要问题是解决强耦合性和不稳定的动力特性。姿态控制是整个飞行控制的关键，如果能精确控制飞行器姿态，模型相对还是比较明确的，难度仍然在PID参数的整定上[1]，其参数调整很大程度上依赖于设计人员自身经验，只能人为的多次调试才能确定，达到稳定状态，但不一定是最佳参数。针对本实验室在设计时遇到的以上问题，提出了改进的PSO算法[2]和遗传算法相结合方法来优化PID参数。四旋翼飞行器是多变量系统，需多目标搜索，PSO算法全局搜索能力强，算法实现简单，能够实现多个目标寻优，寻找合适的参数，以达到理想的效果。遗传算法在不需要给出控制器初始参数的情况下，通过复制、交叉、变异操作，寻找到合适的参数，达到寻优，使控制目标满足要求，同时能解决多值函数问题以及在多参数寻优中，容易造成寻优失败或时间过长的问题，具有局部搜索能力强的特点，并能从多点开始并行操作，高效启发式搜索，避免了过早陷入局部最优解[3]。

1 动力性建模

四旋翼飞行器是一个非线性、多变量、高度耦合的欠驱动系统[4]。为实现对四旋翼飞行器这一欠驱动系统的有效控制，必须准确建立其在各种飞行状态下的数学模型。为了得到四旋翼飞行器的数学模型，先建立2个基本坐标系：如图1所示，地面坐标系G(OXYZ)和机体坐标系S(oxyz)。在飞行器飞行过程中，地面坐标系保持不变。如图2所示，地面坐标系G与机体坐标系S的定位关系分别用欧拉角表示，分别为俯仰角φ，翻滚角θ，偏航角ψ。两坐标系的旋转关系通过旋转矩阵表示为

(1)

(2)

图1 四旋翼飞行器示意图Fig.1 Schematic diagram of quadrotor

图2 欧拉角Fig.2 Euler angle

为了简化四旋翼飞行器的动力学模型[4]，根据牛顿第二定律[5]，飞行器动力学方程表示为

(3)

(4)

式中：F为飞行器所加外力和；m为飞机质量；v为飞行器速度；M为飞行器所有外力距[6]的和；H为飞行器相对于地面坐标系绝对动量距。

根据动力学方程，假设Fx，Fy，Fz为F在飞行器坐标系3个坐标轴上的分量;p，q，r为角速度ω在飞行器坐标系3个轴的分量。根据式(1)，式(3)，式(4)得到机体坐标系下线运动方程(Ki为综合阻力系数)：

(5)

(6)

(7)

本研究是用惯性测量单元测量四旋翼旋转[7]，而欧拉角是时域变化的连续函数，三轴转动角速度，通过矩阵表示为

(8)

经拉格朗日推导式(8)可得运动方程表示为

(9)

由式(9)可推出:

(10)

2 遗传算法优化PID控制器参数

2.1 PID控制

为了把四旋翼飞行器非线性耦合模型分解为4个独立的控制通道[8]，经过实验，利用小扰动原理得到的线性方程组用来分析飞行器操纵性、稳定性、有效性，能得到较好效果和好的准确度。在小扰动下进行线性化处理，可得到飞行器小扰动运动，其中，状态变量：

x=[uvwpqrθφψ]T，

(11)

控制变量：

u=[u1u2u3u4]T。

(12)

为了简化控制系统的设计，假设姿态角和角速度之间为简单的积分关系，即

(13)

根据实验室的四旋翼飞行器有关参数和环境数据以及系统的传递函数G(S)=(s×I-A)-1B和线运动方程(5)、方程(6)、方程(7),可得各控制通道的传递函数。

俯仰通道：

(14)

翻滚通道：

(15)

偏航角：

(16)

X轴与俯仰角：

(17)

Z轴:

(18)

Y轴与翻滚角：

(19)

2.2 GA-PSO算法优化PID参数

为了优化各通道的PID的3个参数，GA-PSO算法的PID控制器以误差e和误差变化率ec作为输入，以满足不同时刻的e和ec对PID参数整定的要求,如图3所示。PID控制器的控制算法为

(20)

在文献[10]中,提出标准的PSO算法：

(21)

(22)

优化PID控制器参数也取得了一定的成效。标准的PSO算法中粒子的移动是以自身的局部最优和全局最优来改变粒子的方向和速度，容易陷入局部最优值，特别对于多峰值函数。

图3 PID控制器优化结构图Fig.3 Optimization of PID controller structure

为了更好地对PID参数进行优化控制，提出将改进的PSO算法和遗传算法结合，其流程如图4所示，算法的核心思想是通过改进PSO算法，算法如下：

(23)

这里的pm是随机选取周边2个粒子，通过比较选取适配值高的粒子位置作为pm的值；Xi(k)和Vi(k)是粒子当前的位置和方向；C是常数，一般取值为2。ω是权重，随着粒子搜索的进程，值会减少，提高搜索精度。标准的PSO和改进的PSO算法最大不同在于学习过程不一样，标准PSO算法通过自身最优和局部最优改变自身的速度和位置；而改进的PSO算法通过周边粒子综合学习来改变自身的速度和位置，有利于充分学习，达到好的性能。当第一个粒子通过周边粒子综合学习后，更新到一个较优的位置，为了避免陷入局部最优，特别对于多峰值函数来说，容易陷入局部最优，通过遗传算法的选择、交叉、变异来对粒子进行重新组合，避免了粒子早熟的现象。

本文的算法具有如下创新：

1)PSO和GA结合，发挥各自特长，改善系统的性能，增强算法的适应性；

2)PSO算法的权重系统ω随搜索进行逐步减小，有利于搜索速度与精度的提高；

3)改进的PSO粒子得到充分学习，遗传算法又可避免粒子的早熟现象。

图4中，X1={x11，x12,x13},D是维数,g是代数，max_gen指最大代数pm={pm1,pm2,pm3}。

3 仿真实验和飞行实践

本仿真实验是通过对标准PSO算法和GA-PSO算法进行比较，来验证算法的优越性。采用时间为1 s,输入指令为一阶跃信号。为获取满意的过度过程动态特性，采用误差绝对值时间积分性能指标作为参数选择的最小目标函数[9]。为了防止控制能量过大，在目标函数中加入控制输入的平方项。选用式(24)作为参数选取的最优指标：

(24)

图4 GA-PSO控制优化流程图Fig.4 Chart of GA-PSO control and optimization

其中：e(t)为系统误差;u(t)为控制器输出;tu为上升时间；ω1,ω2,ω3为权值。

为了避免超调，采用了惩罚函数[10]，即一旦产生超调，将超调量作为最优指标的一项，此时最优指标为

Ife(t)<0,

ω4|e(t)|dt)+ω3×tu。

(25)

PSO算法中粒子数为40，最大代数为100，学习因子为1.51，权重ω代入式(25)计算可得。遗传算法交叉概率和变异概率分别为pc=0.92,pm=0.035。仿真实验结果如图5—图10所示，标准PSO算法和GA-PSO算法都达到了较好的效果，相对来说，GA-PSO算法达到更好的控制效果。俯仰角如图5所示，标准PSO算法在0.1 s时有较大的波动，而GA-PSO能较快地达到稳定效果；偏航角如图6所示，达到稳定时间GA-PSO有明显的改善，相差10倍左右，稳定性比标准PSO更好；翻滚角如图7所示，达到稳定时间和稳定性相近。X轴方向如图8所示，GA-PSO较快达到稳定状态；Y轴方向如图9所示，达到稳定时间和稳定的效果相近；Z轴方向如图10所示，GA-PSO稳定性较好；达到稳定时间比PSO快。经过对飞行器PID优化，得出PID各参数的值，如表1所示。各项数据表明，各项指标在同行研究中达到领先水平，表明本文提出的GA-PSO算法在飞行器中达到了优化的效果。

飞行器在实际飞行中，会受到各种信号的干扰和风力的影响，导致飞行器的位置和姿态角发生变化，因此在仿真中加干扰信号进行测试，在1 s，2 s,3 s的时间给姿态角加一个随机干扰突变信号，仿真结果如图11所示，飞行器的姿态角在0～3 s时间内出现波动，范围在0.7～1 rad之间，波动是非常大的，产生不稳定，但最终通过控制器的调节回到了稳定状态。

图5 俯仰角Fig.5 Angle of pitch

图6 偏航角Fig.6 Yaw angle

图7 翻滚角Fig.7 Rolling angle

图8 X轴方向Fig.8 X direction

图9 Y轴方向Fig.9 Y direction

图10 Z轴方向Fig.10 Z direction

实验室的飞行控制器部分的结构如图12所示，飞行器姿态控制是通过PID参数的设置来达到飞行器的稳定效果，PID参数设置如表1所示，为了检验设计的效果，把表1数据应用于实验室的飞行器，通过多次飞行试验，测试数据如表2所示，达到较好的效果。

图11 干扰信号仿真图Fig.11 Interfering signal simulation diagram

表1 PID参数优化表Tab.1 PID parameters optimization rad

表2 飞行测试数据Tab.2 Flight test data rad

图12 飞行控制器结构Fig.12 Structure of the quadrotor

4 结 语

通过对实验室四旋翼飞行器进行建模，提出改进的GA-PSO算法，通过仿真实验结果表明，飞行器达到稳定状态快，精度高的特点。为了更好地验证算法的效果，对飞行器进行多次试飞，测试数据表明，控制器具有较好的鲁棒性和良好的控制效果。

参考文献/References：

[1] 燕 翔，张持健.工业随动系统中模糊PID控制器的设计与应用[J].河北工业科技,2011,28(2)：122-125.

YAN Xiang,ZHANG Chijian. The design and application of Industrial design of fuzzy PID controller in the servo system[J]. Hebei Journal of Industrial Science and Technology,2011,28(2):122-125.

[2] 宋 遥，李国名，徐 丽.采用蚁群算法模拟机器人寻路的仿真实验[J].河北工业科技,2010,27(5)：340-343.

SONG Yao, LI Guoming, XU Li. The simulation experiment of ant colony algorithm simulation robot in pathfinding[J]. Hebei Journal of Industrial Science and Technology,2010,27(5):340-343.

[3] 刘金琨.先进PID控制MATLAB仿真[M].北京：电子工业出版社,2012.

LIU Jinkun. Simulation of Advanced PID Control and MATLAB[M].Beijing: Electronic industry press,2012.

[4] 黄溪流.一种四旋翼无人直升机飞行控制器的设计[D].南京：南京理工大学，2010.

HUANG Xiliu. Tontroller Design of Four-rotored Helicopters Flight[D].Nanjing：Nanjing University of Science and Technology,2010.

[5] MINH L D.Modeling and control of quadrotor MAV using vision based measurement[A].2010 International Forum on Strxtegic Technology(IFOST)[C].San:[s.n.],2010.70-75.

[6] SUBBARAO A K,LEWIS F.Dynamic inversion of quadrotor with zero-dynamics stabilization[A].IEEE International Conference on Control applications[C].San Antonio:[s.n.],2008.1 189-1 194.

[7] BOUABDALLAH S,SIEGWRT R.Design and control of quadrotors with application to autonomous flying[D]. Lausanne :EPFL,2006.

[8] 李 俊，李运堂.四旋翼飞行器的动力学建模及PID控制[J].辽宁工程技术大学学报,2012,31(1)：114-117.

LI Jun,LI Yuntang. Four rotor aircraft dynamics modeling and PID control[J]. Journal of Liaoning Technology University,2012,31(1):114-117.

[9] 胡海兵，胡庆波，吕征宇.基于粒子群优化PID伺服控制的设计[J].浙江大学学报(工学版),2006，40(12)：2 144-2 148.

HU Haibing， HU Qingbo， LYU Zhengyu. The design of based on particle swarm optimization PID servo control[J]. Journal of zhejiang University(Engineering Science),2006,40(12):2 144-2 148.

[10] 孙增圻.遗传算法:智能控制理论与技术[M].北京：清华大学出版社,2011.

SUN Zenqi. Genetic Algorithm: Theory and Technology of Intelligent Control[M].Beijing: Tsinghua University Press,2011.