基于改进粒子群算法的三维路径规划

谢勇宏,孔月萍

(西安建筑科技大学 信息与控制工程学院，西安 710055)

0 引言

伴随着科技手段的进步，在众多领域逐渐实现无人化，正在改变那些依靠人进行笨重且重复的工作。例如采用无人机方式在建造的建筑群中进行安全作业巡查，可提高工作效率，保证整个项目作业的安全系数[1]。无人机硬件系统具有较好的控制性能，例如四旋翼无人机等[2]。而设计合理高效的三维路径方法，将有助于无人机实现自主执行固定的飞行任务。

传统算法求解三维路径往往计算量大，过程复杂，而智能优化算法通过规则可在一定时间内得到较好的结果[3]。粒子群算法是一种具有收敛性快，实现思想简单等特点的群智能优化算法，依靠粒子的速度和位置不断更新来获取最佳值[4]。迭代过程中，更新规则的限制，会产生不合理的搜索方向，使得粒子的位置滞留在某个狭义最优解区域，不能到达整个可行解空间的最优[5]。

一般来说，求解的最优值与粒子初始位置有关，粒子群的初始化质量对于算法的收敛性有较大影响[6]。文献[7]采用混沌序列初始化粒子群，使得初始粒子质量更好，并且证明多种群机制下粒子分布更均匀，有益于提高搜索性能。为了增强粒子在搜索中的搜索能力，避免陷入局部最优。文献[8]证明通过引入牵引操作，有效帮助算法扩大搜索范围，提高求解性能。文献[9]借助遗传交叉变异策略提升粒子群的多样性，在全区域中能产生高质量的解；帮助减少适应度的拐弯次数，对路径简单化以提升求解效率。另外，粒子搜索过程易跌入局部低谷，这需要借助更有效的跳跃能力来产生足够大的步伐[10]。文献[11]考虑搜索过程中粒子的速度及群体的离散度会有变化，使得惯性权重自适应变化，避免过早收敛。在位置更新中通过自然选择方式保持粒子群的多样性，加快算法的收敛速度。

现阶段可供二次开发的无人机已经非常丰富，拥有较成熟的飞行控制硬件，但是面对的应用场景中环境因素不一样，运动规划系统仍需完善。为此，通过无人机开发系统提出了一种改进融合牵引力粒子的三维路径规划方法。在仿真平台中，通过三维障碍物坐标信息构造地形模型，验证方法的有效性。

1 系统结构及飞行原理

1.1 系统结构

无人机控制系统主要有前期输入的飞行任务，运动规划系统，对功能Agent传输的控制指令、各种功能Agent，状态检测装置。图1为无人机控制系统体系结构。

图1 无人机控制系统体系结构

1.2 三维路径的飞行功能实现

实现无人机的飞行任务，需要在飞行控制管理系统中录入预先设计的路径规划方法。管理人员根据需要选择目标位置点，无人机通过路径跟踪控制器按照路径规划方法优化之后的目标路径进行跟踪飞行控制。

运动规划系统属于控制手柄端的后台管理模块。为了使得无人机接收控制手柄段的三维目标路径信息，采用MSDK与树莓派端的OSDK架起通信链路，完成目标路径信息的交互。环境地形是进行无人机飞行时考虑的必不可少的因素，以下将基于环境地形模型，对三维路径规划方法展开分析设计，以求丰富运动规划系统的知识模型。

2 环境地形模型

在对地形模型建模时，应考虑切合实际，使得路径规划的效率更高。据此，基准的地形模型[12]为：

Z1(x,y)=sin(y+a)+

(1)

其中：x表示横坐标，y表示纵坐标，Z1表示区域的高，a、b、c、d、e、f、g为常数，来模拟地形项目中的高低分布。通过指数函数来布置外形数据点[13]。数学模型可以表示为：

(2)

式中，(xi,yi)表示第i个楼层的中心坐标位置；hi是一个高度的控制参数；xsi和ysi则分别是第i个楼层在x轴和y轴方向的衰减量，控制坡度；N表示山峰总的个数。

3 三维路径规划方法

无人机飞行轨迹要考虑环境地形的情况，一般环境地形越复杂，无人机的飞行难度越大，路径规划的难度也增大。通过第2节建立环境地形，采用立体数字网的形式描述整个三维空间，构造了三维轴线的网格数据。立体数字网的形式有利于简化环境地形的复杂性，即整个地形就是有序的三维点坐标。障碍物采用外形边缘的数据网格表示，存储为一个数据集，定义在数据集闭区间内的点坐标，无人机的飞行路径均需要避开绕道。

另一方面，粒子群算法对于处理散点数据具有优越性。在实际应用中，采用GPS定位当前位置并判断与环境坐标是否冲突，冲突即遭遇障碍物，无人机在路径追踪时需要避开当前位置，重新规划路线。在路径规划上可根据多个点坐标组成一个数据组，首尾点坐标为起始点与终止点。通过粒子寻优，数据组的中间段的点坐标分别分布在合适位置，通过曲线拟合方式可呈现规划的结果路径。

