改进粒子群算法的移动机器人平滑路径规划

1 前 言

路径规划是移动机器人应用在生产实践中所遇到的一个关键任务，其目的是从起点到目标点寻找可行且最优的路径[1].它可以被看作是某些指标(例如，最短距离和最小能量)与某些约束(如无碰撞路线)的优化问题[2].目前，研究人员已经开发出了许多启发式算法来解决这个问题[3]，其中粒子群优化算法[4]是解决该问题最常用的方法之一[5].

在实际应用中，普通路径规划算法主要涉及到某些简单的最优准则(如路径的最小长度)[6]，很少考虑路径的其他重要性能(如路径的平滑性)[7].传统的路径规划算法可以生成一条由多条折线连接而成的路径，由此会造成不可避免的急转弯.沿着这样的路径移动会导致移动机器人在不同模式之间频繁切换(例如，停止、旋转和重启)，从而导致不必要的时间浪费[8].此外，在某些情境下，为了保证机器人服务质量，不允许进行状态切换，从而必须提高其运动平滑性[9].由以上情形可看出，除去考虑路径长度之外，路径平滑度也是在机器人路径规划问题中的需要寻优的另外一个重要指标[10].

本文针对移动机器人路径的全局平滑规划[11]问题，提出了一种新的基于种群进化状态的自适应粒子群算法，并结合Bezier曲线来寻找机器人最优平滑路径[12].在ES-PSO中采用自适应控制算法参数[13]，从而加强种群的勘测能力并结合种群群体跳出操作，从而有效避免算法早熟，提高算法对解空间的寻优能力.在算法初始阶段，采用初始化策略，加快算法的寻优速度，提升算法的寻优精度.并结合带有罚函数的适应值函数优化策略，提高平滑路径规划的成功率.仿真结果表明针对机器人寻找平滑路径问题的ES-PSO算法明显优于传统的PSO算法.

2 ES-PSO算法

2.1 PSO算法基本原理

PSO主要通过随机定位对在N维搜索空间中的粒子群进行初始化.群体中的每个粒子都具有两个向量，即速度向量和位置向量.在每次迭代过程中，每个粒子通过学习粒子自己的历史最佳位置和整个群体发现的最佳位置来更新其速度和位置[14].令和分别为第i个粒子的速度矢量和位置矢量，种群中粒子数量为M.原始PSO算法中的更新规则由以下给出：

速度信息更新迭代公式：

Vi(k+1)=ωVi(k)+c1r1(pBesti(k)-Xi(k))+

c2r2(gBest(k)-Xi(k))

(1)

位置信息更新迭代公式：

Xi(k+1)=Xi(k)+Vi(k+1)

(2)

其中pBesti是粒子i的历史最佳位置，gBest是整个群体历史上的最佳位置，ω为惯性权重，k为当前迭代次数，c1和c2为学习因子，分别是衡量pBesti和gBest相对重要性的两个参数，r1和r2为均匀分布在[0,1]上的随机数.

2.2 改进粒子群算法

针对粒子群算法容易陷入局部最优和收敛速度慢的缺陷，本文提出了一种ES-PSO算法.该算法改进主要体现在四个方面，分别为种群进化状态策略、自适应惯性权重、自适应学习因子策略和群体跳出策略.

1)种群进化状态

种群进化状态的分析是PSO算法的关键内容.且绝大多数改进算法用种群中粒子相互间的平均距离和粒子适应度之间的差异定义种群多样性[15].

若仅考虑粒子当前位置的状态，将会忽略整个种群在每一代进化过程中的进化状态与种群在之后迭代过程中的进化潜力.例如，如果两个粒子本次迭代中位置较近，而粒子速度矢量差异较大，则在下次迭代后，粒子的位置可能相距较远.因此，根据种群粒子的当前位置来判断种群的进化状态并不合理.

在此，本文提出了一种新的种群进化状态的测量方法.设f(gBest(k))表示第k次迭代中全局最佳粒子的适应度.对于种群的整个迭代过程， {f(gBest(0)),f(gBest(1)),…,f(gBest(k))}是一个非上升序列，即：

δgBest(k)=f(gBest(k))-f(gBest(k-1))≤0

