基于改进粒子群算法的水面无人艇全局路径规划*

林法君 李 焰

（1.海参军事训练中心 北京 100036）（2.海军工程大学兵器工程学院 武汉 430000）

1 引言

目前，随着水面无人艇（USV）使用的日益增多，许多有前景的技术研究已经扩散开来［1～2］。USV具有协同合作，智能自主等特点，在编队控制和智能避碰等领域已经有了广泛的研究［3～4］。USV可以执行危险任务，具有一定的自主能力，减少了对人员配备的需求。为了有效地完成实际任务，路径规划在USV的实际应用和理论探索中都起到了良好的作用。

因此，一种安全高效的航路规划技术对于USV在实际海洋环境中的应用具有至关重要的作用。具体来说，USV需要路径规划方法来自主执行海岸巡逻、救援等各项任务［5］。近年来在全局路径规划的研究上，启发式算法被广泛使用，启发式算法在精确算法失效的情况下可以得到最优解［6］。粒子群算法（PSO），蚁群算法（AC）和人工蜂群算法（ABC）等均属于生物启发式算法。粒子群算法具有操作简单，设置参数少等易于实现的特点［7～8］。J.Kennedy，R.Eberhart建立了一个包含粒子群算法的框架来最小化路径长度，但是在障碍复杂的环境下，传统的粒子群算法在全局路径规划中还存在一些不足［9］，比如算法前期收敛慢、迭代后期个体最优解易波动等问题，很大程度上影响了全局路径规划的效率和可靠性［10］。为了解决前期收敛速度慢的问题，张晓莉等［11］提出了动态速度权重法，较好地解决了前期收敛速度慢的问题。但是本文在研究验证过程中，发现动态速度权重法相对于原始的粒子群算法，更容易陷入局部最优。为了解决上述存在的前期收敛速度慢，以及容易陷入局部最优的问题，本文提出了一种改进的速度权重法，在路径寻优过程中，同时运行原始粒子群算法和改进的速度权重法，然后根据权重对每个粒子以轮盘赌的方式决定粒子的迭代公式，最后得到全局路径规划的最优路径。

2 改进粒子群算法

2.1 初始粒子群算法（PSO）

粒子群算法（Particle Swarm Optimization，PSO），也称为鸟群觅食算法，是模仿鸟群觅食行为设计的一种寻优算法，属于进化算法的一种，从一组随机解出发，通过不断地迭代寻找最优解。在迭代的过程中，以某一适应函数对粒子的位置进行评价，每个粒子通过对比全局部最优和个体最优来更新自己的位置和速度的状态。整个粒子群在迭代的过程中不断向目的地靠拢，最后获得最优解［12］。

经典的粒子群算法的数学模型为位置和速度更新模型，表达式为

对上式中的组成部分说明如下：vk(i，d)表示粒子群算法第k次迭代时，粒子i在维度d上的速度分量；qk(i，d)表示为粒子i在k次迭代中历史最优位置在维度d上的分量，即个体极值；xk(i，d)表示当前粒子i在维度d上的位置值；gk(i，d)表示全部粒子在k次迭代中历史最优位置在维度d上的分量，即全局极值。式（1）描述的是粒子的速度更新方式，其中ω为惯性因子，反映了粒子沿原运动方向的运动趋势；c1和c2是学习因子，c1反映的是当前粒子向个体极值加速的趋势，c2反映的是当前粒子向全局极值加速的趋势，r1和r2为［0，1］范围内的随机数。式（2）描述的是粒子的位置更新方式，即下一时刻位置等于当前时刻位置加上当前时刻速度［13］。

2.2 速度权重法（PPSO）

封硕等［14］指出标准的粒子群算法中的粒子位置，可能会因为后期迭代过程中速度过大，个体极值偏离全局极值，于是提出了对速度进行加权处理的方法，并给出了新的位置更新公式：

其中p为速度权重，权重p的确定如下式：

(xp，yp，…)表示在k次迭代中，当前粒子的个体极值，(xg，yg，…)表示为当前时刻粒子群的全局极值坐标点，表示前一时刻粒子群的全局极值坐标点。