4 改进粒子群算法的设计

标准粒子群算法(PSO)的思想是通过随机产生给定数量的粒子群，依靠粒子本身记忆的最好位置及粒子群体之间的信息交流对全区域寻优。伴随着迭代次数的增加，粒子极易陷入某些低谷，无法跳跃局限继续搜索，使得最终获得解非最优。针对粒子群算法的缺陷主要做如下改进。

4.1 个体编码及自适应初始化

在三维路径规划中，起始点到达目标点呈现一条线，需要对障碍点进行避让，线段中的弯道部位存在一个顶点。因此，对粒子引用染色体编码形式，每个粒子由一个数组组成，在数组中包含多个途径点，再和起始点和目标点组成的连线表示规划的路径。途径点的个数过多将会使得计算复杂，过少不利于寻优路径。并且在建造项目中无人机只涉及起始点位置及目标点位置之间的合理规划，在初始粒子时随机产生的粒子次序将决定是否存在反复路径。因此，粒子群的初始位置对算法寻优具有较大影响，考虑障碍区域大小以及起始点和目标点的距离，提出自适应个体初始化机制。主要做法如下：通过计算起始点与目标点的欧式距离：

d=sqrt((g(x)-s(x))2+

(g(y)-s(y))2+(g(z)-s(z))2)

(3)

其中：g(x,y,z)表示目标点位置，s(x,y,z)表示起始点位置。然后结合原有障碍点的个数进行确立粒子组成的位置数组。利用均匀插值方式随机产生随机的n个连续均匀分布的数组，然后对其进行升序排序，产生粒子的初始位置。公式如下：

X(t+1)=sort(unifrnd(start,goal,n))

(4)

4.2 三维路径值的计算方法

在迭代过程中需要衡量前后迭代的粒子质量，才能促使粒子在全区域内不断逼近优秀位置。智能建造中的三维路径规划中主要考虑的指标适应度是最短的路径。为了使得规划后的路径更平滑，无人机飞行转弯时有足够的角度，不至于太急促。为此，采用了三条样条平滑方法进行插值，在对插值进行差分处理，通过计算三次样条得到的散点的路径长度[14]。适应度值公式如下：

fitness=

(5)

其中：K表示插值数量，k表示插值的位次，对所有路径段距离求和得到总的路径长度，该值越小则路径越短。

4.3 线性递减的惯性权重

惯性权重w是粒子记忆前代速度的重要参数，在算法的前期，对于搜索的范围较大，需要较大的惯性权值来提高大范围搜索的性能，而在后期需要进行更精细的搜索。故采用线性递减的方式调节权值，惯性权重将在最大和最小值之间线性变化，以平衡搜索能力[15]。同时，为了均衡惯性权重数值与粒子速度之间的量程差，引进路径量程系数A，防止权重值发生发散，公式如下：

(6)

其中：ωmax为惯性权重最大值，ωmin惯性权重最小值，itermax为最大迭代次数。

4.4 牵引力操作

为解决粒子群算法在寻优过程易陷入局部区域无法跳跃谷底，导致最终获取的非全局最优解的问题。对粒子的速度的更新方式引进加速度思想，使得粒子能够获得足够的动力翻越谷底，增强全局的搜索能力，提高粒子寻优的收敛速度。加速度的公式如下：

(7)

其中：B为加速度量程系数，控制加速度量程。fiti(t+1)为第i粒子在第t次迭代周期的适应度值，best(t)表示为上一代粒子的最优适应度值，ave(t)表示粒子群第t次迭代周期的平均适应度值。分析可知，假如粒子的当前适应度值与前代粒子群的最优适应度值偏差越大，会使得粒子速度增大，搜索的跨度更广，利于摆脱局部困扰。

在迭代中同时监察粒子的适应度在三次迭代中是否具有改善，若无改善则通过一定概率在进行牵引操作增大种群的多样性，若有改善时则缩小进行牵引操作的概率，以此防止不同粒子在迭代中逐渐过于聚集，保持全局搜索。改进后的粒子群算法的粒子速度和位置更新分别为：

X(t+1)=X(t)+v(t+1)

(8)

其中：v表示速度值，t表示第t次迭代数，c1表示个体学习因子，c2表示群体学习因子，pbest表示粒子的个体最优，gbest表示粒子的种群最优，d表示粒子的第d维，rand(0,1)表示0～1之间的随机数。

4.5 改进算法的操作流程

混合引力的粒子群算法(MGPSO,particle swarm optimization algorithm for mixed gravity)流程如图2所示。

图2 改进算法流程图

步骤一：初始化粒子规模，最大迭代数、最大惯性权值和最小惯性权值，自身学习因子，群体学习因子。

