基于改进被囊群算法的四旋翼姿态控制PID整定

PID Parameter Tuning of Quadrotor Attitude Control Based on Mended Tunicate Swarm Algorithm

摘要: 四旋翼无人机模型具有非线性、强耦合、欠驱动的特性，其姿态控制的PID参数整定非常困难。针对上述问题，将被囊群算法引入姿态控制器，进行参数优化。并在原有算法的基础上引入混沌初始化、逐维反向学习策略和自适应权重因子，以此来提高算法的全局探索能力和局部开发能力，从而有效地对PID参数进行整定。最后，通过在MATLAB/Simulink中搭建模型并进行虚拟仿真。经测试验证，运用该算法优化的PID控制器有更好的控制精度和效率。

Abstract: The Quadrotor UAV model has the characteristics of non-linear, strong coupling and underactuated, and it is very difficult to tuning the PID parameters of its attitude control. To solve these problems, The Tunicate Swarm Algorithm is introduced into the attitude controller to optimize the parameters. On the basis of the original algorithm, chaotic initialization, the opposition-based learning strategy and adaptive weight factor are introduced to improve the global exploration ability and local development ability of the algorithm, so as to effectively tune the PID parameters. Finally, the model was built and virtually simulated in MATLAB/Simulink. The test results show that the PID controller optimized by this algorithm has better control accuracy and efficiency.

1. 引言

近几十年来，四旋翼无人机(unmanned aerial vehicle, UAV)因其机动性好、悬停能力强、可靠性高的特点，被广泛应用于遥感航拍、救灾、农业植保等领域。四旋翼无人机工作环境的不断多样化，使其对稳定性的要求越来越高。四旋翼飞行器的姿态控制多采用结构简单、适应性好的PID控制器。但是在实际应用环境中，由于四旋翼模型具有非线性、高耦合等特点，采用常规的PID控制器，参数整定复杂且性能很差，很难达到预想控制效果。因此智能优化算法的引入极有必要。

文献 [1] 针对四旋翼无人机特性，设计了一种模糊PID迭代学习控制(Iterative learning control, ILC)。采用模糊控制对PID参数进行设置，抑制不确定因素对系统的影响。文献 [2] 提出了一种基于改进遗传算法的PID控制方案，并分别采用条件选择最优策略和自适应变分策略避免算法早熟收敛。文献 [3] 采用遗传算法对PID参数进行整定。为避免过早收敛，根据适应度值和进化过程使用不同概率水平的交叉和变异规则，取得良好性能。

本文采用改进被囊群算法(Reverse learning of Tunicate Swarm Algorithm, RTSA)对PID参数进行整定。该算法针对标准被囊群算法(Tunicate Swarm Algorithm)收敛速度慢、易陷入局部最优等不足，引入多重改进机制，提高收敛速度和寻优精度，以满足PID参数整定需求。

Figure 1. Model for the Quadrotor UAV

图1. 四旋翼无人机模型

2. 四旋翼飞行器数学模型

在惯性坐标系和机体坐标系中建立四旋翼数学模型。四旋翼飞行器的运动与其平移(x, y, z)和旋转( $\phi$ ：横滚、 $\theta$ ：俯仰、 $\psi$ ：偏航)有关，横滚是x轴上的旋转，俯仰是y轴上的旋转，偏航是z轴上的旋转，如图1所示 [4] 。

可根据牛顿–欧拉方程建立四旋翼的动力学模型。在建模过程中假定飞行器完全对称且陀螺效应和空气阻力影响较小，忽略上述因素的影响 [5] 。

位置动力学模型为：

$\begin{array}{l}\ddot{x}={\displaystyle \underset{i=1}{\overset{4}{\sum }}{K}_t{w}_i^2\left(\cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \right)/m}\\ \ddot{y}={\displaystyle \underset{i=1}{\overset{4}{\sum }}{K}_t{w}_i^2\left(\sin \psi \sin \theta \cos \varphi -\sin \varphi \cos \psi \right)/m}\\ \ddot{z}={\displaystyle \underset{i=1}{\overset{4}{\sum }}{K}_t{w}_i^2\left(\cos \theta \cos \varphi \right)/m-g}\end{array}$ (1)

其中， $w_i\left(i=1,2,3,4\right)$ 为螺旋桨转速， $K_t$ 为升力系数。

由于飞行器为对称刚体，所以其惯性力为：

$J=\left[\begin{array}{ccc}{J}_x& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]$ (2)

则姿态动力学模型为：