(3)

其次，设f(pBesti(k))表示第k次迭代中粒子的历史最佳位置的适应度.对于种群中每个粒子，{f(pBesti(0)),f(pBesti(1)),…,f(pBesti(k))}也是一个非上升序列，即：

δpBesti(k)=f(pBesti(k))-f(pBesti(k-1))≤0

(4)

因此，所有pBesti (i=1,2,…,M)的适应度之和满足：

(5)

在这三个非上升序列中，δgBest(k)，δpBesti(k)和分别表示全局最佳粒子改进，单个粒子历史最佳位置改进和集体历史最佳位置改进.根据这些改进来评估种群进化状态.进化状态可分为以下三种情况：

a)种群勘探状态(δgBest(k)<0)：在此种情况下，全局最佳粒子位置得到了改进，即整个种群向着更优的方向进化.根据gBest和pBesti的定义，可以推出，至少存在一个粒子使得δpBesti(k)<0，所以也成立.整个种群此时积极在搜索空间中寻找最优值，具有很强的勘探能力.

b)种群收敛状态这种情况下，全局最佳粒子的位置没有得到改善，整个种群的进化停滞不前.但在整个种群中至少有一个粒子能够改善其最佳位置.这意味着种群有可能在接下来的迭代中得到改进.

c)种群困死状态这种情况下，整个种群的进化停止，种群中的粒子也没有了进化的潜力.此时种群可能陷入局部极值点.为了使最优粒子摆脱局部极值点，算法将执行群体跳出策略.

2)自适应惯性权重

由文献[16]可知，较大的惯性权重有利于全局搜索，而较小的惯性权重有利于算法的局部开发，加速算法的收敛.在进化早期，算法应具有较大的惯性权重，当进化代数的增加时，惯性权重应迅速下降.在进化过程中，惯性权重应随着进化状态自适应改变.设ρgbest(k)为种群进化率：

ρgbest(k)=f(gBest(k))/f(gBest(k-1))

(6)

由式(6)可知，在种群勘测状态下，0<ρgbest(k)<1；在种群收敛状态和种群困死状态下，ρgbest(k)=1.在此，ES-PSO引入自适应惯性权重控制器，即考虑种群进化率对惯性权重的影响，采用指数下降形式的惯性权重.自适应惯性权重控制器设计如下：

(7)

3)自适应学习因子

学习因子c1主要是将粒子吸引到自身的历史最佳位置，帮助探索局部最优值并保持种群多样性.学习因子c2有助于种群快速收敛[17].此两种不同的学习机制应在不同的进化状态下给予不同的处理.在ES-PSO中，设学习因子c1和c2初始值为2.0并进行自适应控制，策略如下：

种群勘测状态：在勘探状态下探索需尽可能多的最优值.因此，增加c1和减少c2可帮助粒子单独探索并优化自身的最佳历史位置.

种群收敛状态：当种群处于收敛状态时，应增加c2的值来引导其他粒子尽快到达全局最优区域.此外，应增加c1的值以防止群体将被当前最佳区域吸引，导致过早收敛.

种群困死状态：在此状态下，应增大c2并减小c1值，将有助于算法执行群体跳出策略，即粒子远离局部极值点.

将c1与c2范围限制在[1.5，2.5].此外，当c1和c2总和大于初始值之和4.0时.则将c1和c2归一化为：

(8)

由于上述学习因子的调整需缓慢进行，故两次迭代之间的最大增量或减量应受到如下限制：

|ci(g+1)-ci(g)|≤ξ，i=1，2

(9)

ξ为“加速率”.相关实验表明[6]，在区间[0.005，0.01]中均匀生成的ξ随机值在大多数测试函数中表现最佳.

4)群体跳出策略

当种群处于困死状态时，即所有粒子围绕在当前局部最优区域，此时算法需要新的动力来摆脱局部最优区域.在此本文设计最优值区域扰动策略以帮助粒子将自身推向可能更好的区域.由于局部最优值可能具有一些全局最优值的特性.因此，为了保护这些良好的特性，传统算法只更新局部最优值的一个维度.然而这种方法并不能保证每次跳出操作能够使粒子成功找到一个新的更优点，且效率较低.因此，本文设计了一种群体跳出策略，即：