步骤二：通过表达式(1)～(2)建立三维场景模型，并返回模型界面的三位数值，作为碰撞检验。

步骤三：通过表达式(3)～(4)进行初始化粒子种群，根据公式(4)计算粒子各自的适应度值并保存个体最优及群体最优；

步骤四：通过公式(5)～(7)进行更新粒子的速度及位置，并作碰撞检验，对碰撞位置进行约束调整。

步骤五：判断是否达到最大迭代次数，是则结束循环输出最优粒子；否则返回进入步骤三。

5 实验仿真及分析

仿真实验在Matlab2016a(Windows10系统，主机IntelCorei5-8300H，CPU@2.20GHz，8GBRAM)进行。对算法的基本设置如下：最大迭代次数iter_max=100，种群个数M=50，个体学习因子c1=2，种群学习因子c2=2；最大惯性权重ωmax=0.9，最小惯性权重ωmin=0.4，路径量程系数A=0.1，加速度量程系数B=0.1初始位置startPos = [1,1,1];目标位置goalPos = [100,100,50]。

5.1 自适应初始化机制的检验

首先，检验所提的自适应初始化机制在改进算法中的有效性，采用随机初始化种群的改进粒子群算法(IGPSO,improved gravitational particle swarm optimization)对照组，其他参数一致相同。图3为算法对照组实验的求解迭代图，由图中可以看出在初始位置。MGPSO算法在迭代前期的寻优值明显好于IGPSO，说明自适应初始化种群更有针对性地帮助算法获取初始点与目标点之间的合理路径。同时观察图4(a)(b)的最终寻优路径，可知在有限区域内，障碍物的数量较多且密集，场景复杂。初始粒子群的优劣直接影响算法在寻优时的有效性，而自适应初始化机制有效地提升初始粒子群的质量。

图3 适应度

图4 路径规划图

同时，对比了IGPSO，MGPSO算法收敛的稳定性。障碍物的数量不变情况下，随机产生障碍物，分别运行了10次并统计了10次寻优的平均最优值及其方差，结果如表1所示。可知，MGPSO算法求解稳定性更高。

表1 自适应初始化机制的有效性结果

5.2 与其他算法的比较

同时验证改进算法相对于其他一些策略的算法优先性，采用了IPSO算法[8]、IHPSO算法[11]与MGPSO算法进行对比。

3种算法均采用了自适应初始化机制产生初始种群，实验收敛迭代过程如图5所示。从图中可以看出3种算法都具有较好的收敛效果，同时对比图3可知，采用自适应初始化机制在IPSO算法体现效果最明显。IPSO算法前期的粒子群质量最优，但在迭代10次之后的搜索能力明显减弱，即使后期仍有寻优余热，效果却不明显。本文所提的优化算法虽然在前期粒子群质量均差于其余两种算法，但是在保持寻优的耐力上更具有优势，在20次迭代时基本完成全局最优解区域的定位，后续仍有寻优余热。

图5 3种算法的收敛迭代图

分析可知主要是因为采用了线性递减的惯性权重，使得初期粒子的速度较大，加强在全局区域的最优值搜索，随着迭代周期的增加，权值逐渐减小，减弱大范围的跳跃，同时加强局部的搜索，使得路径值不断逼近最优。同时采用牵引力操作添加了加速度作用，在搜索过程中扩大了步伐，使得粒子更易跳出局部最优。在图6为3种算法的最终寻优路径结果，可以较为直观地看出，MGPSO算法明显相对于其他两个算法，在复杂的场景中跳出了局部最优并获得更优路径。

图6 3种算法规划路径的结果图

障碍物的数量不变情况下，随机产生障碍物，通过三十次的重复实验，统计3种算法求解的平均最优路径值及其

方差，结果如表2所示，MGPSO算法的求解质量最优且稳定性能上也表现不错。稳定性相对于IPSO较好69.75%，相对于IHPSO较好17.41%。

表2 3种算法的性能比较

6 结束语

为了使得无人机在执行任务中具有更完善的路径规划的知识模型，分析了无人机的系统结构及四旋翼无人的飞行原理。指出粒子群算法进行无人机的三维路径规划极易陷入局部最优，提出了一种混合惯性牵引力的三维路径规划方法。实验仿真结果中表明，自适应初始机制对于帮助算法对于最优路径的求解性能，在特定的算法搜索能力下，极大影响求解质量。通过引入线性惯性权重并且结合路径的量程大小，动态地调节算法在全局搜索能力与局部搜索能力之间的平衡。增加了加速度方式，可帮助粒子群增大摆脱陷入局部最优的概率。改进的算法具有较好的搜索能力，也能保持较好的求解稳定性。

由于路径的平滑度对寻优的质量具有较大的影响，因此，后续将结合算法与高效的路径平滑度对智能建造中无人机路径展开研究。