微小型无人直升机建模与航迹跟踪控制

2022-03-15代冀阳孙翊君

计算机仿真 2022年2期

聂航，代冀阳，应进，孙翊君

(南昌航空大学信息工程学院，江西南昌 330063)

1 引言

近年来，由于直升机具有一些独特的特性，例如垂直起降，悬停和低空低速飞行等，人们对其关注度越来越高[1]。无人直升机可在任何环境飞行，使其在军事和民用领域具有很高的应用价值[2，3]。无人直升机是一种重要的动态耦合和高度非线性、强耦合，欠驱动系统，具有六个自由度和四个直接控制量[2，4]，使得其研究难度非常大。

目前，无人直升机控制算法中，PID控制方法是使用最广泛的方法[5，6]，传统的PID控制方法具有实现简单，鲁棒性好，可靠性高的优点，但其参数调整困难，一般是根据经验和仿真调试获得的，控制效果难以达到期望值[7]。PID控制器的关键问题是精确有效地调整参数[7]。利用群体智能方法优化PID控制参数是一种可行而有效的方法[8]。文献[6]中提出设计时间与绝对误差积分(ITAE)性能指标下PID控制算法，该性能指标在缩短抑制时间中效果较好，但仅加入评价指标对PID控制器整定来说稍显欠缺。文献[9]中选择粒子群优化的适应度函数作为“积分绝对误差”(IAE)方法，该方法在抑制小偏差中效果较好，但是在抑制大偏差和抑制响应误差中效果较差，且抑制时间较长，而无人直升机模型较为复杂，不太适用于此类方法。文献[7]中提出对PSO的惯性权重进行动态调整，扩大了搜索面积，但是粒子的搜索的速度减慢，使得其收敛时间变长。文献[10]中提出一种均值粒子群优化算法，该方法对粒子群优化的快速性以及收敛性较好，但因缺少评价指标而使得错过最优解。文献[11]中提出自适应粒子群优化算法，提高了算法的效率，但算法过于复杂，且收敛时间变慢。文献[12]中提出采用了一种基于相似度动态调整惯性权重的方法(LPSO)，提高了算法的收敛时间，但该算法易陷入局部极小值点。

控制器参数直接影响着无人直升机模型性能。为提高控制器的性能，对PID控制器参数整定十分关键。本文在均值粒子群优化算法的基础上加入了评价指标，解决了文献[10]中容易错过最优解问题；同时对惯性因子进行了改进，解决了文献[6]中全局搜索和局部搜索存在一定的互斥关系问题；另外，在传统粒子群算法中加入简单的变异因子，简化了自适应粒子群算法，扩大了搜索范围，解决了文献[12]中算法易陷入局部极小值问题。通过仿真验证该算法对控制器的有效性以及快速收敛性。

2 无人直升机动力学建模

(1)

无人直升机的动力学方程与外力矩作用下的分力以及力矩方程

(2)

(3)

由上述式可推出，无人直升机的非线性动力学方程为

(4)

式中：m表示无人直升机质量；Ixx、Iyy、Izz分别表示直升机对OX轴、OY轴和OZ轴的惯性积。

无人直升机非线性系统是一个高阶、强耦合的复杂系统[2，4]。因此，本文采用文献[12]中的线性化模型以及辨识参数进行建模与求解。

无人直升机线性化处理，既能简化方程组的计算，又能充分描述无人直升机的实际情况。平衡点处采用泰勒级数展开方式，以基准运动状态和小扰动偏差量为基础，忽略二阶及其以上导数。考虑配平条件为悬停状态。本文利用参数化模型和状态输入的扰动关系，得到线性状态空间模型。线性状态空间模型表达式为：

(5)

状态变量为

(6)

控制向量为

(7)

建立好线性状态空间、状态变量和控制变量后，将文献[13]中的参数代入到状态空间的矩阵A与矩阵B中，得到亚拓600型无人直升机的线性化模型。

3 进粒子群算法

3.1 传统粒子群算法

文献[14]中提出传统粒子群算法，并提出了每次迭代的个体极值和全局极值的速度与位置更新公式为

vt+1=wvt+c1r1+(Pt-xt)+c2r2(Gt-xt)

(8)

xt+1=xt+vt+1

(9)

式中：v为粒子速度；w表示惯性因子；x表示粒子位置；c1、c2表示加速常量；r1、r2表示范围[0，1]之间的随机数。Pt和Gt分别表示当前搜索到的个体极值和全局极值最优位置。

传统粒子群算法缺陷：精度低、易发散。如果加速系数或最大速度等参数过大时，容易错过最优解而出现不收敛情况。在收敛情况下，粒子群易出现同一化而失去了多样性，使得后期收敛速度变慢，无法继续优化，而出现精度不高等问题。现如今对粒子群的改进方法很多，如协同粒子群算法、随机离子群算法和耗散粒子群算法等。

3.2 改进粒子群算法

