基于PSO-Fuzzy-PID的四旋翼飞行器悬停控制

Hover Control for the Quadrotor Aircraft Based on PSO-Fuzzy-PID

摘要: 为解决四旋翼控制系统和外部环境发生变化时的悬停控制问题，提出了粒子群优化模糊自适应PID控制策略。建立了四旋翼的数学模型，引入了粒子群优化算法(Particle Swarm Optimization, PSO)和模糊自适应PID控制(Fuzzy adaptive PID control)，通过粒子群算法对模糊自适应PID控制在线优化来整定PID控制参数，进而提出PSO-Fuzzy-PID控制方法并应用于四旋翼的悬停控制中。MATLAB仿真结果表明，相较于传统PID控制、模糊自适应PID控制，本文中设计的控制方法具有更好的控制性能和适应能力。

Abstract: In order to suit the changes of quadrotor aircraft control system and the outside environment, a kind of hover control method for the quadrotor aircraft based on PSO-Fuzzy-PID is proposed in this paper. The model of the quadrotor aircraft is built, and the PSO-Fuzzy-PID control method, composed of particle swarm optimization algorithm and fuzzy adaptive PID control method, is in-troduced to control the hover of the quadrotor aircraft, adjusting the three parameters of fuzzy adaptive PID online by particle swarm optimization algorithm. The MATLAB simulation results show that the above method is superior to classical PID control method and fuzzy adaptive PID control method both on performance and adaptability.

1. 引言

近年来四旋翼飞行器由于可以垂直起降，自由悬停，且易于操控，在军用和民用领域的应用非常广泛 [1]。但是四旋翼是一种非线性、强耦合、欠驱动的复杂系统 [2] [3]，实现其悬停的稳定控制是现在面对的一个关键问题。

对于四旋翼的悬停控制，比较成熟的传统PID控制是人们使用最广泛的方法之一。传统PID控制是一种非常实用的控制方法，它应用的领域非常广泛，具有容易实现、控制参数较少、稳定性好等优点 [4]，但当四旋翼控制系统或外部环境出现变化时，由于只有一组固定的PID控制参数，无法针对变化作出调整 [5] [6]，就会导致四旋翼控制系统的抗干扰性和稳定性变差。模糊自适应PID控制可以在线整定PID参数，根据控制系统和环境的变化，对PID控制的三个参数作出相应的调整，但是也存在过分的依靠专家经验 [7] [8]，无法达到满意的控制效果的问题。粒子群优化算法起源于对鸟群捕食行为的研究，通过粒子群与模糊自适应PID控制的结合，在粒子群每一次优化迭代过程中，先根据系统偏差和偏差变化率由模糊规则表得出初步的PID参数增量，再结合粒子群算法寻优得出的PID参数得到该次迭代的PID参数，进行多次优化迭代得到最终参数，这样就克服了传统PID控制和模糊自适应PID控制的上述缺点，达到更加理想的控制效果。

本文将基于粒子群优化的模糊自适应PID控制器应用于四旋翼定点模式的悬停控制中。首先，建立了四旋翼控制系统的数学模型；其次介绍了模糊自适应PID控制和粒子群优化算法的原理，以及调节PID参数的过程；进而提出了粒子群优化模糊PID控制的悬停控制方法；最后，在Matlab中进行仿真。仿真结果表明该方法在提高系统动态性能和稳定性方面都优于传统PID控制和模糊自适应PID控制。

2. 四旋翼飞行器建模

为建立系统的数学模型，我们首先定义相关的坐标系。 $O\text{-}{x}_g{y}_g{z}_g$ 是地面惯性坐标系， $o\text{-}{x}_b{y}_b{z}_b$ 为机体坐标系。其中 $\theta$ 代表俯仰角度； $\psi$ 代表偏航角； $\varphi$ 代表滚转角，定义的坐标系如图1。

不考虑两个坐标系之间的线运动，即假设O与o相重合时，我们可以通过旋转使得两个坐标系相互重合。所以，我们可以获得两个坐标系的坐标转换矩阵：

$R=\left[\begin{array}{ccc}C\psi & -S\psi & 0\\ S\psi & C\psi & 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{ccc}C\theta & 0& S\theta \\ 0& 1& 0\\ -S\theta & 0& C\theta \end{array}\right]\ast \left[\begin{array}{ccc}1& 0& 0\\ 0& C\varphi & -S\varphi \\ 0& S\varphi & C\varphi \end{array}\right]=\left[\begin{array}{ccc}C\psi C\theta & C\psi S\theta S\varphi -S\psi C\varphi & S\varphi S\psi +C\psi S\theta C\varphi \\ C\theta S\psi & C\varphi C\psi +S\varphi S\theta S\psi & S\psi S\theta C\varphi -C\psi S\varphi \\ -S\theta & S\varphi C\theta & C\varphi C\theta \end{array}\right]$ (1)

