一种蚂蚁粒子群融合的机器人路径规划新算法

２００７，４３（３１）◎学术探讨◎
１引言
移动机器人路径规划是指在有障碍物的工作环境中，寻找一条从给定起始点到终止点的较优运动路径，使机器人在运动过程中能安全、无碰撞地绕过所有的障碍物，且所走路径较短。

根据机器人对环境信息掌握程度的不同，路径规划可以分为两种类型：环境信息完全已知的全局路径规划和环境信息完全未知或部分未知的局部路径规划。

对于环境信息完全已知的情况的研究已有广泛的报道，例如势场法［１］、遗传算法［２］等。

势场法结构简单，易于实现，得到了广泛的应用，但也有较大缺陷：存在ＧＮＲＯＮ（目标不可到达）问题、在障碍物面前振荡等。

遗传算法在求解最短路径时，由于编码长度变化范围很大，尤其在问题规模较大、地形复杂时，产生的无效路径较多，求解效率很低，故能有效求解的问题的规模也比较小。

基于对已有成果的研究并针对已有算法的不足，本文提出了一种新的机器人路径规划方法，用自由空间法［２，３］建立机器人运动的环境模型，在此基础上先用蚂蚁算法［４］规划一条无碰较优路径；再用粒子群算法［５］优化该路径，从而得到全局近似最优的路径。

计算机仿真实验表明，本方法能满足机器人快速路径规划的要求，并且在复杂的环境下也可以快速地规划出一条从起始点到目标点的优化路径，效果十分令人满意。

２环境描述
对机器人运动环境常用的建模方法有栅格法、可视图法等。

这些方法在进行路径规划时可得到较精确解，但建立模型与更新模型时的计算量较大，且对传感器精度的要求较高。

而自由空间法比较灵活，机器人起始点和目标点的改变不会造成连通图的重构，且大大减少建模的存储量。

因此，本文采用该方法进行环境建模。

为了描述方便且不失一般性和应用性，特作如下假设：
假设１将机器人简化为一个质点。

假设２机器人只在一个二维平面上运动。

假设３将机器人运动环境中的所有障碍物都用凸多边形表示。

由自由空间法构造的机器人可自由运动的网络图如图１
一种蚂蚁粒子群融合的机器人路径规划新算法
国海涛，朱庆保，司应涛
ＧＵＯＨａｉ－ｔａｏ，ＺＨＵＱｉｎｇ－ｂａｏ，ＳＩＹｉｎｇ－ｔａｏ
南京师范大学数学与计算机科学学院，南京２１００９７
ＳｃｈｏｏｌｏｆＭａｔｈｅｍａｔｉｃｓａｎｄＣｏｍｐｕｔｅｒＳｃｉｅｎｃｅ，ＮａｎｊｉｎｇＮｏｒｍａｌＵｎｉｖｅｒｓｉｔｙ，Ｎａｎｊｉｎｇ２１００９７，Ｃｈｉｎａ
Ｅ－ｍａｉｌ：ｇｕｏｈａｉｔａｏ２００５＠１６３．ｃｏｍ
ＧＵＯＨａｉ－ｔａｏ，ＺＨＵＱｉｎｇ－ｂａｏ，ＳＩＹｉｎｇ－ｔａｏ．Ｎｏｖｅｌｐａｔｈｐｌａｎｎｉｎｇｆｏｒｒｏｂｏｔｓｓｙｎｃｒｅｔｉｚｉｎｇａｎｔａｌｇｏｒｉｔｈｍａｎｄｐａｒｔｉｃｌｅｓｗａｒｍａｌｇｏｒｉｔｈｍ．ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇａｎｄＡｐｐｌｉｃａｔｉｏｎｓ，２００７，４３（３１）：３９－４１．
Ａｂｓｔｒａｃｔ：ＡｎａｌｇｏｒｉｔｈｍｂａｓｅｄｏｎＡｎｔＡｌｇｏｒｉｔｈｍ（ＡＡ）ａｎｄＰａｒｔｉｃｌｅＳｗａｒｍＯｐｔｉｍｉｚａｔｉｏｎ（ＰＳＯ）ａｌｇｏｒｉｔｈｍｆｏｒｐａｔｈｐｌａｎｎｉｎｇｏｆｔｈｅｒｏｂｏｔｉｓｐｒｏｐｏｓｅｄ．ＦｉｒｓｔｔｈｅＭＡＫＬＩＮＫｇｒａｐｈｉｓｂｕｉｌｔｔｏｄｅｓｃｒｉｂｅｔｈｅｗｏｒｋｉｎｇｓｐａｃｅｏｆｔｈｅｍｏｂｉｌｅｒｏｂｏｔ，ｔｈｅｎｔｈｅａｎｔａｌｇｏｒｉｔｈｍｉｓｕｓｅｄｔｏｏｂｔａｉｎｔｈｅｇｌｏｂａｌｎａｖｉｇａｔｉｏｎｐａｔｈ，ａｎｄｔｈｅｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎａｌｇｏｒｉｔｈｍｉｓａｄｏｐｔｅｄｔｏｇｅｔｔｈｅｂｅｔｔｅｒｐａｔｈ．Ｃｏｍｐｕｔｅｒｅｘｐｅｒｉｍｅｎｔｒｅｓｕｌｔｓｄｅｍｏｎｓｔｒａｔｅｔｈａｔｔｈｉｓｎｏｖｅｌａｌｇｏｒｉｔｈｍｃａｎｐｌａｎａｎｏｐｔｉｍａｌｐａｔｈｒａｐｉｄｌｙｉｎａｃｌｕｔｔｅｒｅｄｅｎｖｉｒｏｎｍｅｎｔ．Ｔｈｅｓｕｃｃｅｓｓｆｕｌｏｂｓｔａｃｌｅａｖｏｉｄａｎｃｅｉｓａｃｈｉｅｖｅｄ，ａｎｄｔｈｅｍｏｄｅｌｉｓｒｏｂｕｓｔａｎｄｐｅｒｆｏｒｍｓｒｅｌｉａｂｌｙ．
Ｋｅｙｗｏｒｄｓ：ｒｏｂｏｔ；ｐａｔｈｐｌａｎｎｉｎｇ；ａｎｔａｌｇｏｒｉｔｈｍ；ｐａｒｔｉｃｌｅｓｗａｒｍａｌｇｏｒｉｔｈｍ
摘要：研究了一种全新的蚂蚁粒子群融合的机器人路径规划算法。