(10)

Xmax和Xmin分别为搜索区域的上限和下限.j表示粒子的维度.因为粒子都收敛于当前局部最优区域内，该策略对整个群体中的粒子都执行一次跳出操作，由于本策略只随机改变一个维度的值，因此每个更新后的粒子仍然保有良好的最优值特性.接着，本算法重新评价了粒子更新后的种群进化状态，当种群进化状态转变为勘探状态时，种群将向着新的最优区域方向进化.若粒子更新后种群进化状态没有得到改善，将更新后的粒子重新回溯到之前的收敛状态，再次采用群体跳出策略对粒子位置进行更新.随后的实验结果证明，群体跳出策略相对于传统的单个粒子跳出策略，能够更有效地跳出局部极值点，寻找到更加精确的最优值.

3 Bezier曲线

Bezier曲线是一种在计算机图形学等实践中得到广泛应用的参数曲线[18].Bezier曲线由许多控制点组成，除了起始点和终点，控制点不在曲线上.设一组控制点向量为P0，P1，…，Pn，则Bezier曲线可表示为：

(11)

其中t是归一化的时间变量，Pi=(xi，yi)T代表第i个控制点的坐标向量，xi和yi分别为X和Y坐标分量;为Bernstein多项式：

(12)

由于Bezier曲线的光滑性与路径的曲率函数关系密切.则在二维平面上，Bezier曲线的曲率可以表示为：

(13)

其中R(t)为曲率半径.和为Bezier曲线P(t)对x和y的一阶，二阶导数，即表示为如下方程：

(14)

(15)

本文将Bezier曲线结合到路径规划过程中，将ES-PSO算法求解得到的路径节点作为控制点，则可生成平滑路径.

4 路径规划问题描述与建模

在机器人路径规划问题中，当用PSO算法求解该问题时，每个粒子代表一条合理的路径[19].设有M条可行路径，即粒子表示为p[p1(x1，y1)，p2(x2，y2)，…，pN(xN，yN)]，N代表路径节点的个数，p1为起始点，pN为终点.每个路径节点代表了路径的一次转向，大量实验表明，在复杂的环境中，机器人只需通过3～5次转向就可避开障碍物寻找到一条最优路径.因此粒子维度不会随着环境的复杂度而大幅增加.

4.1 路径初始化策略

PSO算法的初始化策略通常为随机策略，然而随机选择路径节点很可能导致生成的初始路径穿过障碍物，或使路径变得较为曲折，增加算法的求解时间.本文设计了一种新的初始化策略，可以在路径初始化时，得到接近最优解的初始路径.具体方法如下：

连接起始点和终点，记为线段L.在线段L上均匀取粒子维度N-2个点(去除起始点和终点)，那么每个点的坐标都是确定的，记为：

(16)

在每一点处，作关于线段L的垂线.记为l1，l2，…，lN-2.在每条垂线上随机取一个点，分别记为p2，p3，…，pN-1.最后从起始点将各个点依次连接.

由以上方法可得到尽可能符合机器人运动趋势的路径，但路径仍然有可能穿越障碍物，成为非法路径.如果初始得到的路径是非法路径，则将该路径删除，重新生成初始路径.非法路径存在两种情况：

1)路径节点位于障碍物内部：

本文默认将障碍物区域合理填充或分割为圆形区域.因此只需判定路径节点到圆心的距离dp是否小于圆的半径r即可.

2)路径线路穿越障碍物：

即使路径节点没有在障碍物内部，两路径节点连线生成的路径线路也有可能穿越障碍物，从而导致路径非法.在此情况下，本文介绍一种判断方法来判断路径线路是否穿越障碍物，详细如下.

图1 不同初始化策略下的初始路径
Fig.1 Initial path under different initialization strategies

首先计算障碍物圆心到路径线路之间的垂直距离dl.如果dl>r，则表示路径线路没有穿越障碍物.如果dl<r，则继续讨论该路径线路的两个路径节点与障碍物圆心r(xr，yr)的相对位置.两路径节点分别为P1(x1，y1)和P2(x2，y2).如果两路径节点都位于障碍物圆心同一侧，即(x1-xr)(x2-xr)<0或者(y1-yr)(y2-yr)<0，可以判定路径线路没有穿过障碍物.否则认为该路径线路穿越障碍物.由以上策略规划出的初始路径如图1所示.