$\begin{array}{l}{\stackrel{\dot{}}{\omega }}_x=\left[l\left(F_4-F_2\right)+\left(J_z-J_y\right){\omega }_y{\omega }_z\right]/J_x\\ {\stackrel{\dot{}}{\omega }}_y=\left[l\left(F_3-F_1\right)+\left(J_z-J_x\right){\omega }_x{\omega }_z\right]/J_y\\ {\stackrel{\dot{}}{\omega }}_z=\left[K_d\left({\omega }_1^2-{\omega }_2^2+{\omega }_3^2-{\omega }_4^2\right)+\left(J_x-J_y\right){\omega }_x{\omega }_y\right]/J_z\end{array}$ (3)

其中， $w_x,w_y,w_z$ 为角速度在机体轴上的三个分量， $K_d$ 是阻力系数， $F_i\left(i=1,2,3,4\right)$ 为机翼所受拉力。

可简化飞行器的控制分析：

$\left[\begin{array}{l}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{c}{F}_1+F_2+F_3+F_4\\ F_4-F_2\\ F_3-F_1\\ F_2+F_4-F_1-F_3\end{array}\right]=\left[\begin{array}{c}{K}_t{\displaystyle \underset{i=1}{\overset{4}{\sum }}{w}_i^2}\\ K_t\left(w_4^2-w_2^2\right)\\ K_t\left(w_3^2-w_1^2\right)\\ K_d\left(w_1^2+w_3^2-w_2^2-w_4^2\right)\end{array}\right]$ (4)

其中U₁，U₂，U₃，U₄分别为垂直方向和三个姿态角(滚转角、俯仰角、偏航角)的输入控制量。

综合(1)、(3)、(4)式可得到四旋翼的数学模型为：

$\begin{array}{l}\ddot{x}=\left(\cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \right)U_1/m\\ \ddot{y}=\left(\sin \psi \sin \theta \cos \varphi -\sin \varphi \cos \psi \right)U_1/m\\ \ddot{z}=\left(\cos \theta \cos \varphi \right)U_1/m-g\end{array}$

$\begin{array}{l}\ddot{\varphi }=\left[l{U}_2+\stackrel{\dot{}}{\theta }\stackrel{\dot{}}{\psi }\left(J_y-J_z\right)\right]/J_x\\ \ddot{\theta }=\left[l{U}_3+\stackrel{\dot{}}{\varphi }\stackrel{\dot{}}{\psi }\left(J_z-J_x\right)\right]/J_y\\ \ddot{\psi }=\left[U_4+\stackrel{\dot{}}{\varphi }\stackrel{\dot{}}{\theta }\left(J_x-J_y\right)\right]/J_z\end{array}$ (5)

3. 标准被囊群(TSA)算法原理

TSA是一种元启发式优化算法，其将被囊群体觅食过程中的喷气推进和群体行为作为算法自身寻优过程中所采用的行进方式。其独特之处在于，对搜索区域未知的案例有较大的研究意义。然而，在给定的搜索空间中没有关于食物来源的概念，被囊动物觅食行为中所采用的喷气推进和群体智能两种行为被用于TSA寻优过程。但在实际寻优中，需要满足避免个体冲突、向最佳位置移动和保持接近最佳个体三个条件，才能实现喷气推进行为。而其他搜索个体与最优解之间的位置关系则通过群体行为完成更新 [6] 。

3.1. 避免搜索个体之间的冲突

向量 $\stackrel{\to }{A}$ 用于计算新的搜索个体位置。

$\stackrel{\to }{A}=\frac{\stackrel{\to }{G}}{\stackrel{\to }{M}}$ (6)

$\stackrel{\to }{G}=c_2+c_3-\stackrel{\to }{F}$ (7)

$\stackrel{\to }{F}=2\cdot c_1$ (8)

其中， $\stackrel{\to }{G}$ 为重力作用； $\stackrel{\to }{F}$ 为水流平流； $c_1、c_2、c_3$ 是[0, 1]之间的随机数； $\stackrel{\to }{M}$ 为个体间的互相作用力，其计算如下式：

$\stackrel{\to }{M}=\lfloor P_{\min }+c_1\cdot P_{\max }-P_{\min }\rfloor$ (9)

其中， $P_{\min }$ ， $P_{\max }$ 是初始作用速度范围。在此， $P_{\min }$ 和 $P_{\max }$ 为1和4。

3.2. 向最优邻居方向移动

在避免了冲突之后，个体自身朝向最优邻居移动。

$\stackrel{\to }{PD}=\left|\stackrel{\to }{FS}-r_{and}\cdot \stackrel{\to }{P_p\left(x\right)}\right|$ (10)

其中， $\stackrel{\to }{PD}$ 为个体与食物源之间的距离，x为当前迭代次数， $\stackrel{\to }{FS}$ 为食物源位置， $\stackrel{\to }{P_p\left(x\right)}$ 是个体位置， $r_{and}$ 为[0, 1]的随机数。