该方法首先用链接图建立机器人运动空间模型，在此基础上利用蚂蚁算法进行全局搜索得到全局导航路径，然后用粒子群算法局部调节全局导航路径上的路径点，得到更优路径。

计算机仿真实验表明，即使在复杂的环境下，利用该算法也可以规划出一条全局优化路径，且能安全避障。

关键词：机器人；路径规划；蚂蚁算法；粒子群算法
文章编号：１００２－８３３１（２００７）３１－００３９－０３文献标识码：Ａ中图分类号：ＴＰ２４２．６
基金项目：国家自然科学基金（ｔｈｅＮａｔｉｏｎａｌＮａｔｕｒａｌＳｃｉｅｎｃｅＦｏｕｎｄａｔｉｏｎｏｆＣｈｉｎａｕｎｄｅｒＧｒａｎｔＮｏ．６０６７３１０２）；江苏省自然科学基金（ｔｈｅＮａｔｕｒａｌＳｃｉｅｎｃｅＦｏｕｎｄａｔｉｏｎｏｆＪｉａｎｇｓｕＰｒｏｖｉｎｃｅｏｆＣｈｉｎａｕｎｄｅｒＧｒａｎｔＮｏ．ＢＫ２００６２１８）。

作者简介：国海涛（１９８１－），男，硕士研究生，主要研究领域为人工智能与智能控制；朱庆保（１９５５－），男，教授，硕士生导师，主要研究领域为人工智能与智能控制；司应涛（１９８３－），男，硕士研究生，主要研究领域为人工智能与智能控制。

ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用３９
２００７，４３（３１）ＣｏｍｐｕｔｅｒＥｎｇｉｎｅｅｒｉｎｇａｎｄＡｐｐｌｉｃａｔｉｏｎｓ计算机工程与应用
２０
２１
２５２４
１９
２３
２２１８１７１２
１１
１３１４１５
１６１０７
９
８
６
４
５
３
２
１图１链接图
Ｐｉ
Ｐｉ１
Ｐｉ２
Ｐｉ＋１
图２路径调整示意图
所示，其中黑色表示障碍物，所标序号为路径点即机器人可行走的点。

３蚂蚁粒子群融合新算法３．１
算法总体思路及其相关定义
自由空间法构造的路径点是自由链接线的中点，如果机器
人仅仅沿着路径点行走，显然得到的路径不是最优的。

为了解决这一问题，首先用蚂蚁算法找到一条无碰较优导航路径，得到初始的路径点；再用粒子群算法调整各个初始路径点，从而得到全局近似最优路径。

调节过程如图２所示。

