(完整word版)基于蚁算法的路径规划

MATLAB实现基于蚁算法的机器人路径规划

1、问题描述

移动机器人路径规划是机器人学的一个重要研究领域。它要求机器人依据某个或某些优化原则（如最

小能量消耗，最短行走路线，最短行走时间等），在其工作空间中到一条从起

始状态到目标状态的能避开障碍物的最优路径。机器人路径规划问题可以建模为一个有约束的优化问题，都

要完成路径规划、定位和避障等任务。

2算法理论

蚁算法（AntColonyAlgorithm，ACA），最初是由意大利学者DorigoM.博士于1991年首次提

出，其本质是一个复杂的智能系统，且具有较强的鲁棒性，优良的分布式计算机制等优点。该算法经过十

多年的发展，已被广大的科学研究人员应用于各种问题的研究，如旅行商问题，二次规划问题，生产调度

问题等。但是算法本身性能的评价等算法理论研究方面进展较慢。

Dorigo提出了精英蚁模型（EAS），在这一模型中信息素更新按照得到当前最优解的蚂蚁所构造的

解来进行，但这样的策略往往使进化变得缓慢，并不能取得较好的效果。次年Dorigo博士给出改进模型

（ACS），文中改进了转移概率模型，并且应用了全局搜索与局部搜索策略，来得进行深度搜索。

Stützle与Hoos给出了最大-最小蚂蚁系统（MAX-MIAS），所谓最大-最小即是为信息素设定上限与

下限，设定上限避免搜索陷入局部最优，设定下限鼓励深度搜索。蚂蚁作为一个生物个体其自身的能力是十

分有限的，比如蚂蚁个体是没有视觉的，蚂蚁自身体积又是那么渺小，但是由这些能力有限的蚂蚁组成的

蚁却可以做出超越个体蚂蚁能力的超常行为。蚂蚁没有视觉却可以寻觅食物，蚂蚁体积渺小而蚁却可

以搬运比它们个体大十倍甚至百倍的昆虫。这些都说明蚂蚁体内部的某种机制使得它们具有了体智能，

可以做到蚂蚁个体无法实现的事情。经过生物学家的长时间观察发现，蚂蚁是通过分泌于空间中的信息素进

行信息交流，进而实现体行为的。

下面简要介绍蚁通过信息素的交流到最短路径的简化实例。如图2-1所示，AE之间有两条路

ABCDE与ABHDE，其中AB，DE，HD，HB的长度为1，BC，CD长度为0.5，并且，假设路上信

息素浓度为0，且各个蚂蚁行进速度相同，单位时间所走的长度为1，每个单位时间内在走过路径上留下的

信息素的量也相同。当t=0时，从A点，E点同时各有30只蚂蚁从该点出发。当t=1，从A点出发的

蚂蚁走到B点时，由于两条路BH与BC上的信息素浓度相同，所以蚂蚁以相同的概率选择BH与BC，

这样就有15只蚂蚁选择走BH，有15只蚂蚁选择走BC。同样的从E点出发的蚂蚁走到D点，分别有

15只蚂蚁选择DH和DC。当t=2时，选择BC与DC的蚂蚁分别走过了BCD和DCB，而选择BH

与DH的蚂蚁都走到了H点。所有的蚂蚁都在所走过的路上留下了相同浓度的信息素，那么路径BCD上的

信息素的浓度是路径BHD上信息素浓度的两倍，这样若再次有蚂蚁选择走BC和BH时，或选择走DC与

DH时，都会以较大的概率选择信息素浓度高的一边。这样的过程反复进行下去，最短的路径上走过的蚂蚁

较多，留下的信息素也越多，蚁这样就可以到一条较短的路。这就是它们体智能的体现。

蚁算法就是模拟蚂蚁觅食过程中可以到最短的路的行为过程设计的一种仿生算法。在用蚁算法求

解组合优化问题时，首先要将组合优化问题表达成与信息素相关的规范形式，然后各个蚂蚁独立地根据局部的

信息素进行决策构造解，并根据解的优劣更新周围的信息素，这样的过程反复的进行即可求出组合优化问题

的优化解。

归结蚁算法有如下特点：

（1）分布式计算：各个蚂蚁独立地构造解，当有蚂蚁个体构造的解较差时，并不会影响整体的求解结

果。这使得算法具有较强的适应性；

（2）自组织性：系统学中自组织性就是系统的组织指令是来自系统的内部。同样的蚁算法中的各个蚂

蚁的决策是根据系统内部信息素的分布进行的。这使得算法具有较强的鲁棒性；

（3）正反馈机制与负反馈机制结合：若某部分空间上分布的信息素越多，那么在这个空间上走过的蚂蚁