本研究在验证的过程中对上述式子中未能给出的隐含信息进行了补充。比如上述式子在第一次迭代过程中缺少前一时刻的粒子状态信息，以及前一时刻与当前时刻全局极值坐标点相同的情形如何处理，从而导致仿真实验中出现“变量未定义”、“出现分母为零”之类的报错。因此，本文对上述速度权重法做了一定的补充：

2.3 动态速度权重法（DPSO）

在本研究仿真实验的过程中，发现速度权重法的确大大加快了前期的收敛速度，但是在多次的仿真实验中，该方法更容易陷入局部最优，使得找到最优路径的不确定性大了很多。

为了既加快算法的收敛速度，又提高寻找最优解的可靠性，本文采用轮盘赌的方法结合了速度权重的粒子群算法和初始的粒子群算法。在算法的设计中加入了两轮嵌套判断语句对位置的更新公式做出了引导。

在每次计算p值得时候，引入一个0～1之间的随机数o，令

pi根据当前粒子i的状态计算得到，li表示为归一化后的轮盘指针，由轮盘指针的落点决定位置的迭代公式。当li＜o时，执行原始粒子群算法的位置迭代公式，li≥o时，执行速度加权粒子群算法的位置迭代公式。

2.4 算法测试

为了比较原始粒子群算法（PSO）、速度加权粒子群算法（PPSO）和两者结合的动态粒子群算法（DPSO）的表现，本文选用了Sphere函数作为适应度函数做仿真实验。实验首先用三个粒子群算法求解适度函数的最小值，然后对三组实验做直观上的比较和求解速度以及精度上的比较来评比其表现。

适度函数的计算如下：

(x1，x2，…，xn)为粒子的点坐标，Sphere函数是一个单峰函数，在坐标（0，0，…，0）处取得最小值0。算法的粒子数量定为50个，迭代次数为50次。用三种方法对其进行实验，仿真绘图结果如图1所示：

图1 Sphere函数寻优实验

提取图中三种算法初次进入收敛阶段的迭代代数k和最优值q记于表1中。

表1 收敛阶段的测试数据

对以上图形的直观对比和表格数据的具体对比作出分析如下。

从收敛速度上来看PSO算法在经过13次迭代进入收敛阶段，PPSO算法在经过7次迭代后进入收敛阶段，DPSO算法在经过8次迭代后进入收敛阶段，就收敛速度而言，PPSO≻DPSO≻PSO。

从逼近最小值的情况来看，PSO基本完成了最小值的寻找，DPSO以更高的精度完成了最小值的寻找，PPSO陷入了局部最优，没有找到最小值，就寻求最小值而言DSO≻PSO≻PPSO。

具体原因分析如下所示。

PPSO中，在经过几轮迭代后，当前时刻的全局极值与前一时刻的全局极值差距会缩小，pi值越来越小，使得迭代的步进值减小，因此当粒子陷入局部最优时，也很难再“跑”出来。DPSO中，前面的几轮迭代中，pi比较大，轮盘赌法更大可能地选中了PPSO的位置更新公式，因此也吸收了PPSO前期收敛速度快的特点，随之pi值减小，轮盘赌法更倾向于PSO的位置更新公式，因此后期的收敛逼近效果要优于PPSO，同时，在大量粒子的更新中，后期仍有一部分粒子使用了PPSO的位置更新公式，向外探索新的位置导致可能碰到更优解，因此DPSO的寻优效果反而比PSO更高。

在多次的重复仿真试验后，以上分析的总体趋势并没有偏差，表现出了较强的一致性。因此在适度函数为Sphere的情况下，DPSO算法的总体表现呈现了较强的有效性。

3 障碍水域环境模型建立及路径规划

3.1 水域环境模型的建立

为了验证水面无人艇在复杂环境水域的航行路径规划，本文设计了20×20的模拟海域，并在海域中投放了11个大小各异的障碍物，为了保证船体的航行安全，本研究对障碍物进行了边缘膨胀的处理，将每个障碍物膨胀为矩形，并保证对应矩形的每条边都与障碍物保持了一定的安全距离。同时，为了限制无人艇驶出模拟海域，本模型在限制海域的周边补上了障碍围栏。具体的环境水域模型如图2所示。

图2 环境水域模型

3.2 路径规划

本文在该环境模型的基础上进行仿真实验的过程中，发现在不开阔区域，尤其是被障碍物夹杂的区域，步长过长容易导致“粒子”突然陷入障碍物中，这既不符合船体运动的实际情况，也违背了避障的初衷。考虑到船体在障碍物群中行驶缓慢的特点以及避免船体碰撞障碍物的情形，本研究对粒子群算法做出了两部分的调整。