让Ｐｉ点在Ｐｉ１、Ｐｉ２两点所组成的线段上滑动，其具体位置可由式
（１）表示：Ｐｉ＝Ｐｉ１＋（Ｐｉ２－Ｐｉ１）×ｔｉ（ｔｉ∈［０，１］，ｉ＝１，２，…，ｎ）
（１）
式中，ｔｉ表示粒子位置的第ｉ维变量，当ｔｉ的值变化时，Ｐｉ随之改变。

用粒子群算法来搜索最佳ｔｉ，使得粒子的适应度函数取值最小（机器人自由运动空间由凸多边形构成，保证了Ｐｉ在线段
Ｐｉ１Ｐｉ２上滑动时形成的新路径不会与障碍物相交）。

对每个路径
点都这样处理后，这些新的路径点就组成了一条新的机器人移动路径。

定义１链接线长度指两直接相邻路径点间的距离，记作ｄ
（ｉ，ｊ），由式（２）计算：ｄ
（ｉ，ｊ）＝（ｘｉ－ｘｊ）２
＋（ｙｉ－ｙｊ）２
"（２）
其中链接线也称为弧，若两路径点不直接相连，则两点间的距
离视为无穷大。

定义２ａｎｔ＝｛１，２，…，ｋ，…，ｍ｝表示蚂蚁家族所有蚂蚁的集合，ｋ∈ａｎｔ表示某只蚂蚁，ｍ为蚂蚁家族的蚂蚁总数，τｉｊ（ｔ）表示蚂蚁在ｔ时刻残留在直接相邻路径点ｉ、ｊ连线上的信息量。

定义３蚂蚁ｋ已经选择并行走过的路径点组成的集合表示为ｔａｂｕｋ，它随着蚂蚁的行走动态调整。

定义４"ｉｊ＝Ｄ／ｄ（ｊ，ｇｏａｌ）称蚂蚁从路径点ｉ选择路径点ｊ的启发函数，Ｄ为权重常系数，ｇｏａｌ为目标点。

为了计算方便且不
影响效果，在计算启发式函数时，认为任意路径点都和目标点直接相连。

定义５所有路径点组成的集合称为可行域，记为ＦＳ。

定义６#ｉ∈ＦＳ，ｊ∈ＦＳ且ｊ与ｉ直接相邻，则称ｊ在ｉ的邻域内，记为ｊ∈Ｎｉ。

其中，Ｎｉ为ｉ的邻域。

定义７粒子ｃ第ｈ维和第ｈ＋１维之间的距离根据式（２）、式（３）计算：
ｐｄｃ
（Ｐｈ，Ｐｈ＋１）＝ｄ
（Ｐ（ｔｈ），Ｐ（ｔｈ＋１））（３）
其中，Ｐ（ｔｈ）和Ｐ（ｔｈ＋１）由式（１）计算。

３．２算法步骤
根据以上思想及相关定义，本文算法具体步骤如下：步骤１初始化：初始化出发点Ｓ和目标点Ｇ，如果Ｓ、Ｇ不
是网络图中的路径点，将Ｓ、Ｇ和离其最近的路径点相连。

将ｍ
只蚂蚁放置在Ｓ，并将Ｓ设置到禁忌表ｔａｂｕｋ中（ｋ＝１，２，…，ｍ）。

令τｉｊ（０）＝τ０（τ０
为常数）。

设置迭代次数ｎ＝０，最大迭代次数为ＭＡＸ，设定#、$和%的值。

步骤２#ｋ，以当前路径点ｉ∈ＦＳ为中心，选择并行走到下一节点ｊ，且有ｊ∈Ｎｉ，ｊ$ｔａｂｕｋ。

蚂蚁按式（４）选择节点ｊ：
ｊ＝
ａｒｇｍａｘ｛［τｉｊ（ｔ）］$"#
ｉｊ｝ｉｆｑ≤ｑ０
Ｓｅｌｓ&
ｅ
（４）
其中：ｑ和ｑ０是为了防止出现停滞而设的随机搜索策略所需参数，以增加搜索的多样性。

０＜ｑ０≤１是初始设定的参数，ｑ∈（０，１）是随机数。

β
为从ｉ到ｊ的弧上残留信息的重要程度；"ｉｊ为由定义４给出的启发信息；随机变量Ｓ根据式（５）决定：
ｐｋ
ｉｊ
（ｔ）＝［τｉｊ（ｔ）］$"#
ｉｊ
ｌ∈ｔａｂｕ’［τ
ｉｌ
（ｔ）］$
［"ｉｌ
］#
ｊ$ｔａｂｕｋ
０
ｊ∈ｔａｂｕｋ
(
*****)*
****+
（５）
ｐｋ
ｉｊ
（ｔ）为在ｔ时刻蚂蚁ｋ由节点ｉ转移到节点ｊ的概率，ｉ，ｊ∈ＦＳ。

