D0I:10.13374/i.i8sn1001-t53.2010.03.024 第32卷第3期 北京科技大学学报 Vol 32 No 3 2010年3月 Journal of Un iversity of Science and Techno logy Beijing Mar.2010 基于粒子群算法的移动机器人全局路径规划策略 李擎 徐银梅张德政尹怡欣 北京科技大学信息工程学院，北京100083 摘要提出了一种基于保收敛粒子群优化算法的移动机器人全局路径规划策略，为移动机器人在有限时间内找到一条避 开障碍物的最短路径提供了一种解决方案·首先建立环境地图模型，将连接地图中起点和终点的路径编码成粒子，然后根据 獐碍物位置规划出粒子的可活动区域，在此区域内产生初始种群，使粒子在受限的区域内寻找最优路径·在搜索过程中，粒子 群优化算法的加速系数和惯性权重均随迭代次数自适应调节.仿真实验表明算法可在起点与终点之间找到一条简单安全的 最优路径。与其他文献所提的方法进行了对比研究，结果表明本文所提算法具有更快的搜索速度和更高的搜索质量. 关键词移动机器人：路径规划：粒子群优化算法；活动区域 分类号TP273.5:TP2426 G lobal path plann ing m ethod form obile robots based on the particle swam algo- rithm LIQ ing XU Yin mei ZHANG De-theng YIN Yixin School of Infomation Engineering University of Science and Technobgy Beijng Beijing 100083.Chna ABSTRACT A global path planning method formobile robots based on the guaranteed convergence particle swam optin ization algo- rithm is presented A solution is provided formobile mobots to find the shortest path avoiding obstacles in a lin ited period of tie First l an environm entalm ap is set up and a path connecting the start point and the end point is coded as a particle Then a particular ac- tive region for particles is mapped out according to the location of obstacles The initial particle population is generated withn this re- gion and particles fly in the active region to searh for the optium path In the search process both the acceleration coefficient and n- ertia weight of the particle swam optin ization algorithm are self-adaptively adjusted along with iteration processes It is proved that the algorithm can plan out a smple and safe optmum path connecting the start point and the end point by siulation experm ents Com par ative studies with a recently reported method show that the proposed algorithm has advantages such as faster search speed and higher search quality KEY W ORDS mobile robot path planning particle swam optin ization:active region 路径规划是自主式移动机器人导航的基本环节 通常可以找到最优解，但需要预先知道准确的全局 之一，所谓移动机器人的路径规划问题，是指在其 环境信息，到目前为止，对于全局路径规划问题已 工作空间中找到一条从起始点到目标点的，能避开 经有许多解决方法2-)，但这些方法大都受到信息 动静态障碍物且能满足某个优化目标（如行走路线 存储方式和规划时间的制约，计算量大，实时性差， 最短、能量消耗最少)的最优（或次优）路径，基于 不能很好地适应于全局路径规划问题，如何对全局 环境模型的路径规划方法山可以分为两种类型：环 路径规划方法做出改进，使之在较短的有限时间内 境信息完全已知的全局路径规划和环境信息完全未 规划出最优路径，是本文的研究目的所在， 知或部分未知的局部路径规划，全局路径规划方法 由Ebetharti和Kennedy提出的粒子群优化算法 收稿日期：2009-01-20 基金项目：国家自然科学基金资助项目(N。60374032):第36批国家留学回国人员科研启动基金资助项目 作者简介：李擎(l97-)男，教授，博士，Email liqing@ies ustb edu cn
第 32卷 第 3期 2010年 3月 北 京 科 技 大 学 学 报 ＪｏｕｒｎａｌｏｆＵｎｉｖｅｒｓｉｔｙｏｆＳｃｉｅｎｃｅａｎｄＴｅｃｈｎｏｌｏｇｙＢｅｉｊｉｎｇ Ｖｏｌ．32Ｎｏ．3 Ｍａｒ．2010 基于粒子群算法的移动机器人全局路径规划策略 李 擎 徐银梅 张德政 尹怡欣 北京科技大学信息工程学院北京 100083 摘 要 提出了一种基于保收敛粒子群优化算法的移动机器人全局路径规划策略为移动机器人在有限时间内找到一条避 开障碍物的最短路径提供了一种解决方案．首先建立环境地图模型将连接地图中起点和终点的路径编码成粒子然后根据 障碍物位置规划出粒子的可活动区域在此区域内产生初始种群使粒子在受限的区域内寻找最优路径．在搜索过程中粒子 群优化算法的加速系数和惯性权重均随迭代次数自适应调节．仿真实验表明算法可在起点与终点之间找到一条简单安全的 最优路径．与其他文献所提的方法进行了对比研究结果表明本文所提算法具有更快的搜索速度和更高的搜索质量． 关键词 移动机器人；路径规划；粒子群优化算法；活动区域 分类号 ＴＰ273 ＋．5；ＴＰ242．6 Ｇｌｏｂａｌｐａｔｈｐｌａｎｎｉｎｇｍｅｔｈｏｄｆｏｒｍｏｂｉｌｅｒｏｂｏｔｓｂａｓｅｄｏｎｔｈｅｐａｒｔｉｃｌｅｓｗａｒｍａｌｇｏ- ｒｉｔｈｍ ＬＩＱｉｎｇＸＵＹｉｎ-ｍｅｉＺＨＡＮＧＤｅ-ｚｈｅｎｇＹＩＮＹｉ-ｘｉｎ ＳｃｈｏｏｌｏｆＩｎｆｏｒｍａｔｉｏｎＥｎｇｉｎｅｅｒｉｎｇＵｎｉｖｅｒｓｉｔｙｏｆＳｃｉｅｎｃｅａｎｄＴｅｃｈｎｏｌｏｇｙＢｅｉｊｉｎｇＢｅｉｊｉｎｇ100083Ｃｈｉｎａ ＡＢＳＴＲＡＣＴ Ａｇｌｏｂａｌｐａｔｈｐｌａｎｎｉｎｇｍｅｔｈｏｄｆｏｒｍｏｂｉｌｅｒｏｂｏｔｓｂａｓｅｄｏｎｔｈｅｇｕａｒａｎｔｅｅｄｃｏｎｖｅｒｇｅｎｃｅｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎａｌｇｏ- ｒｉｔｈｍｉｓｐｒｅｓｅｎｔｅｄ．Ａｓｏｌｕｔｉｏｎｉｓｐｒｏｖｉｄｅｄｆｏｒｍｏｂｉｌｅｒｏｂｏｔｓｔｏｆｉｎｄｔｈｅｓｈｏｒｔｅｓｔｐａｔｈａｖｏｉｄｉｎｇｏｂｓｔａｃｌｅｓｉｎａｌｉｍｉｔｅｄｐｅｒｉｏｄｏｆｔｉｍｅ．Ｆｉｒｓｔ- ｌｙａｎｅｎｖｉｒｏｎｍｅｎｔａｌｍａｐｉｓｓｅｔｕｐａｎｄａｐａｔｈｃｏｎｎｅｃｔｉｎｇｔｈｅｓｔａｒｔｐｏｉｎｔａｎｄｔｈｅｅｎｄｐｏｉｎｔｉｓｃｏｄｅｄａｓａｐａｒｔｉｃｌｅ．Ｔｈｅｎａｐａｒｔｉｃｕｌａｒａｃ- ｔｉｖｅｒｅｇｉｏｎｆｏｒｐａｒｔｉｃｌｅｓｉｓｍａｐｐｅｄｏｕｔａｃｃｏｒｄｉｎｇｔｏｔｈｅｌｏｃａｔｉｏｎｏｆｏｂｓｔａｃｌｅｓ．Ｔｈｅｉｎｉｔｉａｌｐａｒｔｉｃｌｅｐｏｐｕｌａｔｉｏｎｉｓｇｅｎｅｒａｔｅｄｗｉｔｈｉｎｔｈｉｓｒｅ- ｇｉｏｎａｎｄｐａｒｔｉｃｌｅｓｆｌｙｉｎｔｈｅａｃｔｉｖｅｒｅｇｉｏｎｔｏｓｅａｒｃｈｆｏｒｔｈｅｏｐｔｉｍｕｍｐａｔｈ．Ｉｎｔｈｅｓｅａｒｃｈｐｒｏｃｅｓｓｂｏｔｈｔｈｅａｃｃｅｌｅｒａｔｉｏｎｃｏｅｆｆｉｃｉｅｎｔａｎｄｉｎ- ｅｒｔｉａｗｅｉｇｈｔｏｆｔｈｅｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎａｌｇｏｒｉｔｈｍａｒｅｓｅｌｆ-ａｄａｐｔｉｖｅｌｙａｄｊｕｓｔｅｄａｌｏｎｇｗｉｔｈｉｔｅｒａｔｉｏｎｐｒｏｃｅｓｓｅｓ．Ｉｔｉｓｐｒｏｖｅｄｔｈａｔｔｈｅ ａｌｇｏｒｉｔｈｍｃａｎｐｌａｎｏｕｔａｓｉｍｐｌｅａｎｄｓａｆｅｏｐｔｉｍｕｍｐａｔｈｃｏｎｎｅｃｔｉｎｇｔｈｅｓｔａｒｔｐｏｉｎｔａｎｄｔｈｅｅｎｄｐｏｉｎｔｂｙｓｉｍｕｌａｔｉｏｎｅｘｐｅｒｉｍｅｎｔｓ．Ｃｏｍｐａｒ- ａｔｉｖｅｓｔｕｄｉｅｓｗｉｔｈａｒｅｃｅｎｔｌｙｒｅｐｏｒｔｅｄｍｅｔｈｏｄｓｈｏｗｔｈａｔｔｈｅｐｒｏｐｏｓｅｄａｌｇｏｒｉｔｈｍｈａｓａｄｖａｎｔａｇｅｓｓｕｃｈａｓｆａｓｔｅｒｓｅａｒｃｈｓｐｅｅｄａｎｄｈｉｇｈｅｒ ｓｅａｒｃｈｑｕａｌｉｔｙ． ＫＥＹＷＯＲＤＳ ｍｏｂｉｌｅｒｏｂｏｔ；ｐａｔｈｐｌａｎｎｉｎｇ；ｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎ；ａｃｔｉｖｅｒｅｇｉｏｎ 收稿日期：2009--01--20 基金项目：国家自然科学基金资助项目 （Ｎｏ．60374032）；第 36批国家留学回国人员科研启动基金资助项目 作者简介：李 擎 （1971— ）男教授博士Ｅ-ｍａｉｌ：ｌｉｑｉｎｇ＠ｉｅｓ．ｕｓｔｂ．ｅｄｕ．ｃｎ 路径规划是自主式移动机器人导航的基本环节 之一．所谓移动机器人的路径规划问题是指在其 工作空间中找到一条从起始点到目标点的能避开 动静态障碍物且能满足某个优化目标 （如行走路线 最短、能量消耗最少 ）的最优 （或次优 ）路径．基于 环境模型的路径规划方法 ［1］可以分为两种类型：环 境信息完全已知的全局路径规划和环境信息完全未 知或部分未知的局部路径规划．全局路径规划方法 通常可以找到最优解但需要预先知道准确的全局 环境信息．到目前为止对于全局路径规划问题已 经有许多解决方法 ［2--4］但这些方法大都受到信息 存储方式和规划时间的制约计算量大实时性差 不能很好地适应于全局路径规划问题．如何对全局 路径规划方法做出改进使之在较短的有限时间内 规划出最优路径是本文的研究目的所在． 由 Ｅｂｅｒｈａｒｔ和 Ｋｅｎｎｅｄｙ提出的粒子群优化算法 DOI :10．13374／j．issn1001—053x．2010．03．024
,398 北京科技大学学报 第32卷 (particle swam optin ization algorith,PS0)作为 新方程，如下面两式所示，而其他粒子仍用原方程 一种模拟鸟群飞行的仿生算法，具有算法简洁、易于 (1)、(2)更新： 实现和鲁棒性好等优点，算法对种群大小不十分敏 1=-考+g+ω号+(1-2×and)(3) 感，其收敛速度快的特点非常适合于对移动机器人 路径规划这种实时性要求较高的复杂问题进行求解 1=g十ω岭+t(1-2Xand) (4) 搜索，文献[6]道次将PS0引入移动机器人路径规 式中， 划中，该方法首先使用D冰sta算法获得链接图的 2中， 15 最短路径，然后用P©0对所得路径中的节点位置进 =110.5, p>5 行二次优化以得到更优的路径，由于链接图并不能 其他 完全体现实际规划环境中的信息，二次优化后得到 为算法一次迭代过程中优化函数值连续保持不变 的路径不一定是全局最优路径，因此该方法限制了 的次数，s为一次迭代过程中优化函数值连续减小 PSO的全局寻优能力.文献[7提出了另一种基于 的迭代次数 PS0的全局路径规划算法，通过坐标变换将需规划 2模型描述 路径的二维编码简化为一维编码，充分利用了PS0 的全局寻优能力，但不能很好地解决早熟现象，文 对机器人运动空间建模时作如下假定：机器人 献[8]采用罚函数法表示粒子适应度函数，并加入 在二维有限空间中运动，空间中分布着有限个位置 碰撞能量测试点，极大地缩短了算法的执行时间，但 己知的静态障碍物，障碍物用多边形描述，且可以忽 没有给出碰撞和距离能量函数的权重比，并且该方 略其高度信息：为保证路径的安全性，这里把障碍物 法也不能保证寻得最优路径 按照机器人半径尺寸膨胀，以便将机器人看作质点， 本文旨在深入研究粒子群算法在移动机器人路 忽略其尺寸大小，即按照点机器人来处理问题 径规划问题中的应用.对所提出的方法在简单和复 如图1所示，在全局坐标系0XY中，S为机器 杂环境下进行了仿真实验，并同文献[7]中所提的 人的出发点，G为终点，图中黑色区域表示障碍物， 方法进行了仿真对比，仿真结果表明，无论是在路 机器人的路径规划就是在图中寻找一个点的集合 径长度还是在执行时间方面，本文所提的算法均优 P={S…,…,G,要求相邻点之间为直线 于文献[7]中的方法 连接且无障碍，问题的解是一条最短可行路径，它 1粒子群算法 对应优化算法中的一个粒子，点的坐标对应粒子的 位置，以机器人当前位置$作为原点，以$与目标 随机产生初始粒子群，而每一次迭代过程中粒 点G的连线作为X轴，以垂直于X且经过S点的直 子根据自身找到的最好解和整个群体目前找到的最 线作为Y轴，建立局部坐标系0XY设α为X轴与 好解来更新位置和速度)， X轴的夹角，则相应的坐标变换公式为： 粒子的速度和位置更新方程为： cosa 1=a岭十aand(房一考)十eand成(g-专) Y (5) sina cosd (1) =专+ 把线段SG作D十1等分，并在各个等分点上作 (2) 垂线，则垂线上点的y坐标即构成粒子的位置编 式中，为粒子在第k次迭代中第维的速度： 码对应于前面更新公式中的)等分点的个数D 为粒子在k十1次迭代中第潍位置；9、。为学习 即为粒子的维数.粒子的适应度取为路径长度： 因子，通常取为2；and、anb为[0,1]区间内 的伪随机数；ω为惯性权重，决定了粒子对前面速度 2 Y)= 十(#1一y)2 (6) 继承的多少；Pg分别为粒子自身找到的最好解和 N D+1 整个群体目前找到的最好解. 式中，L为线段SG的长度 对粒子i如果恰巧X=P:=s则速度更新仅仅 应用优化算法求得局部坐标系中的路径点后， 依赖于ω%项，使得算法不能保证收敛，出现早熟现 可以用反变换公式 象，为此Van den Bergh和Enge brecht提出了保收 PSO guaranteed convergence particle swam opti [[ (7) m ization GCPS0)o,对全局最好粒子用如下的更 得到路径点在全局坐标系中的坐标
北 京 科 技 大 学 学 报 第 32卷 （ｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎａｌｇｏｒｉｔｈｍＰＳＯ） ［5］作为 一种模拟鸟群飞行的仿生算法具有算法简洁、易于 实现和鲁棒性好等优点．算法对种群大小不十分敏 感其收敛速度快的特点非常适合于对移动机器人 路径规划这种实时性要求较高的复杂问题进行求解 搜索．文献 ［6］首次将 ＰＳＯ引入移动机器人路径规 划中该方法首先使用 Ｄｉｊｋｓｔｒａ算法获得链接图的 最短路径然后用 ＰＳＯ对所得路径中的节点位置进 行二次优化以得到更优的路径由于链接图并不能 完全体现实际规划环境中的信息二次优化后得到 的路径不一定是全局最优路径因此该方法限制了 ＰＳＯ的全局寻优能力．文献 ［7］提出了另一种基于 ＰＳＯ的全局路径规划算法通过坐标变换将需规划 路径的二维编码简化为一维编码充分利用了 ＰＳＯ 的全局寻优能力但不能很好地解决早熟现象．文 献 ［8］采用罚函数法表示粒子适应度函数并加入 碰撞能量测试点极大地缩短了算法的执行时间但 没有给出碰撞和距离能量函数的权重比并且该方 法也不能保证寻得最优路径． 本文旨在深入研究粒子群算法在移动机器人路 径规划问题中的应用．对所提出的方法在简单和复 杂环境下进行了仿真实验并同文献 ［7］中所提的 方法进行了仿真对比．仿真结果表明无论是在路 径长度还是在执行时间方面本文所提的算法均优 于文献 ［7］中的方法． 1 粒子群算法 随机产生初始粒子群而每一次迭代过程中粒 子根据自身找到的最好解和整个群体目前找到的最 好解来更新位置和速度 ［9］． 粒子 ｉ的速度和位置更新方程为： ｖ ｋ＋1 ｉｊ ＝ωｖ ｋ ｉｊ＋ｃ1ｒａｎｄ ｋ 1（ｐ ｋ ｉｊ—ｘ ｋ ｉｊ）＋ｃ2ｒａｎｄ ｋ 2（ｇ ｋ ｊ—ｘ ｋ ｉｊ） （1） ｘ ｋ＋1 ｉｊ ＝ｘ ｋ ｉｊ＋ｖ ｋ＋1 ｉｊ （2） 式中ｖ ｋ ｉｊ为粒子 ｉ在第 ｋ次迭代中第 ｊ维的速度；ｘ ｋ＋1 ｉｊ 为粒子 ｉ在 ｋ＋1次迭代中第 ｊ维位置；ｃ1、ｃ2为学习 因子通常取为 2 ［5］；ｒａｎｄ1、ｒａｎｄ2 为 ［ 01］区间内 的伪随机数；ω为惯性权重决定了粒子对前面速度 继承的多少；ｐ、ｇ分别为粒子自身找到的最好解和 整个群体目前找到的最好解． 对粒子 ｉ如果恰巧 Ｘｉ＝Ｐｉ＝ｇ则速度更新仅仅 依赖于 ωｖｉ项使得算法不能保证收敛出现早熟现 象为此 ＶａｎｄｅｎＢｅｒｇｈ和 Ｅｎｇｅｌｂｒｅｃｈｔ提出了保收 敛 ＰＳＯ （ｇｕａｒａｎｔｅｅｄｃｏｎｖｅｒｇｅｎｃｅｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉ- ｍｉｚａｔｉｏｎＧＣＰＳＯ） ［10］对全局最好粒子用如下的更 新方程如下面两式所示而其他粒子仍用原方程 （1）、（2）更新： ｖ ｋ＋1 ｉｊ ＝—ｘ ｋ ｉｊ＋ｇ ｋ ｊ＋ωｖ ｋ ｉｊ＋ρ ｋ （1—2×ｒａｎｄ ｋ ） （3） ｘ ｋ＋1 ｉｊ ＝ｇ ｋ ｊ＋ωｖ ｋ ｉｊ＋ρ ｋ （1—2×ｒａｎｄ ｋ ） （4） 式中 ρ 0＝1ρ ｋ＋1＝ 2ρ ｋ ｓ＞15 0∙5ρ ｋ ｆ＞5 ρ ｋ 其他 ｆ为算法一次迭代过程中优化函数值连续保持不变 的次数ｓ为一次迭代过程中优化函数值连续减小 的迭代次数． 2 模型描述 对机器人运动空间建模时作如下假定：机器人 在二维有限空间中运动空间中分布着有限个位置 已知的静态障碍物障碍物用多边形描述且可以忽 略其高度信息；为保证路径的安全性这里把障碍物 按照机器人半径尺寸膨胀以便将机器人看作质点 忽略其尺寸大小即按照点机器人来处理问题． 如图 1所示在全局坐标系 Ｏ-Ｘ′Ｙ′中Ｓ为机器 人的出发点Ｇ为终点．图中黑色区域表示障碍物 机器人的路径规划就是在图中寻找一个点的集合 Ｐ＝｛Ｓｙ1…ｙｊ…ｙＤＧ｝要求相邻点之间为直线 连接且无障碍．问题的解是一条最短可行路径它 对应优化算法中的一个粒子点的坐标对应粒子的 位置．以机器人当前位置 Ｓ作为原点以 Ｓ与目标 点 Ｇ的连线作为 Ｘ轴以垂直于 Ｘ且经过 Ｓ点的直 线作为 Ｙ轴建立局部坐标系 Ｏ-ＸＹ．设 α为 Ｘ轴与 Ｘ′轴的夹角则相应的坐标变换公式为： Ｘ Ｙ ＝ ｃｏｓα ｓｉｎα —ｓｉｎα ｃｏｓα Ｘ′ Ｙ′ （5） 把线段 ＳＧ作 Ｄ＋1等分并在各个等分点上作 垂线 ｌｊ则垂线上点的 ｙ坐标即构成粒子 ｉ的位置编 码 （对应于前面更新公式中的 ｘ ｋ ｉｊ）．等分点的个数 Ｄ 即为粒子的维数．粒子的适应度取为路径长度： ｆ（Ｙｉ）＝∑ Ｄ ｊ＝0 ＬＳＧ Ｄ＋1 2 ＋（ｙｊ＋1—ｙｊ） 2 （6） 式中ＬＳＧ为线段 ＳＧ的长度． 应用优化算法求得局部坐标系中的路径点后 可以用反变换公式 Ｘ′ Ｙ′ ＝ ｃｏｓα —ｓｉｎα ｓｉｎα ｃｏｓα Ｘ Ｙ （7） 得到路径点在全局坐标系中的坐标． ·398·
第3期 李擎等：基于粒子群算法的移动机器人全局路径规划策略 ,399. y=+X(一)（斤12…，D)(8) 式中，r为[01]上的随机数.y"和"为位置边 界约束，取值由各等分线}与可活动区域的交点 决定，如图2中的“；可活动区域在地图边界以 外的部分，则取为地图边界，如图2中的.速度 极值规定了粒子一次迭代中能够飞行的最大距 离，这里取为位置变化范围的10%，即： 图1路径编码方法 =0.1X(“一") (9) Fig 1 Path coding method 粒子初始速度取为： 0 =(2r-1)Xy (10) 3基于P9O的路径规划 式(1)、(3)和(4)中的惯性权重ω随迭代次数 线性减小)，可以使种群开始时大范围搜索新的解 3.1基本思想 空间，以后逐渐小范围搜索，加强搜索现有解空间的 初始化粒子时要满足粒子第潍分量和第汁 能力，即： 1维分量所对应地图中的两个点之间直线连接且无 障碍，即不在障碍物边界范围以内，为缩小粒子搜 w=wn一kX"a一0B (11) 索空间，提高搜索速度，这里提出了“可活动区域” 式中，k为当前迭代次数，k为设定的最大迭代次 这一概念，现举例说明如下：首先确定与规划起点 数，wm为最大惯性权重，wm为最小惯性权重，这 和规划终点连线(SG线)相交的障碍物顶点，并在 里取为=0.9n=0.41], SG线两侧找到与SG线距离最远的两个顶点，如 为加快搜索速度，本文中的加速系数也采用了 图钟的ef两点，然后通过efS和G作矩形 自适应调节算法，在搜索开始时采用较大的Q和较 abcd并在与所作矩形有交集的障碍物顶点中找到 小的，目的是使粒子飞遍整个搜索空间而不趋于 G线两侧与SG线距离最远的两个顶点，如图2中 个体极值点，而在迭代后期则采用较小的q和较大 的m、n两点，接着采用由m、nS和G形成的矩形 ab'c'代替原有矩形.重复以上过程，直到与所作 的。，以便使粒子趋于全局最优解，自适应调节 的公式如下所示： 矩形相交的障碍物不再存在时为止，将所得矩形在 SG线两侧各扩展一个较小的正数飞，这时得到的矩 (12) 形范围即为本文所指的粒子“可活动区域”，如图2 中的灰色区域所示，根据基本的几何知识可知，最 十2 (13) 短路径一定存在于该区域内，这样就变相地缩小了 式中，q和e分别为q和g的初始值，q和分别 搜索空间 为q和g的最终值.q:=2.5,g:=0.5qr=0.5 gr=2.54 在粒子搜索过程中，每进行一次速度和位置的 更新，部分有效粒子会变为无效粒子，这里的无效是 相对有效而言的，指粒子所代表的路径穿过障碍物， 即为有碰的，有碰路径不能被接受，必须被放弃， 如果对所有无效粒子都重新进行初始化必然要耗费 大量的计算时间，为此本文提出直接将部分有碰路 径粒子的位置取为整个群体目前找到的最优位置， 在该位置附近细化搜索；另一部分有碰路径粒子的 位置取为邻域粒子所找到的最好解L?使得该无效 粒子进入邻域粒子所在的局部最优区域搜索，其中 图2可活动区域图例 L由 Fg 2 Illustration for the active region L,∈{P-pP-H1…,P-P,P+1…,PtP+1, 粒子第维的初始位置由下式计算得到： f(L)=mini f(P1),f(P)...f(P-1).f(P:)
第 3期 李 擎等： 基于粒子群算法的移动机器人全局路径规划策略 图 1 路径编码方法 Ｆｉｇ．1 Ｐａｔｈｃｏｄｉｎｇｍｅｔｈｏｄ 3 基于 ＰＳＯ的路径规划 3∙1 基本思想 初始化粒子时要满足粒子 ｉ第 ｊ维分量和第 ｊ＋ 1维分量所对应地图中的两个点之间直线连接且无 障碍即不在障碍物边界范围以内．为缩小粒子搜 索空间提高搜索速度这里提出了 “可活动区域 ” 这一概念．现举例说明如下：首先确定与规划起点 和规划终点连线 （ＳＧ线 ）相交的障碍物顶点并在 ＳＧ线两侧找到与 ＳＧ线距离最远的两个顶点如 图 2中的 ｅ、ｆ两点然后通过 ｅ、ｆ、Ｓ和 Ｇ作矩形 ａｂｃｄ并在与所作矩形有交集的障碍物顶点中找到 ＳＧ线两侧与 ＳＧ线距离最远的两个顶点如图 2中 的 ｍ、ｎ两点接着采用由 ｍ、ｎ、Ｓ和 Ｇ形成的矩形 ａ′ｂ′ｃ′ｄ′代替原有矩形．重复以上过程直到与所作 矩形相交的障碍物不再存在时为止将所得矩形在 ＳＧ线两侧各扩展一个较小的正数 ε这时得到的矩 形范围即为本文所指的粒子 “可活动区域 ”如图 2 中的灰色区域所示．根据基本的几何知识可知最 短路径一定存在于该区域内这样就变相地缩小了 搜索空间． 图 2 可活动区域图例 Ｆｉｇ．2 Ｉｌｌｕｓｔｒａｔｉｏｎｆｏｒｔｈｅａｃｔｉｖｅｒｅｇｉｏｎ 粒子 ｉ第 ｊ维的初始位置由下式计算得到： ｙｊ＝ｙ ｍｉｎ ｊ ＋ｒ×（ｙ ｍａｘ ｊ —ｙ ｍｉｎ ｊ ） （ｊ＝12…Ｄ） （8） 式中ｒ为 ［01］上的随机数．ｙ ｍａｘ ｊ 和 ｙ ｍｉｎ ｊ 为位置边 界约束取值由各等分线 ｌｊ与 “可活动区域 ”的交点 决定如图 2中的 ｙ ｍａｘ ｊ ；“可活动区域 ”在地图边界以 外的部分则取为地图边界如图 2中的 ｙ ｍｉｎ ｊ ．速度 极值 ｖｊｍａｘ规定了粒子一次迭代中能够飞行的最大距 离这里取为位置变化范围的 10％ ［11］即： ｖｊｍａｘ＝0∙1×（ｙ ｍａｘ ｊ —ｙ ｍｉｎ ｊ ） （9） 粒子初始速度取为： ｖ 0 ｊ＝（2ｒ—1）×ｖｊｍａｘ （10） 式 （1）、（3）和 （4）中的惯性权重 ω随迭代次数 线性减小 ［12］可以使种群开始时大范围搜索新的解 空间以后逐渐小范围搜索加强搜索现有解空间的 能力即： ω＝ωｍａｘ—ｋ× ωｍａｘ—ωｍｉｎ ｋｍａｘ （11） 式中ｋ为当前迭代次数ｋｍａｘ为设定的最大迭代次 数ωｍａｘ为最大惯性权重ωｍｉｎ为最小惯性权重．这 里取为 ωｍａｘ＝0∙9ωｍｉｎ＝0∙4 ［13］． 为加快搜索速度本文中的加速系数也采用了 自适应调节算法．在搜索开始时采用较大的 ｃ1和较 小的 ｃ2目的是使粒子飞遍整个搜索空间而不趋于 个体极值点而在迭代后期则采用较小的 ｃ1 和较大 的 ｃ2以便使粒子趋于全局最优解自适应调节 ［14］ 的公式如下所示： ｃ1＝（ｃ1ｉ—ｃ1ｆ） ｋｍａｘ—ｋ ｋｍａｘ ＋ｃ1ｆ （12） ｃ2＝（ｃ2ｉ—ｃ2ｆ） ｋｍａｘ—ｋ ｋｍａｘ ＋ｃ2ｆ （13） 式中ｃ1ｉ和 ｃ2ｉ分别为 ｃ1和 ｃ2的初始值ｃ1ｆ和 ｃ2ｆ分别 为 ｃ1和 ｃ2 的最终值．ｃ1ｉ＝2∙5ｃ2ｉ＝0∙5ｃ1ｆ＝0∙5 ｃ2ｆ＝2∙5 ［14］． 在粒子搜索过程中每进行一次速度和位置的 更新部分有效粒子会变为无效粒子这里的无效是 相对有效而言的指粒子所代表的路径穿过障碍物 即为有碰的．有碰路径不能被接受必须被放弃． 如果对所有无效粒子都重新进行初始化必然要耗费 大量的计算时间为此本文提出直接将部分有碰路 径粒子的位置取为整个群体目前找到的最优位置 在该位置附近细化搜索；另一部分有碰路径粒子的 位置取为邻域粒子所找到的最好解 Ｌｉ使得该无效 粒子进入邻域粒子所在的局部最优区域搜索其中 Ｌｉ由 Ｌｉ∈｛Ｐｉ—ｌＰｉ—ｌ＋1…Ｐｉ—1ＰｉＰｉ＋1…Ｐｉ＋ｌ—1Ｐｉ＋ｌ｝ ｆ（Ｌｉ）＝ｍｉｎ｛ｆ（Ｐｉ—ｌ）ｆ（Ｐｉ—ｌ＋1）…ｆ（Ｐｉ—1）ｆ（Ｐｉ） ·399·
,400 北京科技大学学报 第32卷 f(Pt)...f(P).f(P) (14) 起始点 100 确定，这里的邻域仅与粒子序号有关，而与粒子所 处的空间位置无关，该方法同重新初始化所有粒子 相比，既缩短了算法规划时间，又加强了粒子群搜索 多个局部最优的能力，保证了多样性、 3.2实现步骤 本文所提路径规划算法的具体实现步骤如下， Sepl初始化M个粒子，其具体过程为：由式 (8)初始化第个粒子位置.在“可活动区域内随 机产生粒子第汁1维分量，检测和第潍分量对应 100L 点的连线是否穿过障碍物，不穿过则产生下一维分 目标点 量，穿过则重新产生，如果对该维分量进行的若干 图3简单环境下粒子初始化 Fg3 Initial path in siple envirmment 次初始化尝试均失败，则要从粒子第1维重新开始 产生.由式(9)、式(10)初始化粒子速度.计算所 起始点 100 有粒子的适应度并将适应度最小的粒子位置设为全 局极值点，个体极值点则设为每个粒子的当前 位置. Sep2对全局最好粒子用式(3)更新粒子各维 的速度其他粒子由式(1)更新粒子各维的速度 同时注意边界约束，即若与>x,则取为 知ar若与<一ax则取为一ar Sep3.由式(2)更新粒子各维的位置其更新 流程既要考虑边界约束也要考虑障碍约束，障碍约 100 束的解决方法如算法基本思想中所述 目标点 Sep4:对每个粒子由式(6)求其适应度并更新 图4简单环境下最优路径 个体极值点P和全局极值点晋 Fig 4 Optinum path in siple envimmment Step5,转Step2进行迭代，直到达到设定的最大 执行时间为0.24s 迭代次数k或全局极值点适应度值连续20代保 起始点 100 持在一定的范围内 4仿真研究 4.1简单环境 假定机器人的工作空间大小为100×100，障碍 物的位置为顶点表示法0b1[(1030),(3030), (3010),(10,10)],0b2[(60,80),(80,80),(80, 50),(6050)]$(00)为起始点，G(100100)为目 标点，如图3所示.取粒子的维数为4，在简单环境 中初始化10个粒子对应的路径如图3所示.算法 100 达到最大迭代次数50时，经过0.31s搜索得到的最 目标点 图5复杂环境下粒子初始化 优路径如图4所示，其路径长度为147.572 Fig 5 Initial path in camplex enviromment 4.2复杂环境 图5为当机器人处于障碍物数目较多的环境空 4.3对比研究 间中时的初始种群，实验中取种群大小M=10,粒子 为了说明本文算法的优越性，同文献[7]中所 的维数D=8,最大迭代次数N=60所得到的最优 介绍的算法进行了对比仿真研究，在P℃(Pentim 路径如图6中所示，其最短路径长度值为143.824， processor1300MHz256MRAM)上分别进行20次
北 京 科 技 大 学 学 报 第 32卷 ｆ（Ｐｉ＋1）…ｆ（Ｐｉ＋ｌ—1）ｆ（Ｐｉ＋ｌ）｝ （14） 确定．这里的邻域仅与粒子序号有关而与粒子所 处的空间位置无关．该方法同重新初始化所有粒子 相比既缩短了算法规划时间又加强了粒子群搜索 多个局部最优的能力保证了多样性． 3∙2 实现步骤 本文所提路径规划算法的具体实现步骤如下． Ｓｔｅｐ1初始化 Ｍ个粒子其具体过程为：由式 （8）初始化第 ｉ个粒子位置．在 “可活动区域 ”内随 机产生粒子第 ｊ＋1维分量检测和第 ｊ维分量对应 点的连线是否穿过障碍物不穿过则产生下一维分 量穿过则重新产生．如果对该维分量进行的若干 次初始化尝试均失败则要从粒子第 1维重新开始 产生．由式 （9）、式 （10）初始化粒子速度．计算所 有粒子的适应度并将适应度最小的粒子位置设为全 局极值点．个体极值点则设为每个粒子的当前 位置． Ｓｔｅｐ2对全局最好粒子用式 （3）更新粒子各维 的速度 ｖｉｊ其他粒子由式 （1）更新粒子各维的速度 ｖｉｊ同时注意边界约束．即若 ｖｉｊ＞ｖｊｍａｘ则 ｖｉｊ取为 ｖｊｍａｘ；若 ｖｉｊ＜—ｖｊｍａｘ则 ｖｉｊ取为 —ｖｊｍａｘ． Ｓｔｅｐ3：由式 （2）更新粒子各维的位置 ｖｉｊ其更新 流程既要考虑边界约束也要考虑障碍约束障碍约 束的解决方法如算法基本思想中所述． Ｓｔｅｐ4：对每个粒子由式 （6）求其适应度并更新 个体极值点 Ｐ和全局极值点 ｇ． Ｓｔｅｐ5：转 Ｓｔｅｐ2进行迭代直到达到设定的最大 迭代次数 ｋｍａｘ或全局极值点适应度值连续 20代保 持在一定的范围内． 4 仿真研究 4∙1 简单环境 假定机器人的工作空间大小为 100×100障碍 物的位置为顶点表示法 Ｏｂ1［ （1030）（3030） （3010）（1010） ］Ｏｂ2［ （6080）（8080）（80 50）（6050） ］Ｓ（00）为起始点Ｇ（100100）为目 标点如图 3所示．取粒子的维数为 4在简单环境 中初始化 10个粒子对应的路径如图 3所示．算法 达到最大迭代次数 50时经过 0∙31ｓ搜索得到的最 优路径如图 4所示其路径长度为 147∙572． 4∙2 复杂环境 图 5为当机器人处于障碍物数目较多的环境空 间中时的初始种群实验中取种群大小 Ｍ＝10粒子 的维数 Ｄ＝8最大迭代次数 Ｎ＝60所得到的最优 路径如图 6中所示．其最短路径长度值为 143∙824 图 3 简单环境下粒子初始化 Ｆｉｇ．3 Ｉｎｉｔｉａｌｐａｔｈｉｎｓｉｍｐｌｅｅｎｖｉｒｏｎｍｅｎｔ 图 4 简单环境下最优路径 Ｆｉｇ．4 Ｏｐｔｉｍｕｍｐａｔｈｉｎｓｉｍｐｌｅｅｎｖｉｒｏｎｍｅｎｔ 执行时间为 0∙24ｓ． 图 5 复杂环境下粒子初始化 Ｆｉｇ．5 Ｉｎｉｔｉａｌｐａｔｈｉｎｃｏｍｐｌｅｘｅｎｖｉｒｏｎｍｅｎｔ 4∙3 对比研究 为了说明本文算法的优越性同文献 ［7］中所 介绍的算法进行了对比仿真研究．在 ＰＣ（Ｐｅｎｔｉｕｍ ｐｒｏｃｅｓｓｏｒ1300ＭＨｚ256ＭＲＡＭ）上分别进行 20次 ·400·
第3期 李擎等：基于粒子群算法的移动机器人全局路径规划策略 401. 起始点 0.45 100 O文献刀的算法 本文算法 0.40 0.35 0.20 89101112131415161718192021 运行次数 目标点 图8简单环境下执行时间对比 图6复杂环境下最优路径 Fig 8 Comparison of mn tie in siple envimmment Fig 6 Optmnum path in camplex envimmment 实验，图4所示的简单环境中取种群大小M=80 5结语 粒子的维数D=4,迭代次数N=100相同的条件下， 两种算法每次运行得到的最优路径长度和执行时间 仿真研究表明，本文所提出的移动机器人路径 分别如图7和图8所示，在如图4所示的简单环境 规划算法可在起点与终点之间得到一条简单安全的 和如图6所示的复杂环境下，两种算法结果对比如 最优路径，其主要特点有：(1)粒子群算法和其他优 表1所示，其中复杂环境下参数取值均为种群大小 化算法相比有实现简单，可调参数少的优点；(2)加 M=50,粒子维数D=8,迭代次数N=80可见改进 速系数和惯性权重均自适应调节，收敛速度快；(3) 后的算法求得最优路径长度和执行时间都显著缩 将迭代过程中的无效粒子直接取为全局极值点或邻 短、可活动区域的引入使粒子群更容易搜索到最 域个体极值点的方案，避免了重新初始化粒子，从而 优路径，改进后的粒子群优化算法加快了收敛速度， 缩短了规划时间；(4)“可活动区域”别除了不必要 156 的搜索区域，提高了粒子群算法的搜索效率 0文献7刀的算法 155 ■本文算法 154 参考文献 [1]Dai B Xiao X M.CaiZ X.Current status and future devebopment 59 of mobile mbot path planning technobgy Control Eng China 200512(3):198 151 (戴博，肖晓明，蔡自兴.移动机器人路径规划技术的研究现状 150 与展望.控制工程，2005,12(3)：198) 149 [2] Lozanoperez T.Aukmatic planning of manipultor transfer move- 148 ments EEE Trans Syst Man Cybem 1981 11 (10):681 ggg地o地， [3]TakahashiO.Schillng R J Motion planning in a plane using gen eralized voronoi diagnms IEEE Trans Rob Autom.1989,5 (2): 运行次数 143 图7简单环境下最优路径长度对比 [4]Yu H B LiX A.Fast path panning based on gril model of mobot Fig 7 Comnparison of the shortest length in smple envimomment M icmelectmon Canput 2005 22(6):98 表1两种算法结果平均值对比 (于红斌，李孝安·基于栅格法的机器人快速路径规划微电子 Table I Camparison of average vahes cakulated by wo algorithms 学与计算机，2005,22(6)：98) [5] Kennedy J Ebethart R C Particle wwam optin ization//Proceed- 最优路径长度 执行时间/ 环境 算法 ings of the 1995 EEE In temational Confennce on Neural Net 均值 方差 均值 方差 works Perth 1995:1942 文献[7]算法152.952.48 0.396.1×10-5 [6]Qn Y Q Sun D B Li N.et al Path planning for mobile mbot 简单 本文算法 147.561.1×10-40.31 1.7×10-4 based on particle swam optin ization Robot 2004.26(3):222 (秦元庆，孙德宝，李宁，等基于粒子群算法的移动机器人路 文献[7]算法160.12 16.86 1.71 4.10X10-3 复杂 径规划.机器人，2004,26(3)：222) 本文算法 143.631.80×10-41.432.04×10-4 [7]Sun B.Chen W D.XiY G.Particle swam optin ization based glob-
第 3期 李 擎等： 基于粒子群算法的移动机器人全局路径规划策略 图 6 复杂环境下最优路径 Ｆｉｇ．6 Ｏｐｔｉｍｕｍｐａｔｈｉｎｃｏｍｐｌｅｘｅｎｖｉｒｏｎｍｅｎｔ 实验．图 4所示的简单环境中取种群大小 Ｍ＝80 粒子的维数 Ｄ＝4迭代次数 Ｎ＝100相同的条件下 两种算法每次运行得到的最优路径长度和执行时间 分别如图 7和图 8所示．在如图 4所示的简单环境 和如图 6所示的复杂环境下两种算法结果对比如 表 1所示其中复杂环境下参数取值均为种群大小 Ｍ＝50粒子维数 Ｄ＝8迭代次数 Ｎ＝80．可见改进 后的算法求得最优路径长度和执行时间都显著缩 短．“可活动区域 ”的引入使粒子群更容易搜索到最 优路径改进后的粒子群优化算法加快了收敛速度． 图 7 简单环境下最优路径长度对比 Ｆｉｇ．7 Ｃｏｍｐａｒｉｓｏｎｏｆｔｈｅｓｈｏｒｔｅｓｔｌｅｎｇｔｈｉｎｓｉｍｐｌｅｅｎｖｉｒｏｎｍｅｎｔ 表 1 两种算法结果平均值对比 Ｔａｂｌｅ1 Ｃｏｍｐａｒｉｓｏｎｏｆａｖｅｒａｇｅｖａｌｕｅｓｃａｌｃｕｌａｔｅｄｂｙｔｗｏａｌｇｏｒｉｔｈｍｓ 环境 算法 最优路径长度 执行时间／ｓ 均值 方差 均值 方差 简单 文献 ［7］算法 152∙95 2∙48 0∙39 6∙1×10—5 本文算法 147∙56 1∙1×10—4 0∙31 1∙7×10—4 复杂 文献 ［7］算法 160∙12 16∙86 1∙71 4∙10×10—3 本文算法 143∙63 1∙80×10—4 1∙43 2∙04×10—4 图 8 简单环境下执行时间对比 Ｆｉｇ．8 Ｃｏｍｐａｒｉｓｏｎｏｆｒｕｎｔｉｍｅｉｎｓｉｍｐｌｅｅｎｖｉｒｏｎｍｅｎｔ 5 结语 仿真研究表明本文所提出的移动机器人路径 规划算法可在起点与终点之间得到一条简单安全的 最优路径．其主要特点有：（1）粒子群算法和其他优 化算法相比有实现简单可调参数少的优点；（2）加 速系数和惯性权重均自适应调节收敛速度快；（3） 将迭代过程中的无效粒子直接取为全局极值点或邻 域个体极值点的方案避免了重新初始化粒子从而 缩短了规划时间；（4） “可活动区域 ”剔除了不必要 的搜索区域提高了粒子群算法的搜索效率． 参 考 文 献 ［1］ ＤａｉＢＸｉａｏＸＭＣａｉＺＸ．Ｃｕｒｒｅｎｔｓｔａｔｕｓａｎｄｆｕｔｕｒｅｄｅｖｅｌｏｐｍｅｎｔ ｏｆｍｏｂｉｌｅｒｏｂｏｔｐａｔｈｐｌａｎｎｉｎｇｔｅｃｈｎｏｌｏｇｙ．ＣｏｎｔｒｏｌＥｎｇＣｈｉｎａ 200512（3）：198 （戴博肖晓明蔡自兴．移动机器人路径规划技术的研究现状 与展望．控制工程200512（3）：198） ［2］ ＬｏｚａｎｏｐｅｒｅｚＴ．Ａｕｔｏｍａｔｉｃｐｌａｎｎｉｎｇｏｆｍａｎｉｐｕｌａｔｏｒｔｒａｎｓｆｅｒｍｏｖｅ- ｍｅｎｔｓ．ＩＥＥＥＴｒａｎｓＳｙｓｔＭａｎＣｙｂｅｒｎ198111（10）：681 ［3］ ＴａｋａｈａｓｈｉＯＳｃｈｉｌｌｉｎｇＲＪ．Ｍｏｔｉｏｎｐｌａｎｎｉｎｇｉｎａｐｌａｎｅｕｓｉｎｇｇｅｎ- ｅｒａｌｉｚｅｄｖｏｒｏｎｏｉｄｉａｇｒａｍｓ．ＩＥＥＥＴｒａｎｓＲｏｂＡｕｔｏｍ19895 （2）： 143 ［4］ ＹｕＨＢＬｉＸＡ．Ｆａｓｔｐａｔｈｐｌａｎｎｉｎｇｂａｓｅｄｏｎｇｒｉｄｍｏｄｅｌｏｆｒｏｂｏｔ． ＭｉｃｒｏｅｌｅｃｔｒｏｎＣｏｍｐｕｔ200522（6）：98 （于红斌李孝安．基于栅格法的机器人快速路径规划．微电子 学与计算机200522（6）：98） ［5］ ＫｅｎｎｅｄｙＪＥｂｅｒｈａｒｔＲＣ．Ｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎ∥Ｐｒｏｃｅｅｄ- ｉｎｇｓｏｆｔｈｅ1995ＩＥＥＥＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＮｅｕｒａｌＮｅｔ- ｗｏｒｋｓ．Ｐｅｒｔｈ1995：1942 ［6］ ＱｉｎＹＱＳｕｎＤＢＬｉＮｅｔａｌ．Ｐａｔｈｐｌａｎｎｉｎｇｆｏｒｍｏｂｉｌｅｒｏｂｏｔ ｂａｓｅｄｏｎｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎ．Ｒｏｂｏｔ200426（3）：222 （秦元庆孙德宝李宁等．基于粒子群算法的移动机器人路 径规划．机器人200426（3）：222） ［7］ ＳｕｎＢＣｈｅｎＷ ＤＸｉＹＧ．Ｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎｂａｓｅｄｇｌｏｂ- ·401·