基于改进粒子群算法的机器人路径规划*

王志中

(重庆建筑工程职业学院，重庆 400072)

机器人可以帮助人类完成危险性高、重复性高、条件艰苦的工作，将人类从高风险、高体力劳动的工作中解放出来，而且机器人的使用是国家经济发展的巨大推力。移动机器人是机器人中智能性较高的一类，移动机器人导航包括位置定位、任务规划和路径规划，其中路径规划是机器人导航的基础性工作[1]。一条好的规划路径可以节省行走时间、减少能耗和磨损，达到提高工作效率、节约资源、减少能耗和成本等目的，因此机器人路径规划技术的研究具有重要的理论和现实意义。

机器人路径规划方法的研究可以分为两个阶段，前期的路径规划方法又称为传统方法，包括可视图法[2]、栅格法[3]、人工势场法[4]；当前路径规划方法主要为智能路径规划方法，就是使用智能算法搜索规划出最优路径，包括神经网络[5]、遗传算法[6]、蚁群算法[7]、模糊控制[8]以及它们的改进算法等。传统方法不具有智能性，面对复杂、随机、多约束的路径规划问题，其规划结果和普适性较差；智能算法在路径规划中会遇到不匹配或不理想的情况，因此出现了各种改进方法用于路径规划。

本文对环境障碍物膨化处理，并对环境坐标进行坐标变换，简化了环境模型；分析了粒子群算法的缺陷并融入了跳出机制和牵引操作，从而提出改进粒子群算法。将此方法应用于路径规划中，规划出的路径在长度、平滑性、规划时间上表现更优。

1 问题描述与适应度函数建立

1.1 障碍物处理

本文研究的是静态环境下机器人路径规划问题，使用智能算法对机器人路径进行规划，首先要进行环境建模，就是将机器人实际工作区域转化为数学模型，以便机器人进行识别。

在二维空间中，对各种形状的障碍物进行“膨化处理”，如图1所示，就是使用能够包含该障碍物的最小圆表示，障碍物大小用圆的面积衡量。

1.2 问题描述与简化

静态环境下机器人路径规划就是在障碍物分布已知的环境中，寻找某种意义下最优从起点至终点的无碰路径。以图2为例，图中在机器人工作区域建立了坐标系OXY，黑色实心物体为未膨化处理的障碍物，记起点坐标为S(xs,ys)，终点坐标为G(xg,yg)，由起点至终点的无碰路径为Path={S,P1,P2,…,Pj,…,PN,G}，式中j=1,2,…,N，Pj=(xj,yj)为一个位置坐标，若位置点Pj之间的连线不穿过障碍物则此路径为有效路径。

为了将上述问题进行简化，将二维优化问题转化为一维优化问题 ，本文使用了坐标变换的思想。就是以出发点S为坐标原点，X轴为出发点S与目标点G连线，正向为出发点指向目标点，根据右手定则确定Y轴，这样就建立了新坐标系O′X′Y′，记新旧坐标系之间的夹角为α，则两坐标系之间的转换公式为

(1)

1.3 适应度函数建立

在机器人路径规划中，规划路径的优劣程度使用适应度函数来评价。在路径优化中，可以考虑的因素很多，包括路径长度、平滑性、能耗等诸多方面，考虑到路径长度与能耗具有很强的相关性，本文对路径评价时考虑路径长度、路径平滑性两个方面。

(1)路径长度评价函数fit1的建立。本文使用欧氏距离构建路径长度评价函数，即:

(2)

式中：d=((xg-xs)/(N+1))2是一个定值。

(2)路径平滑度适应度函数fit2的建立。众所周知，两条路径的夹角越大则机器人的转弯压力越小；相反地，路径夹角越小则机器人转弯压力越大。所以本文使用路径之间的夹角作为评价路径平滑度的标准，即：

(3)

式中：β(lj,lj+1)表示路径lj与路径lj+1之间的夹角；N为路径中夹角的个数。分析式(3)可知，fit2值越大则路径平滑性越好，机器人转弯压力越小。

结合以上分析，给出机器人路径规划的适应度函数fit为：

(4)