计算转移概率ｐｋ
ｉｊ，根据赌轮盘规则选择下一个路径点ｊ，并将ｊ加入禁忌表ｔａｂｕｋ。

步骤３按式（６）信息素更新：
τｉｊ（ｔ＋１）＝（１－%）τｉｊ（ｔ）＋%!τｋ
ｉｊ
!τｋｉｊ
＝ＱＬｋ若蚂蚁ｋ在本次循环中走过（ｉ，ｊ）０
否,
则（６）
１－%表示信息消逝程度；Ｑ为一个常数，表示信息素强度；Ｌｋ表
示蚂蚁ｋ在本次循环中已走的路径长度。

步骤４蚂蚁选择完下一个路径点ｊ后，若ｊ＝Ｇ，则转步骤
５，否则转步骤２。

步骤５计算蚂蚁所走路径的总长度，将新的路径与已知最优路径相比较，若新路径更优，则用新路径替换更新已知最优路径总长度Ｌｍｉｎ。

步骤６在已知最优的路径上按式（７）信息素更新：τｎｅｗ
ｉｊ＝（１－%）τｏｌｄ
ｉｊ＋%!τｉｊ!τｉｊ
＝ＱＬｍｉｎ若蚂蚁ｋ在本次寻优中走过（ｉ，ｊ）０
否&
则（７）
４０
２００７，４３（３１）２０２１ｓ
ａ
ｂ１９１８１７１２
１１
１３
１４１５
１６１０７
９８６４５
３
２
１ｇｃ图３简单环境
２０
２１
２５２４
ｓ
ａ
ｂ１９
２３２２１８
１７
１２
１１
１３
１４１５
１６１０７
９
８
６４
５
３２
１ｇ
ｃ
图４较复杂环境
步骤７迭代次数加１，若未达到最大迭代次数，则清空禁忌表，转步骤２重复上述过程，直到ｎ＝ＭＡＸ为止。

选出最优路径Ｐ＝｛Ｐ０Ｐ１…Ｐｕ｝（其中路径点个数为ｕ＋１）转步骤８。

步骤８初始化粒子数Ｐｎ，在允许的范围内随机初始化粒子的位置和速度，把每个粒子的初始化位置视为其初始最优位置ｐｃ。

粒子的维数由最优路径中路径点的个数（不含出发点和目标点）决定，即粒子有ｕ－１维。

设定!、ｃ１、ｃ２和最大迭代次数
Ｐｍａｘ。

步骤９依据定义７和式（８）计算每个粒子ｃ的适应度Ｆｃ，适应度最小的粒子记为ｇ。

Ｆｃ＝ｕ－１
ｈ＝１
!
ｐｄｃ
（Ｐｈ，Ｐｈ＋１）
（８）
步骤１０依据式（９）更新粒子ｃ的各维速度，依据式（１０）更新粒子ｃ的位置。

Ｖｃ，ｈ＝!Ｖｃ，ｈ＋ｃ１×ｒ１［ｐｃ，ｈ－ｘｃ，ｈ］＋ｃ２×ｒ２［ｇｈ－ｘｃ，ｈ］
（９）ｘｃ，ｈ＝ｘｃ，ｈ＋ｖｃ，ｈ
（１０）
其中ｒ１、ｒ２是［０，１］之间的随机数，若ｖｃｈ＜－ｖｍａｘｘｈ，则令ｖｃｈ＝
ｖｍａｘｘｈ；若ｖｃｈ＞ｖｍａｘｘｈ，则令ｖｃｈ＝ｖｍａｘｘｈ，即对粒子的速度进
行限制。

若ｘｃｈ＜ｍｉｎｘｈ，则令ｘｃｈ＝ｍｉｎｘｈ＋ｒａｎｄ×０．０１；若ｘｃｈ＞ｍａｘｘｈ，则令ｘｃｈ＝ｍａｘｘｈ，即对粒子的位置进行限制。

步骤１１依据式（８）计算每个粒子的适应度，若适应度小于原来的适应度则更新ｐｃ、ｇ。

步骤１２若迭代次数未到达最大次数或者未满足精度要求则转步骤１０，否则转步骤１３。

步骤１３输出最优路径。

４仿真实验
为了体现该算法的有效性及其特点，作者做了大量的实
验，实验环境为：ＣＰＵ赛扬２．６６Ｇ，内存为２５６Ｍ，编译工具
ＶＣ＋＋６．０。