其中 $S\theta$ 形式代表 $\sin \theta$，其它的符号以此类推。 $\psi ,\theta ,\varphi$ 分别表示偏航、俯仰、滚转角度。在机体坐标系下，设四个旋翼的升力为 $F_i\left(i=1,2,3,4\right)$，则四旋翼受到的升力F_B可表示为：

Figure 1. Mathematical description of quadrotor system

图1. 四旋翼系统的数学描述

$\{\begin{array}{l}{F}_B={\left[F_x,F_y,F_z\right]}^{\text{T}}={\left[0,0,U_1\right]}^{\text{T}}\\ U_1=F_1+F_2+F_3+F_4\end{array}$ (2)

利用上面得到坐标转换矩阵R得出地面惯性坐标系下的升力F_E为：

$F_E={\left[F_X,F_Y,F_Z\right]}^{\text{T}}=U_1{\left[S\varphi S\psi +C\psi S\theta C\varphi ,S\psi S\theta C\varphi -C\psi S\varphi ,C\varphi C\theta \right]}^{\text{T}}$ (3)

忽略四旋翼所受的空气阻力可得：

$\{\begin{array}{l}\ddot{X}=U_1\left(S\varphi S\psi +C\psi S\theta C\varphi \right)/m\\ \ddot{Y}=U_1\left(S\psi S\theta C\varphi -C\psi S\varphi \right)/m\\ \ddot{Z}=\frac{U_{1}C\varphi C\theta }{m}-g\end{array}$ (4)

令 $I_X,I_Y,I_Z$ 为四旋翼三个轴上的转动惯量， $U_2,U_3,U_4$ 为四旋翼的滚转力矩、俯仰力矩和偏航力矩，假设四旋翼的结构是完全对称的，忽略陀螺效应和空气阻力，四旋翼的小角度运动模型如下：

$\{\begin{array}{l}\ddot{\varphi }=\frac{U_2}{I_X},\left(U_2=kb\left(w_2^2-w_4^2\right)\right)\\ \ddot{\theta }=\frac{U_3}{I_Y},\left(U_3=kb\left(w_3^2-w_1^2\right)\right)\\ \ddot{\phi }=\frac{U_4}{I_Z},\left(U_2=kb\left(w_1^2+w_3^2-w_4^2-w_2^2\right)\right)\end{array}$ (5)

最后我们将四旋翼的数学模型简化如下：

$\{\begin{array}{l}\ddot{X}=U_1\left(S\varphi S\psi +C\psi S\theta C\varphi \right)/m\\ \ddot{Y}=U_1\left(S\psi S\theta C\varphi -C\psi S\varphi \right)/m\\ \ddot{Z}=\left(U_{1}C\varphi C\theta /m\right)-g\\ \ddot{\varphi }=U_2/I_X\\ \ddot{\theta }=U_3/I_Y\\ \ddot{\phi }=U_4/I_Z\end{array}$ (6)

3. 模糊自适应PID控制系统

四旋翼无人机模型是非线性的，在实际飞行过程中，环境发生变化，传统PID控制难以实现稳定的控制。设计一种模糊自适应PID控制系统，对四旋翼仿真系统输出的悬停信号与期望悬停信号取差，遵照设定好的模糊规则，根据悬停信号差值和差值变化率，得到相应的PID参数的增量，实现PID参数的调整 [9]。

反馈悬停信号与期望悬停信号差值和差值变化率分别为e和ec，模糊自适应PID控制结构框图如图2。

Figure 2. The diagram of the fuzzy adaptive PID control structure

图2. 模糊自适应PID控制框图

以悬停信号差值以及悬停信号差值变化率为输入，比例 $k_p$ 、积分 $k_i$ 、微分 $k_d$ 的模糊矩阵如图3~5。

设“NB，NM，NS，Z，PS，PM，PB”为“−3，−2，−1，0，1，2，3”，根据e和ec，遵照模糊矩阵的整定规则按照式(7)对比例 $k_p$ 、积分 $k_i$ 、微分 $k_d$ 的参数进行调节。

$k_p=k_{p0}+\Delta k_p,k_i=k_{i0}+\Delta k_i,k_d=k_{d0}+\Delta k_d$ (7)

Figure 3. $k_p$ setting rules

图3. $k_p$ 整定规则

Figure 4. $k_i$ setting rules

图4. $k_i$ 整定规则

Figure 5. $k_d$ setting rules

图5. $k_d$ 整定规则

4. 基于粒子群优化的模糊自适应PID参数优化结构

粒子群优化算法(PSO，又叫鸟群觅食算法)：是1995年由Eberhart博士和Kennedy博士提出，源于对鸟群捕食的行为研究。我们把待优化问题的解看作一个粒子，粒子经过迭代寻找最优解，实现对参数的优化 [10] [11]。

粒子群优化算法的位置和速度更新公式如下：

$v_{k+1}=\omega v_k+c_{1}rand\left(\right)\left(pbes{t}_k-x_k\right)+c_{2}rand\left(\right)\left(gbes{t}_k-x_k\right)$ (8)

$x_{k+1}=x_k+v_{k+1}$ (9)

式中， $\omega$ 称为惯性权重，惯性权重的大小决定粒子当前速度继承的大小； $v_k$ 是粒子的速度； $x_k$ 是当前粒子的位置； $pbes{t}_k$ 表示粒子本身找到的最优解的位置； $rand\left(\right)$ 是介于(0,1)之间的随机数； $gbes{t}_k$ 表示整个种群目前找到的最优解的位置； $c_1$ 和 $c_2$ 称为学习因子；通过改变 $c_1$ 和 $c_2$ 值的大小可以调整 $pbes{t}_k$ 和 $gbes{t}_k$ 对粒子吸引的影响强度。

本文设计的基于粒子群优化的模糊自适应PID控制器，通过粒子群优化算法对 $k_p$ 、 $k_i$ 、 $k_d$ 三个参数进行寻优，同时通过实际输出的悬停信号与目标悬停信号的差值e，差值变化率ec，按照制定好的模糊规则得到比例 $k_p$ 、积分 $k_i$ 、微分 $k_d$ 三个参数的增量，即 $\Delta k_p$ 、 $\Delta k_i$ 、 $\Delta k_d$，然后按照式(7)得到优化的参数。基于粒子群优化的模糊自适应PID控制器参数优化结构框图和仿真流程图如图6，图7。

Figure 6. Fuzzy PID control block diagram based on PSO

图6. 粒子群优化模糊PID控制框图

Figure 7. Flow chart of fuzzy PID simulation based on PSO

图7. 粒子群优化模糊PID仿真流程图

1) 粒子群优化算法初始化。待优化的参数分别为比例 $k_p$ 、积分 $k_i$ 、微分 $k_d$，维数为3；设定粒子群算法的惯性权重 $\omega$ 为1；为了方便优化，本文构造50个粒子，40次迭代，初始的搜索范围设计在已知数据的上下空间内。

2) 适应度值的计算。本文对四旋翼悬停控制的自适应PID控制器的比例、积分、微分的参数优化，采用ITAE指标作为适应度函数，即时间与绝对误差乘积的积分。它可以很好地体现系统的偏差与时间的关系，数值越小时控制性能越好，公式为：

$J={\displaystyle \underset{0}{\overset{\infty }{\int }}t}\left|e\left(t\right)\right|\text{d}t$ (10)

3) 粒子群优化算法的更新和终止。粒子群优化仿真主要分为两个环节，一个是粒子群优化寻优的m文件，另一个是Simulink仿真模型，这两个环节是相互联系合作的。在优化过程中，粒子群算法构造粒子并在Simulink模型中仿真计算得到适应度值，然后将得到的适应度值传递到粒子群算法进行下一代的迭代计算。如此循环往复，直到达到预先设定好的指标或最大迭代次数时，终止算法。将仿真计算得出的比例、积分、微分参数作为自适应PID控制器的参数输入到位置控制仿真模型中，控制无人机的悬停。

在四旋翼无人机悬停控制仿真中，采用仿真模型的运行时间与系统偏差的绝对值来计算适应度值：

$J={\displaystyle \underset{0}{\overset{T}{\int }}t}\left|e\left(t\right)\right|\text{d}t$ (11)

上式中，T为仿真模型运行时间，e为系统偏差。

5. 系统仿真及结果

5.1. 基于粒子群优化的模糊自适应PID控制器的参数优化仿真

四旋翼的仿真控制模型主要包括两个回路：一个是内环的姿态控制回路，另一个是外环的位置控制回路 [12] [13]。定点模式下的四旋翼仿真模型主要包括x、y、z三个输入阶跃信号的悬停控制器。本文是在四旋翼仿真模型的位置控制封装模块中加入了三个粒子群优化模糊自适应PID控制器，来控制x、y、z三个悬停控制器。在MATLAB的Simulink模块构建了四旋翼定点模式下的仿真模型如图8。

Figure 8. Quadrotor simulation model

图8. 四旋翼仿真模型

仿真过程包括粒子群优化模糊自适应控制器仿真模型、模糊PID控制程序、粒子群优化控制程序和Simulink仿真模型连接的程序、粒子群优化控制程序四部分。

第一部分：在图8四旋翼仿真模型的位置控制模块中加入三个粒子群优化的模糊自适应PID控制器，分别控制，x、y、z三个输入。粒子群优化模糊自适应控制器主要包括两个模块：一个是模糊自适应PID控制器，另一个是粒子群优化PID控制器。x、y、z三个输入的粒子群优化模糊自适应PID控制器原理相同，以x输入的控制器为例，位置控制模块、粒子群优化模糊自适应PID控制器模块如图9、图10。

第二部分：构建模糊自适应PID控制程序的m文件，初始化参数，确定描述输入输出变量语言值的模糊子集，并设置输入输出变量的论域，利用Matlab模糊控制工具箱设计仿真模型中的模糊自适应PID控制器。

第三部分：在图7中，构建m文件，编写连接四旋翼Simulink仿真模型与粒子群优化算法的程序。在m文件中将x(1)、x(2)、x(3)的值赋给 $k_p$ 、 $k_i$ 、 $k_d$，将参数导入仿真模型中进行仿真，并将得到的ITAE性能指标传递给粒子群优化算法，用于粒子群的迭代更新。

第四部分：编写粒子群优化算法程序，在编写好的程序中调用第三部分的连接程序，将由Simulink仿真模型得到的适应度值调用到编写好的算法当中。

5.2. 仿真结果比较分析

本文对四旋翼x，y，z三个悬停控制器进行了控制，在四旋翼位置控制模块(如图9)加入了三个粒子群优化模糊自适应PID控制器(如图10)，控制原理相同。以四旋翼定点模式下的仿真模型的x输入为例进行仿真结果的比较分析。

Figure 9. Position control module

图9. 位置控制模块

Figure 10. Fuzzy adaptive PID controller based on PSO

图10. 粒子群优化模糊自适应PID控制器

普通PID参数 $k_p=100$， $k_i=2$， $k_d=88$ ；采用模糊自适应PID控制后参数 $k_p=\text{379}$， $k_i=\text{0}$， $k_d=\text{200}$ ；采用粒子群优化算法之后得到四旋翼x控制器最优控制参数为别为 $K_p=500$， $K_i=0$， $K_d=200$，最优适应度值为 $J=268$，粒子群优化选优过程中的适应度值的变化如图11。

将加入粒子群优化算法后的模糊自适应PID控制器的仿真结果与模糊自适应PID控制及普通PID控制仿真结果放到一个图进行比较，仿真结果如图12。

图12中，曲线1代表普通PID对阶跃信号的响应曲线，曲线2表示未加入粒子群优化算法的模糊自适应PID控制器的悬停信号输出曲线，曲线3代表使用粒子群优化算法优化之后的悬停信号输出曲线。由图可以看出，模糊自适应PID控制的抗干扰性和稳定性要优于普通PID控制。在响应阶跃悬停命令的过

Figure 11. Fitness curve with iteration

图11. 适应度值随迭代次数变化曲线

Figure 12. Comparisons of simulation results

图12. 仿真结果比较图

程中，模糊自适应PID控制的超调更小，波动也更少；在对四旋翼进行悬停控制时，模糊自适应PID控制抗干扰性能好，波动性小，而只采用普通PID控制算法容易受到外界影响，容易失稳。普通PID控制和模糊自适应PID控制都可以使悬停输出信号趋于稳定，但存在大约4%的稳态误差。经过粒子群优化算法优化过后的悬停信号输出曲线调节时间更短、上升时间更短、超调量更小、稳态误差更小，具有更好的控制性能。

6. 结论

本文构建了四旋翼定点模式下的悬停控制模型，提出了一种粒子群优化模糊自适应PID控制器的控制策略，并将其应用到构建的模型当中去。结果表明，加入粒子群优化算法的模糊自适应PID控制系统，控制效果达到显著的提升。通过不同控制策略下的系统性能对比，得到如下结论：

1) 普通PID控制在系统参数变化或外部扰动情况下，达不到理想的控制效果。

2) 模糊自适应PID控制，是一种可以在线整定的控制策略，可以根据偏差和偏差变化率的不同，调整PID参数，达到良好的控制效果。将模糊自适应PID控制应用到四旋翼悬停控制中，仿真结果验证了该控制方法的可行性，但是存在实际输出存在一定的稳态误差的问题。

3) 采用粒子群优化的模糊自适应PID控制器的策略，对系统PID参数进行优化，确保了四旋翼悬停控制系统的动态性能和稳定性，通过仿真验证了该控制方法可以使系统输出达到稳定、准确、快速的要求。相较于传统的控制方法，粒子群优化模糊自适应PID控制的方法具有稳定性好、调整速度快、鲁棒性强的特点。