基于改进粒子群算法的智能机器人路径规划
摘 要：
针对粒子群算法局部寻优能力差的缺点，提出一种非线性动态调整惯性权重的改进粒子群路径规划算法。该算法将栅格法与粒子群算法进行有效结合，在路径长度的基础上引入安全度和平滑度概念，建立动态调整路径长度的适应度函数。与传统的粒子群算法相比，实验结果表明，改进算法具有较强的安全性、实时性及寻优能力。
关键词：智能机器人；路径规划；栅格法；粒子群算法
中图分类号： TP301.6
文献标志码：A
Path planning for intelligent robots based on improved particle swarm optimization algorithm
Abstract：
As regards the poor local optimization ability of Particle Swarm Optimization （PSO）， a nonlinear dynamic adjusting inertia weight was put forward to improve the particle swarm path planning algorithm. This algorithm combined the grid method and particle swarm algorithm， introduced the two concepts of safety and smoothness based on path length， and established dynamic adjustment path length of the fitness function. Compared with the traditional PSO. The experimental results show that the improved algorithm has stronger security， real-time and optimization ability.
Key words：
intelligent robot； path planning； grid method； Particle Swarm Optimization （PSO） algorithm
0 引言
路径规划是智能机器人导航的最基本环节之一，它是指智能机器人在具有障碍物的工作环境中，按照某一性能指标（如距离、时间、能量等），不间断地利用所携带的传感器去认知周围的环境，读取障碍物的大小、位置和距离，不断地感知环境信息和周围障碍物的变化，搜索一条从起始状态到目标状态的最优或近似最优的安全、无碰撞路径。根据智能机器人对环境信息的已知程度，路径规划可分为两类：一类是环境信息已知的全局路径规划，另一类是环境信息未知或部分已知的局部路径规划[1]。目前，常用的路径规划方法主要有粒子群（Particle Swarm Optimization，PSO）算法、人工势场法、栅格法、神经网络法和蚁群算法等。相比其他算法而言，粒子群算法具有收敛速度快、设置参数少、实现简单等特点，近年来受到很多学者的重视，并成功应用于许多领域。但粒子群算法本身还存在着一些缺陷，如局部寻优能力差、速度和位置更新公式不够完善等问题，严重影响了路径规划的计算效率和可靠性。在对粒子群算法的深入研究中，国内外学者对其固有缺陷提出了各种改进方法，主要通过引入固定的惯性权重和学习因子等方法对速度更新公式进行修改，有所改观但并不完美。针对上述问题，本文采用栅格法建立环境模型，以粒子群算法为基本演化算法，引入非线性动态调整惯性权重和改进适应度函数的方法对粒子群算法加以改进[2]，将粒子群算法直接运用到栅格法中，得到智能机器人全局最优路径。
1 粒子群算法基本思想
粒子群（PSO）算法是一种群智能算法，由Eberhart博士和Kennedy博士于1995年提出[3]，其基本思想源于对鸟群的行为模拟，通过群体中粒子间的合作与竞争而产生的群体智能来指导优化搜索。PSO算法用随机解初始化一群随机粒子，通过迭代找到最优解，在每一次迭代中，每个粒子通过跟踪个体极值（pbest）和全局极值（gbest）来更新自己。同时，每个粒子不断改变其在解空间中的速度，以决定个体的走向及飞行距离，并尽可能朝个体极值（pbest）和全局极值（gbest）所指向的区域“飞”去。
PSO算法常规的数学描述为：设在一个N维的目标搜索空间中，有m个粒子组成一个群落，其中第i个粒子在N维空间里第d维分量（1≤d≤N）的速度和位置变化公式为：
其中：vkid表示迭代第k次时粒子i飞行速度矢量的第d维分量；xkid表示迭代第k次时粒子i位置矢量的第d维分量；pkid表示迭代第k次时粒子i个体最优位置的第d维分量；gkid表示迭代第k次时粒子群全局最优位置的第d维分量；c1和c2为非负常数的加速因子，加速因子使粒子具有自我总结和向群体中优秀个体学习的能力，从而向自己的历史最优点及群体内（或邻域内）的历史最优点靠近；r1和r2为[0，l]内的随机数；w为惯性权重，通过调整w可增强粒子局部搜索的能力，克服粒子自身存在局部搜索能力差的缺陷。
2 环境模型的建立
在二维平面上对智能机器人的工作环境用改进的栅格法进行建模。环境建模的有效性对于机器人能否高效地规划和避障具有关键性的作用，栅格法的优点是建模方便迅速，能运用多个栅格的拼合来代表多种形状的障碍物，以减少可行区域由障碍物建模造成的损失。但该方法也存在不足，采用传统栅格的序列进行路径规划时，当栅格粒度越小，虽然障碍物的表示越精确，但同时会占用大量的存储空间和延长规划时间，算法的搜索范围将按指数增加；当栅格粒度太大，规划的路径会很不精确。
本文将采用一种改进的栅格法进行建模[4]，为保证机器人能够在环境模型中无碰撞地运动， 将机器人模型简化为一个很小的质点，把机器人的实际尺寸折算进障碍物的面积内，根据机器人的实际尺寸将障碍物的边界向外扩展。如果某一栅格中存在障碍物，则定义此黑色栅格为障碍栅格，表示为1；反之白色为自由栅格，表示为0。用多个栅格的拼合来代表不同形状的障碍物，自由栅格则被合并成为机器人的可达区域，且栅格的大小以对障碍物表示的精确度为准。
本文在对空间进行建模时，对空间内的障碍物做如下处理，实际障碍物如图1（a）所示，处理后障碍物如图1（b）所示：
1）不满一个栅格时算一个栅格；
2）障碍物的空凹部分和这个障碍物算成一个整体障碍物，避免局部死区的出现，这叫作障碍物的合并；
3）把地图的边界当成障碍物来处理。
3 改进粒子群算法的路径规划
在建立的空间模型中，利用粒子群算法直接找出一条最优路径。粒子群算法中的每一个粒子都有一个解，也就是一条路径，粒子群算法从众多的解中寻找一个最优解形成最优路径。每个粒子的优化函数是粒子所代表的路径长度，在这里具体指的是每一个粒子所跨过的格数。
3.1 粒子的有效性
粒子的有效性是指粒子中任意两个相邻的元素之间的矩形区域能自由连通，中间不能有障碍物。即满足约束条件且所经过的栅格为自由栅格的粒子为有效粒子，只要粒子中任意两个相邻的元素不能自由连通或栅格中同一行（同一列）迂回出现两次路径，该粒子就无效。图2中三幅图表示的是粒子p从第k次迭代到第k+1次有效路径的实例，图3中三幅图表示的是粒子p从第k次迭代到第k+1次无效路径的实例。
3.2 粒子适应度函数
适应度函数是粒子群优化算法中的一个很重要的因素，它决定了算法能否收敛。本文在以路径长度作为适应度函数的基础上加入安全度、平滑度，对所有参数进行加权平均[2]，以满足复杂环境下机器人路径规划的要求。
1）规划的路径尽可能短，路径的长度可由下式表示：
式中：f1表示一个粒子中所有相邻顶点之间的直线距离的和，（xi，yi）表示粒子当前的坐标，（xi+1，yi+1）表示粒子下一位置的坐标。
2）引入惩罚函数，提高路径的安全度。当机器人与障碍物相撞时，在机器人的路径长度上引入一个惩罚函数，粒子碰撞的障碍物越多，施加的惩罚越大，使得此路径生成的概率越小。惩罚函数表示为：
3.3 非线性动态调整惯性权重的PSO算法
标准粒子群算法中，前期该算法容易陷入局部极值，产生早熟收敛。同时，由于迭代后的速度呈线性递增趋势，到后期无法进行较细的局部搜索，无法达到完全避碰的路径规划要求。
针对以上缺陷，以Kennedy和Eberhart为代表的专家学者引入惯性权重w的粒子群算法（Particle Swarm Optimization algorithm with Weight，WPSO）[1]，以限制迭代后期由于速度过快而无法进行精细局部搜索。引入初期，取固定值w可有效改善这一缺陷，随后又提出了线性递减规律改变惯性权重的粒子群算法（Particle Swarm Optimization algorithm with Linearly Decreasing Weight，LDWPSO），具体计算公式[5]如下：
式中：wk为当前迭代所得的值，其初始值为wmax；k表示当前迭代次数；kmax表示最大迭代次数。惯性权重wk+1从初始值wmax随着n的不同值以及迭代变化wk的值，非线性下降，当k=0时wk+1=wmax；当k=kmax时减小到最小值wmin。根据个体粒子和粒子群所处的不同区域，惯性权重按不同的非线性指数n下降[6]。当个体粒子和粒子群处于远离个体极值和全局极值的中心区域时，取n=1.1，由式（8）可知，惯性权重下降速度减慢，这样惯性权重在这一阶段具有较大值，粒子群将以较快的速度飞向群体最优位置；当个体粒子和粒子群逐渐靠近目标最优值时，进入中心范围后，取n=0.9，由式（8）可知，惯性权重下降加快，粒子在最优值所在区域对优化目标进行更加细致的搜索。中心区域的范围界限为个体粒子和全局粒子所发现的距个体极值和全局极值最大距离的中间位置。
4 仿真结果与分析
本文利用Matlab7.1在Intel Core2主频 1.86GHz 计算机上进行了仿真实验。仿真参数选择如下：种群规模即粒子个数M=50，粒子最大速度vmax=15，加速因子c1=c2=2，算法最大迭代次数kmax=200，最大惯性权重wmax=0.9，最小惯性权重wmin=0.4[7]，固定的惯性权重设定为w=0.7，对于动态的惯性权重，惯性权重w随着迭代次数k的增加非线性从0.9减少到0.4。
为了验证本算法的有效性，首先进行粒子群算法收敛性的判断，本文采用经典评价函数Rastrigin函数作为粒子群算法的适应度函数，仿真结果如图4所示。Rastrigin函数公式如下：
Rastrigin函数用于求解粒子群最小的群体全局最优值，对WPSO算法的群体全局最优值zbest=1.0×exp（-0.1），LDWPSO算法的群体全局最优值zbest=1.0×exp（-0.4），本文算法的群体全局最优值zbest=1.0×exp（-0.6），本文算法全局最优值最好。三种惯性权重都是趋向于0，但也存在差距，相对而言，非线性的本文算法曲线比其他两种曲线更平滑地收敛于零。本文算法迭代到10次时已经收敛到群体全局最优；而LDWPSO算法迭代到32次时收敛到群体全局最优，WPSO算法迭代到65次时才收敛到群体全局最优。总体来说，本文算法收敛速度更快，准确度更高。
利用栅格模型对粒子群算法的智能机器人路径规划进行仿真，复杂障碍物环境模型下的仿真结果如图5所示。
从仿真结果可以看出，智能机器人从开始点“S”运动到终止点（目标点）“E”，三种算法都能实现寻优和避障能力。但WPSO机器人路径规划易陷入局部最优，在避障中过多地考虑障碍物带来的影响，而忽略了实时性；而LDWPSO机器人路径规划相比WPSO路径规划能跳出局部最优，却没有考虑障碍物和机器人边缘的影响，机器人路径中的安全性下降；本文算法针对两种算法存在的缺陷，考虑障碍物边缘和机器人转角次数的影响，引入路径长度的安全度和平滑度，增强了局部寻优能力，提高了路径中的安全性和实时性。将各种算法分别单独执行50次，表1记录了复杂环境下三种算法最优路径长度及仿真时间的结果。
从表1可以看到，三种算法中本文算法的最优路径最短，由于引入了非线性惯性权重及新的适应度函数，计算量较大，仿真时间的均值也大于其他两种算法，但在机器人路径规划应用中本文算法的收敛速度大于其他两种算法，且最优路径最短，则运行时间最短。总的来说，本文算法无论最优路径长度还是运行时间都优于对比算法。
5 结语
本文以栅格法为环境建模方法，直接将粒子群算法运用到智能机器人路径规划中，可在起点与终点之间得到一条简单安全的最优路径。计算机仿真证明该方法不仅可以找到最优路径，而且算法过程简单，容易实现，比传统粒子群算法能得出较优结果。
参考文献：
[1] LI L，YE T，TAN M. The present situation and the future of mobile robot technology research[J]. Robot， 2002，24（5）： 475-480.（李磊，叶涛，谭民.移动机器人技术研究现状与未来[J].机器人，2002，24（5）： 475-480.）
[2] GONG J，LI X. Path planning of small-size soccer robot based on particle swarm optimization[J]. Journal of Mechanical & Electrical Engineering， 2010，27（12）：116-121.（宫金超，李晓明. 基于粒子群优化算法的小型足球机器人路径规划[J].机电工程，2010，27（12）：116-121.）
[3] KENNEDY J， EBERHART R. Particle swarm optimization[C]// Proceedings of the 1995 IEEE International Conference on Neural Networks. Piscataway： IEEE， 1995： 1942-1948.
[4] YU H，LI X. Fast path planning of robot based on grid method[J].Microelectronics and Computer， 2005， 22（6）： 98-100.（于红斌，李孝安.基于栅格法的机器人快速路径规划[J].微电子学与计算机， 2005， 22（6）： 98-100.）
[5] ZHANG H， HE X. The Theories and applications of adaptive fuzzy control[M]. Beijing： Beihang University Press， 2002： 244-247.（张化光，何希勤.模糊自适应控制理论及其应用[M].北京：北京航空航天大学出版社， 2002： 244-247.）
[6] WANG H， QIAN F. Nonlinear inertia weight dynamic changing based on particle swarm optimization[J]. Computer Science， 2008，35（3）：146-148.（王辉，钱锋.基于惯性权重非线性动态变化的微粒群算法[J].计算机科学，2008，35（3）：146-148.）
[7] XIN C， YANG M. Smooth path planning of a mobile robot using stochastic particle swarm optimization[C]// Proceedings of the 2006 IEEE International Conference on Mechatronics and Automation. Piscataway： IEEE， 2006： 1722-1727.
[8] WANG L， CAO J， HAN C. A robot multi-sensor self-calibration method based on particle swarm optimization[J]. Robot， 2009，31（5）：391-396.（汪霖，曹建福，韩崇昭.基于粒子群优化的机器人多传感器自标定方法[J]. 机器人，2009，31（5）：391-396.）
[9] BACCARELL P， BURGHIGNOL I P，FREZZA I F. Fundamental modal properties of surface wave on metamaterial grounded slabs[J]. IEEE Transactions on Microwave Theory and Techniques， 2005， 53（4）： 2075-2084.
[10] KLOSE A. Algorithms for solving the single-sink fixed-charge transportation problem[J]. Computers and Operations Research，2006，35（6）：2079-2092.
[11] YANG S，ZHANG H. Matlab technology to implement the swarm intelligence and bionic calculation[M].Beijing： Publishing House of Electronics Industry，2012.（杨淑莹，张桦.群体智能与仿生计算-Matlab技术实现[M].北京：电子工业出版社，2012.）
[12] LI S. Mobile robot path planning based on particle swarm optimization algorithm[D].Xian： Xidian University， 2011.（李珊.基于粒子群算法的移动机器人路径规划研究[D].西安：西安电子科技大学，2011.）