式中:w1、w2分别为路径长度、路径平滑性的调节参数，此参数的设立，一是为了调节不同优化目标在适应度函数中的重要性，二是使不同性能指标实现量级上的统一。分析适应度函数fit可知，此函数值越大说明路径质量越高。

2 粒子群算法及改进

2.1 粒子群算法

(5)

2.2 改进粒子群算法

粒子群算法存在以下两个缺陷：(1)算法迭代后期，粒子种群多样性急剧下降，使算法陷入局部最优值而出现“早熟”现象；(2)当粒子被困于较差搜索区域而难以自动跳出时，就会导致搜索效率慢，算法收敛慢的问题。针对这两个问题，本文分别提出了跳出机制和牵引操作，用于提高粒子群算法的搜索精度和收敛速度。

(1)跳出机制。在粒子群算法迭代的后期，种群多样性减少，针对这种情况，当粒子多次迭代而适应度值相差不大时，认为此粒子搜索处于停滞状态，此时对于停滞状态的粒子，以一定概率p1执行跳出机制，使粒子摆脱停滞状态，继续执行搜索操作，从而保持种群的多样性。

在给出粒子跳出机制前，首先要定义粒子的视觉范围Visual和步长Step。粒子的视觉范围是粒子所能感知或看到的范围，粒子步长为粒子一步前进的最大幅度，且Visual>Step。则粒子的跳出机制定义为：

(6)

执行跳出机制时，首先在粒子i视觉范围内选择一个粒子j，若粒子j的适应度值大于粒子i，则粒子i向粒子j的方向前进一步；若粒子j的适应度值不大于粒子i，则在视觉范围内重新寻找一粒子，重复上述过程；若反复寻找Ntry次后，依然没有满足条件的粒子，则粒子i向任意方向前进一步，即：

(7)

而后粒子继续进行最优解的搜索。跳出机制能够保持种群的多样性，增强粒子群的全局搜索能力，避免粒子陷入局部最优。本文具体执行跳出机制时，规定连续T次出现本次和上次适应度值的差值在0.01之内时，认为粒子处于搜索停滞状态而执行跳出机制。

(2)牵引操作。当粒子被困于较差区域而无法自动跳出时，粒子在较差区域的反复搜索是无效的，是对搜索资源的浪费，导致搜索效率低，算法收敛速度慢。为了解决这一问题，本文提出了牵引操作，就是以一定概率p2选择陷入较差区域的粒子，对其执行牵引操作，使其尽快跳出较差区域而在其他可能包含最优解的区域搜索，实现搜索资源的有效分配。

以最小值优化问题为例，当粒子适应度值连续三次大于种群平均适应度值时，认为此粒子被困于较差区域而无法自动跳出，对于此类粒子以一定概率p2执行牵引操作，即：

(8)

式中:p0为牵引量，式(8)表示将被困粒子向个体最优和群体最优的中间位置牵引;c3为自适应牵引因子。其计算方法为：

(9)

式中：fit(i)为被困粒子i的适应度值；fitave、fitmin分别为种群中适应度平均值和最小值。分析式(9)可知，被困粒子i适应度值越大，说明此粒子被困区域越差，此时牵引因子越大，越能够使被困粒子摆脱被困区域，所以牵引因子c3具有自适应性。

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

综上所述，基于改进粒子群算法的机器人路径规划步骤为：

Step1：根据起点位置S和终点位置D建立新坐标系，并进行坐标变换，以下步骤均在新坐标系中进行，路径规划完毕后，再将规划结果转换到旧坐标系；

Step2：对所有障碍物进行处理，膨化为圆形；

Step5：需要执行牵引操作的粒子使用式(8)进行状态更新，其他粒子使用式(5)进行粒子状态更新；

Step7：更新个体历史最优值和种群最优值；

Step8：若搜索结果满足精度要求或算法迭代次数达到上限，则算法结束；否则转至Step5继续迭代。

4 仿真实验验证