也就越多；走过的蚂蚁越多，在那个空间上留下的信息素也就越多，这就是存在的正反馈机制。但蚁算

法中解的构造是通过计算转移概率实现的，也就是说构造解的时候可以接受退化解，这限制了正反馈机制，

可以使得搜索范围扩大，这是蚁算法中隐含的负反馈机制。

3求解步骤

应用蚁算法求解机器人路径优化问题的主要步骤如下：

（1）输入由0和1组成的矩阵表示机器人需要寻最优路径的地图的地图，其中

示此处可以通过的，1表示此处为障碍物。

0表

图的表示矩阵为：

00000000;000

00000;00000000;000000111

;00000000;011

10000;000000

00;11000000;

11000000;11000000;000000

00;

00000000;

11011110;

11011110;

11011110;

00000000;

00000110;

00100110;

00100000;

00000000;

(2)输入初始的信息素矩阵，选择初始点和终止点并且设置各种参数。在此次计算中，我们设置所有位

置的初始信息素相等。

(3)选择从初始点下一步可以到达的节点，根据每个节点的信息素求出前往每个节点的概率，并利用轮

盘算法选取下一步的初始点。

p

k

ij

t

ijtijktabu

0

ijij

k

ifjtabuk

otherwise

其中ijt为析取图中弧i,j上的信息素的浓度。ij为与弧i,j相关联的启发式信息。，分别为ijt，ij的权

重参数。

（4）更新路径，以及路程长度。

（5）重复（3）（4）过程，直到蚂蚁到达终点或者无路可走。

（6）重复（3）（4）（5），直到某一代m只蚂蚁迭代结束。

（7）更新信息素矩阵，其中没有到达的蚂蚁不计算在内。

ijt11ijtij

ijtQ

Lkt，如果蚂蚁k经过i，j

0，如果蚂蚁k不经过i，j

其中为信息素挥发系数。Q为信息量增加强度。Lkt为路径长度。

（8）重复（3）-（7），直至n代蚂蚁迭代结束。

4运行结果（图、表等）将上述矩阵输入到程序中，画出最短路径的路线，并且输入每一轮迭代的最短路

径，

查看程序的收敛效果，在程序中设置plotif=1则输出收敛和最短路径图，在程序中设置plotif2=1则输出每一

代蚂蚁的路径图。

最终输出的结果如图

20

18

16

14

12

10

8

6

4

2

24681

收敛曲线（最小路径长度）

35

30

25

20

15

度

长

径

路

5

0

5MATLAB程序functionm_main()

G=[

108090100迭代次数

00000000;

0

00000000;

00000000;

00000000;

00000000;

00000000;

00000000;

11000000;

11000000;

11000000;

11000000;

00000000;

11011110;

11011110;

11011110;

00000000;

00000110;

00100110;

00100000;

00000000;];

MM=size(G,1);%G地形图为01矩阵，如果为1表示障碍物

Tau=ones(MM*MM,MM*MM);%Tau初始信息素矩阵(认为前面的觅食活动中有残留的信息素)

Tau=8.*Tau;

K=100;%K迭代次数(指蚂蚁出动多少波)

M=50;%M蚂蚁个数(每一波蚂蚁有多少个)

S=1;%S起始点(最短路径的起始点)

E=MM*MM;%E终止点(最短路径的目的点)

Alpha=1;

Beta=7;

%Alpha表征信息素重要程度的参数

%Beta表征启发式因子重要程度的参数

Rho=0.3;%Rho信息素蒸发系数

Q=1;

minkl=inf;

mink=0;

minl=0;

D=G2D(G);

=size(D,1);%表示问题的规模(象素个数)a=1;%小方格象素的边长

Ex=a*(mod(E,MM)-0.5);%终止点横坐标

ifEx==-0.5

Ex=MM-0.5;end

Ey=a*(MM+0.5-ceil(E/MM));%终止点纵坐标

Eta=zeros();%启发式信息，取为至目标点的直线距离的倒数%下面构造启发式信息矩阵

fori=1:ix=a*(mod(i,MM)-0.5);

ifix==-0.5

%Q信息素增加强度系数

ix=MM-0.5;

endiy=a*(MM+0.5-ceil(i/MM));

ifi~=E

Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;

else

Eta(i)=100;

end

end

ROUTES=cell(K,M);%用细胞结构存储每一代的每一只蚂蚁的爬行路线PL=zeros(K,M);%用矩阵

存储每一代的每一只蚂蚁的爬行路线长度%%----------------------------------------------启动K轮蚂

蚁觅食活动，每轮派出M只蚂蚁-------------------------------------------

fork=1:Kform=1:M

%%第一步：状态初始化W=S;%当前节点初始化为起始点Path=S;%爬行路线初始化

PLkm=0;%爬行路线长度初始化

TABUkm=ones();%禁忌表初始化