1）减小了粒子的运动步长；

2）对运动到障碍矩形框内的粒子进行了姿态矫正处理。

调整操作如下：

由于障碍物边界的宽度设定为0.5，为了避免粒子跳入障碍物中心，本文设定粒子的运动步长固定为0.5。

本文研究是基于两维度上的USV运动，因此设置了两个方向上的运动速度，vx和vy分别表示环境模型中横轴方向和纵轴方向的运动速度，vT表示两者的合速度，vx'和vy'表示调整后的速度大小，调整后的速度与原迭代得到的速度方向一致，但调整后的合速度大小固定为0.5。

如图3所示，粒子的原位置为A，经过下一轮迭代计算得到新的粒子位置为B，通过判断粒子落在了图1中灰色的障碍区域，则对粒子进行位置矫正处理，将位置修正到障碍区域的边缘C处，则粒子的移动由A→B矫正为A→C。

图3 粒子的姿态矫正

算法的适应度函数设为f：

l为粒子走过的路径长度：

M用来表示粒子目前点位与目的地的距离估计，M的计算如下：

xi和yi表示粒子i的当前位置坐标，分别取目标点横纵轴坐标与粒子横纵轴坐标的距离，用距离的和表示实际距离估计值，取权重为2，是为了加快粒子向目标点靠拢。

4 仿真结果与分析

在建立了图2的环境水域模型后，分别对初始粒子群算法（PSO）、速度权重粒子群算法（PPSO）和动态权重粒子群算法（DPSO）三种算法进行仿真实验。本研究使用Python语言编辑脚本进行仿真实验，算法中的具体参数设置如下：

1）设定USV的起点为（1，1），目的地为（20，20）；

2）粒子的规模为10，迭代次数设为100。粒子的学习因子c1=0.3，c2=0.7，惯性因子ω=0.5。

3）粒子群算法的适度函数为f。

三种算法运算后得出的水面无人艇路径规划仿真如图4～6所示。

图4 PSO算法路径规划仿真结果

图5 PPSO算法路径规划仿真结果

图6 DPSO算法路径规划仿真结果

三种算法均有效地寻得了可行路径，方向和路径大体一致，但在细节处的表现略有偏差。在中间的开阔无障碍地带，三种算法表现基本一致，但是在障碍物夹缝处，三者的表现出现了明显的差异。在障碍物夹缝处，PSO算法和PPSO算法出现了明显的直角转弯，这是由于大量的粒子跳入障碍区之后被矫正所导致，水面船体的机动制动能力不如陆地载具，直角转弯并不适合水面无人艇的航行，而DPSO算法得到的路径在夹缝处的航行转角均为钝角，有利于USV航行的实现。

接下来分别采用PSO、PPSO、DPSO算法仿真50次。为了比较三种算法的性能，从寻找路径的成功次数、进入收敛阶段的平均迭代代数以及平均路径长度（仅计算寻找路径成功的迭代次数和路径长度）三个方面进行对比。测试的数据如表2。

表2 三种算法的测试数据

由表2可知，DPSO无论在寻找路径的成功率上表现出较强的优势，从平均路径长度来看，较其他两种算法略微有所缩短，而收敛速度略慢于PPSO，但是差距不大，与前面的理论分析结果一致。

实验数据结果表明，动态权重的粒子群算法在路径规划模型中的应用，有效地提高了寻找路径的成功率，能在一定程度上缩短路径长度。

5 结语

本文在建立了栅格化的障碍区海域模型的基础上，针对路径规划的问题，提出了动态权重的粒子群算法DPSO。结合水面无人艇实际的运动情况，为了避免碰撞到障碍物，减小了粒子的运动步长，并对跳入障碍区的粒子进行了姿态矫正处理。

在构建的环境海域模型中对初始粒子群算法PSO、速度权重粒子群算法PPSO和动态粒子群算法DPSO进行仿真实验，结果表明，DPSO算法吸收了收敛速度快的优点，而且提高了寻优成功率，缩短了行驶路径长度，减小了USV的转弯幅度，突出了DPSO算法在水面无人艇路径寻优上的有效性。