使用仿真实验对本文提出的改进粒子群算法进行验证。机器人工作环境大小设置为200×200，算法最大迭代次数为2 000，种群大小m=100，粒子维数与平行直线数量一致N=30，学习因子c1=c2=2，粒子视觉范围Visual=5，步长Step=2，尝试次数Ntry=5，跳出机制的执行概率p1=0.2，牵引操作的执行概率p2=0.3，在初始坐标中起点坐标为S(0,0)，目标点坐标为G(70.7,-70.7)。

为了验证改进粒子群算法在机器人路径规划中的有效性，分别使用传统的粒子群算法与改进粒子群算法在同样条件下进行路径规划，共进行两组实验，第二组实验相较于第一组环境略微复杂，每组实验均执行100次，图4和图5给出了不同环境下两组算法的规划结果，表1给出了在环境一下两种算法的统计数据。

表1 环境一下两算法适应度值

算法最大值平均值最小值平均时间/s传统算法154141111945改进算法199179138653

由图4和图5中两种算法的规划结果可以看出，两种算法都能够规划出一条无碰路径。但是对比图4中两条路径。可以看出改进算法的路径平滑度明显优于传统算法；对比图5中两算法的规划结果，改进算法的规划路径不仅在平滑度上明显优于传统算法，而且改进算法的规划路径明显短于传统算法，传统算法规划出的路径存在绕弯路的情况。

由表1中数据可以看出，在100次实验中，改进算法规划路径适应度的最大值、平均值、最小值均大于传统算法。按照式(4)建立的适应度函数可知，改进算法规划出的路径在长度和平滑性上优于传统算法；且改进算法的路径规划时间也短于传统算法。

改进算法在路径长度、路径平滑度、规划时间上的优势，是因为改进算法中引入了跳出机制和牵引操作，跳出机制保持了种群多样性，也就保证了算法的全局搜索能力，所以其相对于传统算法能找到更优解；牵引操作能够牵引被困粒子向全局最优和自身历史最优运动，提高算法收敛速度，所以改进算法的规划时间短于传统算法。此仿真实验充分说明了改进算法在路径规划中的有效性。

5 结语

通过以上分析可以得出以下结论：(1)通过坐标变换，可以将二维优化问题简化为一维优化问题，简化了问题的求解；(2)跳出机制可以保持种群多样性，也就保持了算法的全局搜索能力；(3)牵引操作能够使被困粒子跳出被困区域，向全局最优和自身历史最优收敛，提高了算法收敛速度；(4)本文提出的改进算法规划出的路径在长度、平滑度和规划时间上均具有优势。

[1]李文学, 饶运清, 戚得众,等. 全向轮机器人路径规划与导航系统设计[J]. 机械设计与制造,2014(12):18-22.

[2]陈超, 唐坚, 靳祖光,等. 一种基于可视图法导盲机器人路径规划的研究[J]. 机械科学与技术, 2014, 33(4): 490-495.

[3]柴寅, 唐秋华, 邓明星,等. 机器人路径规划的栅格模型构建与蚁群算法求解[J]. 机械设计与制造, 2016, 4(4): 178-181.

[4] 刘传领. 基于势场法和遗传算法的机器人路径规划技术研究[D]. 南京：南京理工大学, 2012.

[5]吕战永, 曹江涛. 自反馈生物激励神经网络机器人路径规划[J]. 计算机工程与应用, 2014, 50(16):255-258.

[6] 田欣. 基于改进遗传算法的移动机器人路径规划研究[D]. 郑州：郑州大学, 2016.

[7] 金飞虎, 高会军, 钟啸剑. 自适应蚁群算法在空间机器人路径规划中的应用[J]. 哈尔滨工业大学学报, 2010, 42(7):1014-1018.

[8]徐悦, 林志贤, 姚剑敏,等. 关于机器人导航目标点搜索路径模糊控制[J]. 计算机仿真, 2016, 33(10):300-304.

[9] Singh G, Deep K. A new membrane algorithm using the rules of Particle Swarm Optimization incorporated within the framework of cell-like P-systems to solve Sudoku[M]. Elsevier Science Publishers B. V. ，2016.

[10]张浩, 张铁男, 沈继红,等. Tent混沌粒子群算法及其在结构优化决策中的应用[J]. 控制与决策, 2008, 23(8):857-862.