粒子群算法对于全局搜索能力要求很高。但当Vmax过小时，无法达到最大搜索能力，只能局部搜索，无论惯性因子w取多大；如果Vmax过大时，可以通过惯性因子w，提供很多的搜索范围。由此可知，最大速度Vmax和惯性因子w间接或直接影响全局搜索能力，必须找一个平衡全局搜索和局部搜索的惯性因子w的值。

1)改进惯性因子

为解决上述问题，本文采用逐渐降低惯性因子w的方式进行改进，从而使其搜索面积更大，可以较快的选取最优解的大致区域。而随着惯性因子w的值降低，粒子搜索速度减慢，搜索更加细致，提高了算法的性能。其改进的惯性因子公式为

(10)

2)粒子群速度修正

除了惯性因子w，速度对全局搜索能力也影响十分明显。在搜索时，粒子们要综合考虑自身与同伴之间的优劣不断调整自身方向。本文将修正速度公式，对当前的个体极值与全局极值取均值。这样在搜索中更能快速考虑到自身与同伴之间的优劣，更快的搜索到全局最优值。粒子群速度修正公式为

vt+1=wvt+c1r1((Pt+Gt)/2-xt)…

+c2r2((Pt+Gt)/2-xt)

(11)

3)加入自适应变异

针对传统粒子群算法易出现早收敛、搜索精度低、后期收敛速度变慢等缺点。本文将借鉴遗传算法的变异想法，对粒子群算法进行变异操作，维持种群的多样性。变异操作可以扩大种群的搜索空间，使得粒子可以跳出之前搜索的最优值的位置，提高了算法寻找最优解的能力。本文在粒子群算法的基础上引入一个简单变异算子，粒子每次更新后，以一定的概率对粒子进行初始化。

(12)

式中：R表示更新的粒子；L表示重新初始化的粒子；r表示限定值。

算法步骤：

Step1：初始化粒子群。对粒子群各个参数进行设置：种群大小n，搜索空间M维，加速常量c1、c2，Kp、Ki、Kd以及终止条件等；

Step2：随机产生粒子群的速度Vi与位置Xi，并确定粒子群的当前个体最优值Pt与全局最优值Gt；

Step3：将粒子群中的粒子赋值给PID控制器参数Kp、Ki、Kd，然后运行系统的Simulink模型，输出评价指标反馈值(即粒子适应值)执行Step10；

Step4：将式(10)代入式(11)中更新粒子速度，根据式(9)更新当前粒子的位置；

Step5：根据式(12)判断是否超过限定值，若超过限定值，则执行Step6，若没有执行Step7；

Step6：重新初始化粒子；

Step7：计算每个粒子适应度；

Step8：每个粒子个体适应值Pi与其个体最优位置Pt适应值进行比较，若Pi>Pt，则将其替换为Pt；

Step9：每个粒子全局适应值Gi与全局最优位置Gt适应值进行比较，若Gi>Gt，则将其替换为Gt，返回步骤3；

Step10：若没有满足终止条件，则返回Step4，否则退出算法，得到最优解。

4 基本人工势场法航迹规划

4.1 传统人工势场法

传统人工势场法表示目标对机器人产生一个引力场，而障碍物会对机器人产生一个斥力场。机器人在运动过程中，受到目标的吸引力而向目标移动，同时还受到障碍给它的排斥力而远离障碍。

图1 传统人工势场法

传统人工势场中，无人直升机受到的引力场函数为

Uat(X)=0.5αtρ2(X，Xt)

(13)

无人直升机受到的斥力场函数为：

(14)

对上面的引力场与斥力场分别求取负梯度，可得引力场负梯度为

-∇Uat(X)=-αtρ(X，Xt)

(15)

斥力场负梯度为

-∇Uao(X)

(16)

传统人工势场法的缺陷分析：传统人工势场法容易出现目标不可达问题；容易陷入了局部极小值点；传统人工势场法缺少航路评价机制，无法判断航路优劣。

4.2 改进人工势场法

1)目标不可达问题改进方案：

针对上述问题，本节将对上节中的引力场函数和斥力场函数为进行改进。改进之后的引力场函数式为

(17)

改进之后的斥力场函数式为

(18)

类似的，求得引力函数负梯度为

(19)

求得斥力函数负梯度为

Fao(X)=-∇Uao(X)

(20)

式中：ρ1=max{ρomax，ρamax}，ρ2=min{ρomin，ρamin}；αt1和αt2表示无人直升机受到的引力增益常数；βo表示无人直升机受到的斥力增益常数。ρ(X，Xt)和ρ(X，Xi)分别表示无人直升机重心位置到目标和威胁的空间距离；ρamin表示无人直升机侦查到威胁影响的最小范围；ρamax表示无人直升机侦查到威胁影响的最大范围；ρomin表示无人直升机受到威胁影响的最小范围；ρomax表示无人直升机受到威胁影响的最大范围。

2)容易陷入局部极小值点问题改进方案：

针对上述问题，本节根据文献[15]引入一种“回环力”。该力定义在威胁斥力的垂直面上，方向与目标引力在斥力垂直面的投影方向一致，回环力大小可以通过斥力数值大小与回环力增益系数的乘积得到[15]。回环力函数表达式为

