粒子群算法优化机器人路径规划的研究

巫光福，万路萍

（江西理工大学信息工程学院，江西赣州 341000）

在移动机器人领域中，导航技术是非常关键的一个研究点，而路径规划是导航中一个很重要的任务。所谓的路径规划[1-2]就是在一个包含障碍物的环境中，从起点到终点寻找到一条最优或者近似最优的路径，并满足一定的约束条件，如最短路径、耗时最少和安全性最高等等。随着国内外学者对机器人路径规划领域越来越深入的研究，提出的解决路径规划的方法也越来越多。由于A*算法、人工势场法等传统算法[3-5]在解决路径规划问题存在很大不足，如在复杂环境中，计算量大，占用内存多，寻找符合标准的最优路径的效率比较差等。由于仿生智能算法的出现，越来越多的学者研究智能算法及其改进方法，并将其应用在解决机器人路径规划问题上。实验证明，灰狼算法[6]、蚁群算法[7-8]、遗传算法[9-11]、粒子群算法[12-13]等智能算法在优化移动机器人路径问题上很有效。

粒子群优化算法（Particle swarm optimization algorithm，PSO）最早于1995年11月由Kennedy 和Eberhart 提出[14]，它是基于自然界中鸟类、鱼类等群体的思想的进化计算方法。与遗传算法等群智能优化算法相比，粒子群优化算法具有相对简单性、可调参数少和收敛速度较快的特点，这也使得其成为解决移动机器人路径规划问题的一个应用比较广泛的算法。但是粒子群优化算法在搜索过程中也容易出现早熟现象[15]，使得路径规划陷入局部最优，因而也出现了很多改进算法。文献[16]将蚁群算法与粒子群优化算法融合，通过蚁群算法得到全局最优路径，改进的粒子群算法在全局范围求最优解，并将其作为蚂蚁的初始信息素，从而蚁群算法能进一步寻优，两者优势互补，提高了算法寻找最优解的能力。文献[17]设计了在全局最优位置引入扰动全局更新机制，避免了算法的停滞。文献[18]利用机器人到障碍物和目标点的距离，构造了一个新的目标函数，机器人能够成功地避开障碍物并朝着目标点运动。尽管这些改进算法都取得了不错的效果，但是粒子群优化算法在移动机器人路径规划的仍然存在收敛速度慢和易陷入局部最优等问题。

针对目前粒子群优化算法的不足，对算法进行了一些改进。首先对全局最佳粒子的速度进行了一个轻微的扰动，以免其陷入局部最小；然后使用了非线性惯性权重来平衡算法全局搜索和局部搜索能力；最后定义了考虑路径长度和平滑的适应度函数。实验证明，改进的粒子群优化算法能较快的寻找到符合要求的路径。

1 环境模型

路径规划是移动机器人在复杂环境下完成自主移动导航的重要必备条件之一，在环境已经确定的情况下，按照一定的标准找到一条从起点到目标点的最优路径，在这个过程中，要能够避开与障碍物的碰撞。对机器人在二维工作环境进行了简单的建模，整个环境在一个二维平面坐标中，考虑这是个没有自身尺寸的点机器人，在搜索区域内，随机放置一些圆形障碍物，圆形障碍物的数量和半径可根据实际设置。如图1所示，粒子群算法初始化路径采用随机策略，粒子随机分散在搜索区域，而随机生成的路径节点可表示为 p=（x,y），假设粒子经过了m个节点，生成的路径则表示为 P=（p1,p2,···,pm）。

图1 环境模型

路径在随机初始化的时候，可能生成的路径是经过障碍物的。在避障方面，用重定位法代替罚分法进行避障。在罚分法中，我们需要把掉在障碍物内的粒子或者路径从障碍物内通过的粒子去除。然而，在重定位方法中，粒子的位置会被重新定位，因此不会丢失粒子。当障碍物与连接粒子的新旧位置的直线有交点时，旧位置的粒子就需要重新寻找位置，反之，没有交点时，那么该位置就是粒子的新位置。这样的判定方法就可以在避免障碍物的同时还能不损失粒子。

2 粒子群优化算法的移动机器人路径规划

2.1 标准粒子群优化算法

在粒子群优化算法中，每个粒子会根据自己和同伴的飞行经验来调整自己的行进路径。每个粒子经过了的最佳位置就是个体最优值，而整个种群所经过的最佳位置叫全局最优值。每个粒子通过这两个最优值来不停地更新自己的位置。根据所需要优化的问题，提出合适的适应度函数，得到的适应度值则用来评价粒子的优劣。粒子群算法采用的是速度-位置的搜索模型，每个粒子都有一个速度来决定运动的方向和大小的更新。粒子寻优过程就是根据自我经验学习和社会经验学习来更新自己的位置和速度。若该粒子群有N 个粒子且每个粒子有n维，则它的速度和位置更新式分别为：

标准粒子群算法如图2所示。

图2 标准粒子群算法

标准粒子群算法具体的步骤如下：

步骤1在优化问题的d 维环境下随机初始化粒子的位置和速度。

步骤2根据适应度函数计算粒子的适应度值。

步骤3对于每次迭代，将每个粒子的适应度与其先前获得的最佳适应度（pbest）进行比较。如果当前值优于pbest，则将pbest设置为当前值。

步骤4比较每个粒子的pbest值，最佳值则更新为全局最优值gbest。

步骤5 根据式（1）和式（2）更新粒子速度和位置。

步骤6更新迭代次数，直到达到设置的最大迭代次数，则停止搜索，输出全局最优值的位置，否则，转至步骤2，继续执行。

2.2 改进粒子群算法

2.2.1 速度更新