由图1示例可知，传统的随机初始化方法得到的路径具有不确定性，生成的路径曲折并有可能穿越障碍物，不利于之后算法迭代寻优.本文算法提出的初始化方法，能够使算法得到较好的初始化，将会减少算法的求解时间，并提升寻优精度.

4.2 带有罚函数的适应值函数

传统的PSO算法，其适应值函数一般定义为路径长度函数，即：

Fl=‖p1-p2‖+…+‖pN-1-pN‖

(17)

‖p1-p2‖表示p1与p2表示与节点之间的距离.但在实际路径规划中，路径的安全性有时比路径的长度更受重视.为使机器人预先感知障碍物，做好避障准备.本文在障碍物周围设置一个“防护区域”，并设计了基于罚函数的适应值函数.当穿越“防护区域”时，罚函数会相应增大：

(18)

Dob代表障碍物区域，在此区域，设定罚函数为200.Dsa为防护区域，在此区域，当前点的罚函数与障碍物中心ob的距离成反比.其他区域的罚函数都为0.d为粒子距离障碍物中心的距离，rob为障碍物区域半径，rsa为防护区域的半径.

本文结合路径长度函数与罚函数，将适应值函数定义为：

F=aFl+bFf，0≤a，b≤1，a+b=1

(19)

a，b代表分别为长度函数权重与罚函数权重.本文a，b取0.5.

4.3 算法流程

算法的流程描述如下：

Step 1.按照初始策略初始化粒子群，包括各参数：粒子位置Xi、速度Vi、粒子的历史最佳位置pBesti，整个群体历史上的最佳位置gBest，惯性权重ω，学习因子c1、c2.

Step 2.按式(1)和式(2)更新粒子速度和位置.

Step 3.将粒子位置作为控制点，得到平滑的Bezier曲线.

Step 4.计算种群中各粒子的适应度.

Step 5.根据适应度更新粒子的历史最佳位置.

Step 6.根据适应度更新粒子的全局最佳位置.

Step 7.计算δgBest(k)，δpBesti(k)和来判定种群的进化状态.

Step 8.根据种群的进化状态，更新惯性权重ω，学习因子c1、c2.

Step 9.判断种群是否为困死状态，如果种群处于困死状态，转Step 10.否则转Step 11.

Step 10.根据式(10)执行群体跳出策略.

Step 11.判断算法终止条件，如满足则停止.否则转Step 2.

5 实验结果与分析

本节首先将验证初始化策略的有效性.接着在两个测试环境上使用ES-PSO算法与其他改进PSO算法进行路径规划，并比较算法的寻优精度与寻优速度.

5.1 初始化策略的效果

假设在一个测试环境中，共设置四个圆形障碍物，机器人起点坐标为[50，50]，终点坐标为[450，450].算法种群粒子数量M设为30，路径节点N设为5，迭代200次，粒子速度限制为[-50，50].惯性权重ω=[0.9，0.4]，学习因子c1和c2初始值设为2.0.ω、c1和c2三个参数随算法迭代过程中种群进化状态的变化实现自适应控制.

在使用本文提出的ES-PSO算法进行路径规划时，折线为ES-PSO算法规划出的路径，曲线为结合Bezier生成的平滑路径.在比较初始化策略的效果时，主要比较算法的寻优精度与寻优速度的差异.

图2 初始化策略下的结果比较
Fig.2 Comparison of results using initialization strategy

由图2可知，采用初始化策略后的迭代曲线相比未采用初始化策略的迭代曲线较低，则表明初始化策略可增强算法的寻优精度.此外，采用初始化策略的曲线，率先进入平稳状态，则表明初始化策略可缩短算法的寻优时间.

本实验重复测试100次，得到平均适应值函数Fitness、平均迭代次数Iteration和路径可行的成功率Success，如表1所示.由表1可得，采用初始化策略后的路径规划成功率比无初始化策略较高，则表明初始化策略可增强路径规划成功率.

