动态环境下机器人多目标路径规划蚂蚁算法

动态环境下机器人多目标路径规划蚂蚁算法作者：徐守江
来源：《电脑知识与技术·学术交流》2008年第32期
摘要：研究了一种新颖的动态复杂不确定环境下的机器人多目标路径规划蚂蚁算法。

该方法首先根据蚂蚁觅食行为对多个目标点的组合进行优化，规划出一条最优的全局导航路径。

在此基础上，机器人按照规划好的目标点访问顺序根据多蚂蚁协作局部路径算法完成局部路径的搜索。

机器人每前进一步都实时地进行动态障碍物运动轨迹预测以及碰撞预测，并重新进行避碰局部路径规划。

仿真结果表明，即使在障碍物非常复杂的地理环境，用该算法也能使机器人沿一条全局优化的路径安全避碰的遍历各个目标点，效果十分令人满意。

关键词：机器人；多蚂蚁协作；全局导航路径；局部路径
中图分类号：TP24文献标识码：A文章编号：1009-3044(2008)32-1185-03
Ant Algorithm for Mobile Robot Path Plan with Multi-objects in an Unfamiliar Environment
XU Shou-jiang
(Jiangsu Food Science College, Huai'an 223003, China)
Abstract: Based on Ant Colony Optimization (ACO), this thesis presents a novel algorithm underlying the robot multi-objects path planning and dynamic obstacle avoidance in a complex and unfamiliar environment, and the algorithm optimizes the combination of all objects and can get a globally optimal navigation path. The locally optimal path between the position of robot and the present object which is gotten from the globally optimal navigation path is accomplished by local path planning with multi-ants in cooperation. Collision prediction proceeds timely after ever step of robot and local dynamic planning for obstacle avoidance is executed. Our computer experiments demonstrate that the algorithms are robust and stable.
Key words: robot; multi-ants cooperation; global navigation path; local path
1 引言
移动机器人运动导航或路径规划属于研究机器人控制系统的重要应用基础问题，也是机器人研究领域中的一个重要分支。

它是指在有障碍物的工作环境中，如何寻找一条从给定起始点到终止点的较优的运动路径，使机器人在运动过程中能安全无碰撞地绕过障碍物，且所走路线最短。

对这一问题，各国学者已经做了大量的研究，其中包括Dijkstra算法及其改进[1]、启发
式搜索算法[2]、模糊算法[3]、神经网络[4]、遗传算法[5]，滚动窗口规划方法[6]等等，这些方法都不同程度上提高了机器人路径规划的效率，但其研究的对象大多局限于机器人单目标搜索，对单机器人进行多目标搜索鲜有报道。

随着机器人技术的发展，机器人将被应用于多目标搜索，如机器人邮递员需要将每一份邮件递送到指定的地点等。

即如何保证机器人在有障碍物的工作环境中可以快速地沿一条较优化避碰路径安全无碰地访问多个目标点。

在这类应用中机器人需要遍历多个目标，当目标数增多时，算法的复杂度呈指数级增长[7]，将严重影响规划的时效性。

根据目前的研究现状和不足并受意大利学者M.Dorigo等人于上世纪90年代提出的蚁群优化算法[8-9]的启示。

为此，本文研究了一种动态复杂环境下移动机器人多目标路径规划蚂蚁算法，该算法首先根据Ant Colony System（简称ACS）算法[8]对多个目标点的组合进行优化，规划出一条最优的全局导航路径，然后机器人依赖全局导航路径信息进行各个目标点的遍历，机器人在当前点再根据已知的静态环境信息利用多蚂蚁局部路径规划算法[10]规划出一条局部路径，机器人根据该局部路径每走一步都进行动态环境信息探测，根据该局部路径预测与动态物碰撞点并进行局部动态避障规划。

仿真实验结果表明，即使在障碍物非常复杂的地理环境，用本算法也能使机器人沿一条全局较优化的路径安全避碰的遍历各个目标点，效果十分令人满意。

2 环境描述及问题描述
记AS为机器人Rob在二维平面上的凸多边形有限运动区域，其内部分布着有限个已知的静态障碍物Sb1,Sb2,…,Sbn和有限个未知的动态障碍物Db1,Db2,…,Dbq；在任意时刻，机器人Rob能探测到以当前位置为中心，r为半径区域内的环境信息，称该区域为Rob视野域
View ；Rob的运动速率记为VR；环境探测及路径规划所需的时间忽略不计；各动态障碍物之间的最近距离>2r；t时刻Dbi(i=1,2,…,q)的运动速率记为vid(t), vid(t)∈[0,vdh]，其中，vdh为一有限速率。

Dbi的运动轨迹和方向属未知，但其沿不闭合、不自相交的光滑平缓轨迹单向运动，且在机器人传感器探测范围内近似为方向不变的直线运动。

规划的目的是使机器人由起点Gbegin出发，经过一系列目标点G2,G3,……,Gn，安全避碰地回到出发点Gbegin，从而构成一条回路。

由于随着目标点的增多，解的规模呈指数级增，属于NP-Hard问题，并且各个目标点之间的局部信息未知。

假设机器人的访问目标点的顺序为p1,p2,p3,……,pn,p1，规划的目的就是使得相邻两个目标点之间的局部避碰路径之和最短或者较短，能够保证机器人快速安全地访问各个目标点，其中p1=Gbegin,p2,p3,……,pn为G2,G3,……,Gn的一个全排列。

其中
Gbegin∈A,Gbegin?埸OS ， Gi∈A,Gi?埸OS , i={2,…,n}，其它约束条件为：Gbegin与任意动态障碍物的距离≥r。

在AS中以AS左上角为坐标原点0，以横向为X轴，纵向为Y轴建立系统直角坐标系，如图1。

假设机器人在水平方向上的行走步长为δ,并且AS在X、Y方向的最大值分别为Xmax
和Ymax。

以δ为步长形成一个栅格。

则每行的栅格数Nx=Xmax/δ,每列的栅格数y=Ymax/δ。

其中bi(i=1,2…,n)占一个或多个栅格，当不满一个栅格时算一个栅格。

记g∈AS为任意栅格，A为AS中g的集合，记OS＝{b1,b2,…,bn} A为静态障碍栅格集。

g∈A在坐标系中都有确定的坐标(x,y)，记做g(x,y)，x为g所在的行号，y为g所在的列号。

令C={1,2,3,…，,M}为栅格序号集，g(1,1)的序号为1，g(1,2)序号为2,g(2,1)序号为
Nx+1…，如图1所示。

gi∈A的坐标（xi,yi）与序号i∈C构成互为映射关系，序号i的坐标可由（1）式确定：
xi=((i-1) mod Nx)+1,
yi=(int)((i-1)/Nx)+1 （1）
式中，int为取取整运算，mod为求余运算。

Rob在AS中的位置记为PR，?坌 PR在∑0都有确定的坐标(x,y)，t时刻的位置表示为
PR(t)，其坐标为(xR(t),yR(t))。

对于任意二维地形，规划的目的是使机器人由任意起点Gbegin,安全地沿一条较短路径到达任意终点Gend。

且Gbegin、Gend OS,其它约束条件为：begin ,end∈C, begin≠end。

3算法的总体思路及算法描述
算法首先根据蚂蚁觅食行为对多个目标点的组合进行优化，规划出一条最优的全局导航路径。

在此基础上，机器人按照规划好的目标点访问顺序根据多蚂蚁协作局部路径算法完成局部路径的规划。

规划过程分两步完成，第一步，暂时不考虑动态物，以Rob当前位置PR(t)为蚁穴，Gi为食物源或反之，蚂蚁从PR(t)或Gi出发，进行寻找食物的过程，经过蚂蚁群体的反复寻食，由于蚂蚁留下的信息素的正反馈作用，最终绕开所有障碍物找到一条最短局部路径。

根据该静态局部优化路径和动态物的轨迹进行碰撞预测，当碰撞点无法回避时，则将其列为禁入域，由蚂蚁重新快速规划出一条动态避障局部优化路径。

Rob每行进一步，都进行上述局部避碰规划过程，因此，其规划出的局部路径是实时动态修改的。

为了叙述方便，进一步作出如下约定和定义：
定义1：View(PR(t))={P|P∈A,d(P,PR(t))≤r}称为Rob在位置PR(t)处的视野域。

机器人路径滚动规划的蚂蚁算法步骤描述如下：
Step 1：利用ACS算法根据蚂蚁觅食行为对多个目标点的组合进行优化，规划出一条最优的全局导航路径{G'1,G'2,…,G'i,…Gin, G'n+1},其中G'1=G'n+1=Gbegin，G'2,…,G'i,…Gin为
G2,…,Gi,…Gn的一个全排列；并设N=1；
Step 2：若N>n+1，规划结束，否则转Step 3；
Step 3：Rob根据环境已知的Sbi, i=(1,2,…h), 用多蚂蚁协作局部路径规划算法迅速规划出不考虑Dbi时从G'N到G'N+1的局部静态优化路径；
Step 4：Rob在当前位置PR(t)进行环境探测，识别出动态障碍物Dbi。

其中PR(t)～gR。

Step 5: 若无Dbi，转Step7，否则，则测出其方向、速度、运动轨迹并预测出Rob沿上一次规划出的局部静态优化路径向gsub前进时与Dbi的碰撞点，碰撞点的集合记作CO；
Step 6：根据CO情况和避碰策略进行避碰处理。

当碰撞点不能回避时，将CO作为静态障碍物，再用多蚂蚁协作算法重新规划当前位置PR(t)到G'N+1的一条避碰局部优化路径。

Step 7：Rob沿着规划出的局部最优路径向G'N+1前进一步，若PR(t)不等于G'N+1，转到Step 4；
Step 8：N=N+1，返回Step 2。

4 仿真实验
为了验证算法的效果，作者进行了大量的仿真实验，结果都十分令人满意。

假设环境地图大小为30*30,静态障碍物信息已知。

为了实验算法的效果，设计了图2所示的复杂地形。

本文算法首先根据ACS算法可以迅速规划出一条全局导航路径引导机器人按照该顺序遍历各个目标点，如图2所示。

图中红色圆心节点为机器人起始位置Gbegin，方形红色填充区域为机器人需要访问的目标点，黑色栅格为障碍物。

Rob到达某个目标点位置时（开始时为Gbegin）再根据多蚂蚁协作局部路径规划算法规划出到达下一个目标点的局部避障路径，Rob每前进一步，都进行碰撞预测和动态避碰局部规划，如图3所示，图中粗线为机器人根据探测到的动态障碍物再次规划出的局部路径。

从实验结果可以看出，只要各个目标点之间存在路径，总可以规划出有效路径，且Rob从Gbegin逐一访问各个目标点后返回到Gbegin的路径为一条优化路径。

5 小结
算法首先快速规划出各个目标点的访问次序，给出了Rob的一种引导趋势。

Rob行进过程中采用两组蚂蚁协作完成基于静态全局信息的局部路径优化规划，其中，一组蚂蚁置于机器人当前位置，另一组蚂蚁置于目标点，两组蚂蚁相向并行搜索，由两组蚂蚁工程完成最优路径的搜索，加上设有随机搜索策略，保证了搜索的多样性，使搜索不易限于停滞。

只要有可行通道客观存在，即使在非常复杂的地形环境，也能迅速找到一条优化路径。

由于采用的启发函数具有趋近导向作用和蚂蚁信息素的正反馈，加上两组蚂蚁的并行性，因而局部路径规划效率较高。

在此基础上，实时地进行动态障碍物运动轨迹预测以及碰撞预测，在机器人视野域范围，将该动态物轨迹上预测碰撞点及附近的栅格视为静态障碍物，从而可以再次实时地作出动态避障局部规划，使得不仅安全避碰，而且使机器人行走路径基本达最优。

仿真结果表明，该算法具有简单、快速、高效、安全等特点，可以有效解决多个目标点的机器人路径规划问题。

参考文献：
[1] Hwang J Y, Kim J S. A fast path planning by path graph optimization Systems[J].Man and Cybernetics,Part A,IEEE Transactions,2003,33(1):121-129.
[2] Ikeda T, Hsu M Y, Imai M. A Fast Algorithm for Finding Better Routes by AI Search Techniques[C].IEEE Vehicle Navigation & Information Systems Conference Proceedings,1994:291-296.
[3] Kim B N, Kwon O S. A study on path planning for mobile robot based on fuzzy logic controller[C].Proceedings of the IEEE Region 10 Conference,1999,2(15-17):1002-1005.
[4] Araújo F, Ribeiro B, Rodrigues L.A Neural Network for Shortest Path Computation[J].IEEE Transactions on Neural Networks, Sep. 2001,12(5):1067-1073.
[5] Gen M, Cheng R W, Wang D W. Genetic Algorithms for Solving Shortest Path
Problems[J].IEEE,1997:401-406.
[6] 张纯刚,席裕庚.全局环境未知时基于滚动窗口的机器人路径规划[J].中国科学（E
辑）,2001,31(1):51-58.
[7] Lawler E L, Lenstra J K, Rinnooy A H G, et al. The Traveling Salesman Problem[M]. New York:John Wiley & Sons,1985.
[8] Colorni A, Dorigo M, Maniezzo V, Distributed optimization by ant colonies[C].
Paris:Proceeding of ECAL91 European Conference of Artificial Life,1991:134-144.
[9] Dorigo M. Di Caro G. Ant colony system: a cooperative learning approach to the traveling salesman problem[C].IEEE Transactions on Evolutionary Computation, 1997,1(1):53-66.
[10] 朱庆保.全局未知环境下多机器人运动蚂蚁导航算法[J].软件学报,2006,17(9):1890-1898.。