粒子群算法中最基本的概念是每个粒子在每一次迭代过程中，以一定的权重的速度朝着局部和全局最佳位置移动，如图3所示，群中一个粒子以一定的速度移动的示意图。

图3 一个粒子位置移动概念

粒子的速度决定粒子的运动的方向和距离，而速度也会根据自身以及其他粒子的经验来调整，并实现个体在可行解空间搜索到最优解。但是随着迭代次数的增加，粒子可能陷入局部最优值。为了保证算法收敛速度更快和尽可能避免算法陷入局部最小值，提出了速度更新方式，即当粒子陷入局部值时，对全局最优粒子的速度进行一个轻微的扰动，速度更新方式表达式为

式中： r3是单位随机值；α =0.2，β =0.3。

2.2.2 惯性权重

在标准粒子群算法中，惯性权重能够调节粒子的速度，影响着粒子的全局搜索能力和局部搜索能力，为了防止粒子发生早熟现象，保证能够找到最优解，那么粒子的搜索能力都应该随着进化过程改变。Shi第一次提出粒子群优化算法中惯性权重的概念，并且分析了惯性权重越大越有利于全局搜索，越小则越有利于局部搜索[19]。为了更好的平衡局部和全局的搜索能力，提出了线性递减权重（Linear decreasing inertia weight,LDIW），表达式为

式中：惯性权重 ω的值是从 ωmax递 减到 ωmin； Tmax为最大迭代次数。

随着迭代次数的增加，惯性权重线性递减。在最开始的迭代过程中，惯性权重值较大，保证了算法比较强的全局搜索能力，在后期的迭代过程，惯性权重值较小，提高算法在局部的搜索精度。为了使粒子群优化算法在避免陷入局部最优和获得更好的搜索能力，本文提出了非线性递减惯性权重（Nonlinear decreasing inertial weight，NDIW），表达式为

式中c =2.3。

两种惯性权重的变化曲线如图4所示。

图4 两种惯性权重的变化曲线

2.3 适应度函数

要规划好机器人的路径，一个合适的适应度函数是非常重要的。根据移动机器人在实际环境中的要求，过多的转弯会使得机器人能耗增大，移动时间也会增加，因此最优路径应该要满足最短性和平滑性这样的约束条件。本文提出了一个新的适应度函数，路径的距离计算方式为每次迭代过程中每个粒子与目标点之间的欧几里得距离，路径的平滑度判定则是迭代过程中目标点与机器人两个连续的位置连接而成的两条假想直线的夹角，目标点与两个连续位置的夹角如图5所示。

图5 目标点与两个连续位置的夹角

这两个连续位置即为gkbest和gk-1best。最短距离表达式为

式中k 为迭代次数。

平滑路径的数学表达式为

这里平滑度的表示为在两次连续迭代过程中机器人最小移动角度，而不是一条样条曲线。最后的适应度函数公式为

3 实验仿真分析

为了验证算法的正确性和有效性，设置了一个3000×3000单位边界的二维正方形搜索空间。通过反复的实验和大量参数的使用，确定了在更快的收敛速度、有效避障、生成可行性路径和避免局部最优等条件下，所使用的最终参数。

具体设置如下：粒子群规模N=100，最大迭代次数Tmax=50，学习因子 c1=c2=2，惯性权重ωmax=0.9，ωmin=0.4，常数 c =2.3， 适应度函数中λ1=1，λ2=0.25。障碍物个数为5和8的路径寻优结果分别如图6和图7所示，起点位置是（1000,1000），终点位置是（-500,-1000），图中圆形是障碍物，粒子的颜色基于迭代。每次迭代时，粒子的颜色从蓝色（初始粒子分布）更改为红色（最终粒子分布）。实验中，环境中的障碍物的位置是动态变化的，个数自行设置，不同的障碍物个数，该算法都能找到符合要求的路径，拐弯的次数不多，粒子都够避开障碍物，收敛速度很快。初始粒子都比较分散，可以看到，粒子（红色）是最终都在目标点附近聚集。

图6 障碍物个数为5的路径寻优结果

图7 障碍物个数为8的路径寻优结果

为了对比改进粒子群优化算法与标准优化算法，在障碍物个数为10的相同的环境下实验，寻优结果如图8和图9所示，在经过50次迭代过程后，两个算法都找到了路径，但是，改进粒子群优化算法搜到的路径相对来说更平滑，粒子最终分布都比较集中。而在标准粒子群算法中，粒子的最终位置分布相对较散。

图8 改进PSO 算法路径寻优结果

图9 基本PSO的路径寻优结果

粒子在寻找路径的时候，有的时候还是会无法找到路径，或者找到的路径是经过了障碍物的，如表1是在50次迭代的实验结果，可知，改进的算法在这样已知搜索空间内，对路径的搜索的成功率比标准算法要高一些。由于算法收敛速度很快，实验时在控制台观察看到的是比较少的迭代结果。

表1 路径规划的实验结果比较

4 结论

仿真结果表明，所实现的方法能够满足路径规划的要求，寻找到合适的最优路径。当粒子陷入局部最优的时候，给了全局最优速度一个轻微的扰动，并使用了非线性递减惯性权重，使得粒子的收敛速度加快，也平衡了全局和局部的搜索能力，从而粒子能搜索到最优路径。在环境中随机放置障碍物，生成的路径能够避免所有障碍物，最终粒子的位置也不在障碍物边界内。该方法易于实现，算法速度快，适用于复杂多变的移动障碍物环境。正如本文所展示的，该方法可以在所有类型的环境中执行。这种方法是灵活的，这样我们可以改变任何参数，来实现一定需求的路径规划。