TABUkm(S)=0;%已经在初始点了，因此要排除

DD=D;%邻接矩阵初始化

%%第二步：下一步可以前往的节点DW=DD(W,:);

DW1=find(DW);

forj=1:length(DW1)

ifTABUkm(DW1(j))==0DW(DW1(j))=0;

end

end

LJD=find(DW);

Len_LJD=length(LJD);%可选节点的个数

%%觅食停止条件：蚂蚁未遇到食物或者陷入死胡同

whileW~=E&&Len_LJD>=1

%%第三步：转轮赌法选择下一步怎么走PP=zeros(Len_LJD);

fori=1:Len_LJDPP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta);

end

sumpp=sum(PP);

PP=PP/sumpp;%建立概率分布

Pcum(1)=PP(1);

fori=2:Len_LJDPcum(i)=Pcum(i-1)+PP(i);end

Select=find(Pcum>=rand);to_visit=LJD(Select(1));%%第四步：状态更新和记录

Path=[Path,to_visit];%路径增加

PLkm=PLkm+DD(W,to_visit);%路径长度增加W=to_visit;%蚂蚁移到下一个节点

forkk=1:ifTABUkm(kk)==0DD(W,kk)=0;DD(kk,W)=0;endend

TABUkm(W)=0;%已访问过的节点从禁忌表中删除DW=DD(W,:);

DW1=find(DW);

forj=1:length(DW1)

ifTABUkm(DW1(j))==0

DW(j)=0;

end

endLJD=find(DW);Len_LJD=length(LJD);%可选节点的个数end

%%第五步：记下每一代每一只蚂蚁的觅食路线和路线长度ROUTES{k,m}=Path;

ifPath(end)==EPL(k,m)=PLkm;ifPLkm

elsePL(k,m)=0;

end

end

%%第六步：更新信息素Delta_Tau=zeros(,);%更新量初始化form=1:M

ifPL(k,m)ROUT=ROUTES{k,m};

TS=length(ROUT)-1;%跳数

PL_km=PL(k,m);

fors=1:TSx=ROUT(s);y=ROUT(s+1);Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km;

Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;

end

end

end

Tau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分

end

%%-------------------------绘图------------------------

plotif=1;%是否绘图的控制参数

ifplotif==1%绘收敛曲线

minPL=zeros(K);

fori=1:K

PLK=PL(i,:);

onzero=find(PLK);

PLKPLK=PLK(onzero);

minPL(i)=min(PLKPLK);

end

figure(1)

plot(minPL);

holdon

gridon

title('收敛曲线(最小路径长度)');

xlabel('迭代次数');

ylabel('路径长度');%绘爬行图

figure(2)axis([0,MM,0,MM])fori=1:MMforj=1:MMifG(i,j)==1x1=j-1;y1=MM-i;

x2=j;y2=MM-i;

x3=j;y3=MM-i+1;

x4=j-1;y4=MM-i+1;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);

holdon

else

x1=j-1;y1=MM-i;

x2=j;y2=MM-i;

x3=j;y3=MM-i+1;

x4=j-1;y4=MM-i+1;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);

holdonendendendholdon

ROUT=ROUTES{mink,minl};

LEROUT=length(ROUT);

Rx=ROUT;

Ry=ROUT;

forii=1:LEROUT

Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);ifRx(ii)==-0.5

Rx(ii)=MM-0.5;endRy(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));end

plot(Rx,Ry)end

plotif2=0;%绘各代蚂蚁爬行图

ifplotif2==1figure(3)axis([0,MM,0,MM])fori=1:MMforj=1:MMifG(i,j)==1x1=j-1;y1=MM-i;

x2=j;y2=MM-i;x3=j;y3=MM-i+1;x4=j-1;y4=MM-i+1;fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);

holdonelse

x1=j-1;y1=MM-i;x2=j;y2=MM-i;

x3=j;y3=MM-i+1;x4=j-1;y4=MM-i+1;fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);holdon

endendendfork=1:K

PLK=PL(k,:);minPLK=min(PLK);

pos=find(PLK==minPLK);

m=pos(1);

ROUT=ROUTES{k,m};

LEROUT=length(ROUT);

Rx=ROUT;

Ry=ROUT;

forii=1:LEROUT

Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);

ifRx(ii)==-0.5

Rx(ii)=MM-0.5;

end

Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));

end

plot(Rx,Ry)

holdon

end

end

functionD=G2D(G)

l=size(G,1);

D=zeros(l*l,l*l);

fori=1:l

forj=1:l

ifG(i,j)==0

form=1:l

forn=1:l

ifG(m,n)==0

im=abs(i-m);jn=abs(j-n);

ifim+jn==1||(im==1&&jn==1)

D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5;endendend

end

end

end

end