Fac(X)=

(21)

式中：γ1表示回环力增益常数；η1表示回环力作用距离常数。

4.3 无人直升机轨迹跟踪控制器设计

单机控制器主要运用改进人工势场法规划的路径作为期望轨迹，实时将规划的路径输入到无人直升机模型。运用Matlab与Simulink交互，将Matlab数据实时传输到Simulink模型里面。路径规划时均预留了10米距离，保证无人直升机飞行过程安全的同时，也为后续真实无人直升机航迹规划预留足够安全距离。

无人直升机模型分别从姿态、速度和位置控制器进行设计，来介绍无人直升机的轨迹跟踪器问题。其整体设计如图2所示，图中九个输出量分别为u、v、w、θ、φ、ψ、q、p、r。其中u、v、w为线速度，θ、φ、ψ为姿态角，q、p、r为姿态角速度。本文将采用串级PID控制的方法对无人直升机轨迹跟踪进行控制，其中位置控制器将包含坐标转换环节，速度控制器与姿态控制器将分别控制无人直升机飞行速度与姿态变化。期望轨迹实质就是引导无人直升机飞行，通过无人直升机模型，得到与真实无人直升机相近的轨迹。

图2 无人直升机控制器设计结构

5 仿真分析

对粒子群算法进行非线性函数极小值点寻优实验，实验选取函数如下所示

非线性函数1

(22)

非线性函数2

f(x，y)=x2+y2-10*cos(2πx)…

-10*cos(2πy)+20

(23)

非线性函数3

(24)

将本文提出的改进粒子群算法(IPSO)与文献[6]提出的PSO算法、文献[12]中的提出的LPSO算法进行对比实验。实验过程中，保证各组粒子群的初始化参数一致，并对每组算法进行20次重复实验，记录非线性函数运行20次后的最优值和平均值，以及平均迭代次数，最终汇总到表1与表2进行对比分析。具体结果如表1、表2所示。

表1 非线性函数运行20次后的结果比较

表2 非线性函数运行20次后的结果比较

从表1中可以看出，不论是三个函数的最优解还是20次运行结果的平均值，IPSO的算法精度都是最高的。从表2中可以看出来，IPSO算法的平均迭代次数也是最少的。这说明，IPSO算法不仅搜索能力强，而且能够快速找到最优解。

首先，根据粒子群算法对无人直升机控制器参数整定，具体参数整定如表3所示，其各个通道的适应度值，如图3所示。航迹规划算法首先设置一个三维空间，在三维空间中设置7个障碍物，并初始化改进人工势场法的各个参数。航迹规划起始位置为(-50，-50，0)，无人直升机真实位置为(0，0，0)，目标位置为(500，450，0)。本文无人直升机轨迹由改进人工势场法航迹规划路径进行引导，理论航迹与无人直升机飞行轨迹图，如图4所示。无人直升机姿态响应曲线，如图5所示。无人直升机误差响应曲线，如图6所示。

表3 粒子群算法整定的各控制器参数

图3 各个通道的适应度值

图4 理论航迹与无人直升机航迹图

图5 无人直升机姿态响应曲线

图6 无人直升机误差响应曲线

由图3可知，改进粒子群对姿态环的收敛速度最快，是由于姿态环处于内环。而位置环收敛速度最慢，是由于其处于外环。但从适应度值来看，速度环的适应度值最好。从三个环适应度曲线均在迭代10次时收敛，说明改粒子群算法对PID控制器的收敛速度很快。从适应度值看，三个环的适应度值普遍都很小，说明改进算法的寻优能力较快。

由图4可知，红色为无人直升机曲线，黑色为航迹规划曲线。蓝色的点为航迹规划起点，绿色点为目标终点。无人直升机首先从真实位置向航迹规划起点飞行，然后再根据航迹规划线路飞行。从图中可以看出，无人直升机真实的红色轨迹，与航迹规划的黑色轨迹重合程度高，且最终到达目标点。

由图5可知，由于航迹规划的为无规律轨迹，图中的姿态变化也较为复杂，从图中可以看出，其滚转角与俯仰角前期变化较大，但后期变化较小，而偏航角一直处于变化中，由于航迹规划中，无人直升机一直受到目标点的吸引力、障碍物的排斥力以及回环力而处于不规则运动状态，所以偏航角一直处于左右运动状态。

由图6可知，三个轴的轨迹误差中，X轴与Y轴初始误差较大，是由于无人直升机起始点为(0，0，0)，而规划起始点为(-50，-50，0)，但模型的响应速度很快，在短时间飞行到航迹路径后，X轴和Y轴的误差随即变得很小。Z轴误差较大处，为直升机上升转下降点处，且该处障碍物较多，导致误差超过2米，且直升机出现徘徊误差，但在之前航迹规划中已经预留10米的安全距离，所以误差处于被控范围内，出现徘徊误差，是由于航迹规划时直升机受到多个力而出现航迹徘徊。根据上述实验可知，该控制器对无规律航迹的跟踪较好。