3.3. 收敛于最好的搜索个体

搜索个体保持其自身朝向即食物源的位置靠近。

$\stackrel{\to }{P_p\left(x\right)}=\{\begin{array}{l}\stackrel{\to }{FS}+\stackrel{\to }{A}\cdot \stackrel{\to }{PD},\text{if}{r}_{and}\ge 0.5\\ \stackrel{\to }{FS}-\stackrel{\to }{A}\cdot \stackrel{\to }{PD},\text{if}{r}_{and}<0.5\end{array}$ (11)

其中， $\stackrel{\to }{P_p\left(x\right)}$ 是被囊动物相对于食物源的位置 $\stackrel{\to }{FS}$ 的更新位置。

3.4. 群体行为

被囊动物的群体行为中，搜索个体位置根据最佳搜索个体的位置进行更新，其过程如下所示：

$\stackrel{\to }{P_p\left(x+1\right)}=\frac{\stackrel{\to }{P_p\left(x\right)}+\stackrel{\to }{P_p\left(x+1\right)}}{2+c_1}$ (12)

4. 改进被囊群(RTSA)算法

本文对被囊群算法的不足之处进行分析研究，并结合其他智能算法改进的原理，提出一种改进的被囊群(RTSA)算法。

4.1. Sin混沌初始化

全局收敛速度和收敛精度受优化算法初始种群质量的影响 [7] 。TSA算法在初始化阶段，搜索种群的位置是随机所产生的，这种随机的方式往往会使搜索区域内的初始种群分布分散性不够，从而导致搜索精度不够，速度太慢。针对上述TSA算法所存在的问题，采用混沌映射对种群初始化进行改良，极大的改变了粒子在搜索空间的分布状况。相较于常规的Logistic模型，本文采用的混沌模型映射折叠次数是无限的，其混沌特性更加优越 [8] 。Sin混沌1维映射表达式如下：

$\{\begin{array}{l}{x}_{n+1}=\sin \frac{2}{x_n},n=0,1,\cdot \cdot \cdot ,N\\ -1\le x_n\le 1,x_n\ne 0\end{array}$ (13)

利用Sin混沌产生的初始化种群，种群多样性得以提升，针对算法容易陷入局部最优的不足之处有一定的改进作用。

4.2. 逐维反向学习策略

标准的TSA算法极易陷入局部最优，将反向学习策略引入原始算法，有助于算法寻得最优解 [9] 。因此，本文在原始被囊群算法的个体位置更新中引入逐维小孔成像反向学习策略，对一个可行解逐维求反向解，能增强种群多样性，使粒子遍布搜索区域，使算法具有摆脱局部最优的能力 [10] 。逐维小孔成像反向学习公式如下：

${X^{\prime }}_{\text{best}}^j=\frac{low+up}{2}+\frac{low+up}{2n}-\frac{X_{\text{best}}^j}{n}$ (14)

其中， $X_{\text{best}}^j$ 是第j维最优解； ${X^{\prime }}_{\text{best}}^j$ 是其反向解；up、low为第j维解的上下限；n为调节因子。

利用(14)式产生一个由反向解构成的，且与原种群数量相等的新种群，再将新旧种群所有个体按照适应度值排序，选取适应度最佳的前二分之一作为后续新种群。

4.3. 非线性自适应权重

在标准的TSA算法中，个体的位置更新发生在喷气推进行为之后。由公式(12)可知，新个体位置的决定因素有三个：个体当前位置、上一代个体位置和一个[0, 1]的随机参数。该参数的大小决定了位置更新的步长，影响了算法的开发能力与探索能力 [11] 。

本文使用非线性自适应权重因子代替标准TSA算法群体行为中的随机参数，平衡了算法在迭代前期和后期的探索和开发能力。非线性自适应权重c表达式如下：

$c=\{\begin{array}{l}\left|\cos \left(5\pi t/4T_{\max }\right)\right|t<T_{\max }/2\\ 6/5\sin \left(\pi t/T_{\max }-\pi /4\right)t\ge T_{\max }/2\end{array}$ (15)

4.4. 算法步骤

5. 四旋翼飞行器控制系统设计优化

Figure 2. Block diagram of quadcopter UAV control

图2. 四旋翼无人机控制框图

四旋翼飞行器的控制依赖于四个旋翼的转速调节。通常采用串级PID进行控制，包括控内环的姿态控制器和外环位置控制器。本文在此基础上，采用RTSA算法对常规PID参数进行整定，进而对四旋翼进行控制。在控制回路中，将输出的姿态角与输入信号的偏差积分值(ITAE)返回给RTSA算法，作为算法寻优的适应度值进行迭代搜索 [12] 。控制框图如图2所示。

在姿态环中引入RTSA算法对三个姿态角PID控制参数进行迭代寻优，基于RTSA算法的无人机姿态环设计如图3所示。

Figure 3. PID simulation of UAV attitude angle based on RTSA

图3. 基于RTSA算法的无人机姿态角PID仿真

6. 测试仿真

为验证算法优越性，本文分别利用测试函数与四旋翼飞行器模型进行算法测试与仿真验证。

6.1. 算法测试

测试采用6个基准测试函数，见表1。

Table 1. Benchmark functions

表1. 基准测试函数

F₁~F₄为单峰高维函数，可测试算法精度。F₉、F₁₁为多维高峰函数，可用于测试函数跳出局部最优的能力。将RTSA算法与标准TSA算法进行对比实验 [13] ，设置相同参数，种群数N = 30、最大迭代次数T_max = 500，测试结果如图4所示。

Figure 4. Benchmark functions

图4. 测试函数收敛曲线图

从测试结果可以看出，标准TSA算法对上述6个基准测试函数的适应度值分别为7.0656e−195、5.7647e−99、4.0457e−174、3.7914e−90、178.54、0；改进的RTSA算法适应度值则分别为0、3.9824e−185、3.3404e−317、5.15e−166、0、0。从F₁~F₄可以看出，改进后算法精度在原算法基础上大大提高，且收敛速度更快；F₉、F₁₁用于验证改进后的算法具有更好的寻优能力，能更好的跳出局部最优。如图4中F₉测试图所示，标准被囊群算法已陷入局部最优，改进的被囊群算法则保持较好的收敛速度和精度。

6.2. 仿真验证

根据本文所述的四旋翼动力学模型，在MATLAB/Simulink中搭建仿真模型，对算法优越性进行仿真验证 [14] ，四旋翼飞行器的系统参数为：无人机质量为0.65 kg，重力加速度为9.8 N/kg，四旋翼绕X轴的转动惯量为7.5e−3 kg∙m²，绕Y轴的转动惯量为7.5e−3 kg∙m²，绕Z轴的转动惯量为1.3e−2 kg∙m²，四旋翼臂长0.23 m。

本文分别采用TSA算法与RTSA算法对串级PID进行优化，滚转角、俯仰角、偏航角的跟踪偏差对时间积分随迭代次数变化如图5所示。

(a) (b) (c)

Figure 5. Relationship between fitness values with the number of iterations

图5. 适应度值与迭代次数关系

通过对比TSA和RTSA两种算法的迭代次数与适应度值的关系曲线，得出对于该模型RTSA算法有更好的优化效果，相较于标准TSA算法有更好的收敛速度和精度，算法寻优结果更趋于全局最优。不同算法收敛精度与收敛速度见表2。

Table 2. Optimization results of different algorithms

表2. 不同算法寻优结果

(a) (b) (c)

Figure 6. Comparison of quadrotor attitude angle curves in three control schemes

图6. 三种控制方案四旋翼姿态角曲线对比

对比传统串级PID、TSA优化的PID以及RTSA优化的PID三种控制方案对四旋翼姿态角的控制效果，以此验证RTSA算法的优越性。设定无人机期望值，对其姿态角进行跟踪，三种控制方案的控制效果如图6所示。

从图6仿真结果可以看出，经RTSA算法整定的PID控制策略，相较于其他两种控制方案超调量更小、调节时间更短。由图6(a)，图6(b)结果来看，RTSA算法响应时间(均在5 s左右) < TSA算法调节时间(6 s左右) < PID控制器调节时间(7~8 s)，稳态误差为0。图6(a)亦可看出，相较于PID与TSA，经RTSA算法整定的滚转角调节曲线超调量更小。观察图6(c)可得，RTSA算法曲线上升更快，调节时间更短，更平稳。

综上所述，经RTSA算法整定后，三个姿态角可以较快的响应，以满足四旋翼飞行的姿态稳定性需求。相较于PID控制策略，RTSA-PID的控制性能有一定的提高，可实现快速响应。

7. 结论

针对四旋翼姿态控制中传统PID参数整定困难、稳定性不足的问题，在控制器中引入改进被囊群算法(RTSA)。该算法在标准TSA算法基础上，通过改变初始化和位置更新机制，并在群体位置更新中引入非线性自适比例因子，提高算法的寻优精度和收敛速度。通过MATLAB进行算法测试与方针验证，结果显示，改进的被囊群(RTSA)算法能够有效的对PID参数进行整定，提高系统控制效率和精度。

参考文献

NOTES

^*通讯作者。