在图３所示的实验中，运行参数设定如下：蚂蚁数为１０，
"＝２、#＝１、$＝０．１、
ＭＡＸ＝５０、Ｐｍａｘ＝１００、Ｐｎ＝３０。

由基本蚂蚁算法求的路径序列为｛ｓ，８，９，１１，１７，１６，１５，４，ｇ｝，由本文算法求得路径为｛ｓ，ａ，ｂ，ｃ，ｇ｝，运行时间为０．３０５ｓ，同一环境下，用文献［３］中的算法运行时间为１．４３２ｓ。

为了进一步说明本文算法的有效性，又对较复杂的环境进行了实验，实验结果如图４所示。

运行参数设定如下：蚂蚁数为
２０，"＝２、#＝１、$＝０．１、ＭＡＸ＝１００、Ｐｍａｘ＝２００、Ｐｎ＝４０。

文献［６］给出
实验结果路径序列为｛ｓ，２４，２３，１８，８，６，３，ｇ｝，由本文算法求得路径为｛ｓ，ａ，ｂ，ｃ，ｇ｝。

仿真实验结果表明，本文算法具有收敛快，得到的路径更优等优点。

５结语
本文采用链接图法对机器人运动环境进行建模，算法简单
容易实现。

在此基础上，给出的算法首先将出发点和目标点与离其最近的路径点相连，用蚂蚁算法快速搜索出一条全局路径，然后对该路径上的点进行调节，得到近似最优路径。

与其他算法相比，该算法简单可行，适于复杂环境下的机器人路径规划。

（收稿日期：２００７年７月）
参考文献：
［１］ＡｇｉｒｒｅｂｅｉｔｉａＪ．ＡｎｅｗＡＰＦｓｔｒａｔｅｇｙｆｏｒｐａｔｈｐｌａｎｎｉｎｇｉｎｅｎｖｉｒｏｎ－
ｍｅｎｔｓｗｉｔｈｏｂｓｔａｃｌｅｓ［Ｊ］．ＭｅｃｈａｎｉｓｍａｎｄＭａｃｈｉｎｅＴｈｅｏｒｙ，２００５，４０：６４５－６５８．
［２］孙树栋，林茂．基于遗传算法的多移动机器人协调路径规划［Ｊ］．自动
化学报，２０００，２６（５）：６７３－６７６．
［３］ＱｉｎＹｕａｎ－ｑｉｎｇ，ＳｕｎＤｅ－ｂａｏ．Ｐａｔｈｐｌａｎｎｉｎｇｆｏｒｍｏｂｉｌｅｒｏｂｏｔｕｓｉｎｇ
ｔｈｅｐａｒｔｉｃｌｅｓｗａｒｍｏｐｔｉｍｉｚａｔｉｏｎｗｉｔｈｍｕｔａｔｉｏｎｏｐｅｒａｔｏｒ［Ｃ］／／Ｐｒｏｃｅｅｄ－ｉｎｇｓｏｆｔｈｅＴｈｉｒｄＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＭａｃｈｉｎｅＬｅａｒｎｉｎｇａｎｄＣｙｂｅｒｎｅｔｉｃｓ，Ｓｈａｎｇｈａｉ，２６－２９Ａｕｇｕｓｔ２００４：２４７３－２４７８．［４］朱庆保．复杂环境下的机器人路径规划蚂蚁算法［Ｊ］．自动化学报，
２００６，３２
（４）：５８６－５９３．［５］孙波，陈卫东，席裕庚．基于粒子群优化算法的移动机器人全局路径
规划［Ｊ］．控制与决策，２００５，２０（９）：１０５２－１０６０．
［６］ＨａｂｉｂＭＫ，ＡｓａｍａＨ．Ｅｆｆｉｃｉｅｎｔｍｅｔｈｏｄｔｏｇｅｎｅｒａｔｅｃｏｌｌｉｓｉｏｎｆｒｅｅ
ｐａｔｈｓｆｏｒａｕｔｏｎｏｍｏｕｓｍｏｂｉｌｅｒｏｂｏｔｂａｓｅｄｏｎｎｅｗｆｒｅｅｓｐａｃｅｓｔｒｕｃｔｕｒｉｎｇａｐｐｒｏａｃｈ［Ｃ］／／ＰｒｏｃＩＥＥＥ／ＲＳＪＩＲＯＳ，１９９１：５６３－５６７．
国海涛，朱庆保，司应涛：一种蚂蚁粒子群融合的机器人路径规划新算法
４１。