表1 初始化策略效果
Table1 Initialize policy effects

5.2 算法寻优性能比较

在如图4和图6所示的测试环境下比较标准粒子群算法PSO[1]、全局粒子群算法GPSO[20]、自适应粒子群算法APSO[21]和本文提出的ES-PSO算法寻优性能.

为增强算法比较的公平性，所有算法种群粒子数量M设为30，路径节点N设为5，迭代2000次，粒子速度限制为[-50，50].PSO算法中，惯性权重ω设为固定值0.4.学习因子c1和c2设为固定值2.0.GPSO算法中，惯性权重ω=[0.9，0.4].ω随迭代次数线性递减.APSO算法中，学习因子c1和c2初始值设为2.0.ω、c1和c2三个参数随算法迭代过程中种群多样性的变化实现自适应控制.ES-PSO算法中，惯性权重ω=[0.9，0.4]，学习因子c1和c2初始值设为2.0.ω、c1和c2三个参数随算法迭代过程中种群进化状态的变化实现自适应控制.

图4所示的简单测试环境，用以检验本文所提出的ES-PSO算法在寻优速度方面的性能.在此设置四个圆形障碍物，机器人起点坐标为[50，50]，终点坐标为[450，450].图3和图4分别为在简单测试环境下四种粒子群算法适应值变化曲线与规划所得路径.

图3 简单环境下适应值变化曲线
Fig.3 Adaptive value curve in a simple environment

由图4可知，四种算法都找到了可行路径，ES-PSO算法可以跳出局部最优值，寻找到更加优化的路径.在该测试环境下重复测试100次，实验数据如表2所示.由表2可知，PSO算法因为结构最为简单，收敛速度最快，但是会陷入局部最优值，导致算法寻优精度不高，甚至影响路径规划的成功率.ES-PSO在保持算法快速收敛的特性下，显著提升了算法的寻优精度，并保证路径规划的成功率.

图4 简单环境下规划路径结果
Fig.4 Planning path results in a simple environment

表2 简单环境路径规划
Table 2 Simple environmental path planning

如图6所示的复杂测试环境，用以检验本文所提出的ES-PSO算法在寻优精度方面的性能.设置大量障碍物，且障碍物形状不仅限于圆形，机器人起点坐标为[50，50]，终点坐标为[450，450].图5和图6分别为复杂测试环境下四种粒子群算法适应值变化曲线与规划所得路径.

在图6所示测试环境下重复测试100次，实验数据如表3所示.由于测试环境变得复杂，PSO算法和GPSO算法因为没有跳出局部最优值的策略，算法过早收敛，导致路径规划的成功率下降.APSO算法寻优精度和成功率都较为满意，但是损害了算法快速收敛的特性.ES-PSO算法在寻优精度，寻优速度和成功率三个性能方面，相比其他三种粒子群算法，都有明显优势.

图5 复杂环境下适应值变化曲线
Fig.5 Adaptation value curve in complex environment

图6 复杂环境下规划路径结果
Fig.6 Planning path results in a complex environment

表3 复杂环境路径规划
Table 3 Complex environmental path planning

6 结 论

本文提出了一种基于种群进化状态的自适应粒子群算法(ES-PSO).ES-PSO的特点是根据种群在连续迭代的过程中，粒子历史最佳位置和种群全局最佳位置的改进情况，从而判定种群的进化状态.并根据种群的进化状态设计自适应控制器用于调节惯性权重与学习因子.ES-PSO保留标准PSO算法的快速收敛特性，并采用群体跳出策略摆脱局部最优区域，防止算法早熟.将ES-PSO算法应用到路径规划实际问题中去，并采用本文提出的初始化路径策略和带有罚函数的适应值函数策略，在两种测试环境下进行实验，检验算法性能.实验测试结果表明，ES-PSO算法在测试中脱颖而出，具有显著的优势，并结合Bezier曲线，可得到平滑的路径，更加符合实际机器人运动情况，是一种有效的寻优算法.此外，由于ES-PSO算法具有很好的实时性，可以进一步将ES-PSO算法应用于网络优化、图像处理等其它实际问题中.