MATLAB实现基于“蚁群算法”的AMR路径规划

1 问题描述

2 算法理论

3 求解步骤

4 运行结果

5 代码部分

1 问题描述

移动机器人路径规划是机器人学的一个重要研究领域。它要求机器人依据某个或某些优化原则 (如最小能量消耗，最短行走路线，最短行走时间等)，在其工作空间中找到一条从起始状态到目标状态的能避开障碍物的最优路径。机器人路径规划问题可以建模为一个有约束的优化问题，都要完成路径规划、定位和避障等任务。

2 算法理论

蚁群算法（Ant Colony Algorithm，ACA），最初是由意大利学者Dorigo M. 博士于1991 年首次提出，其本质是一个复杂的智能系统，且具有较强的鲁棒性，优良的分布式计算机制等优点。该算法经过十多年的发展，已被广大的科学研究人员应用于各种问题的研究，如旅行商问题，二次规划问题，生产调度问题等。但是算法本身性能的评价等算法理论研究方面进展较慢。

Dorigo 提出了精英蚁群模型（EAS），在这一模型中信息素更新按照得到当前最优解的蚂蚁所构造的解来进行，但这样的策略往往使进化变得缓慢，并不能取得较好的效果。

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

Stützle 与Hoos给出了最大-最小蚂蚁系统（MAX-MINAS），所谓最大-最小即是为信息素设定上限与下限，设定上限避免搜索陷入局部最优，设定下限鼓励深度搜索。蚂蚁作为一个生物个体其自身的能力是十分有限的，比如蚂蚁个体是没有视觉的，蚂蚁自身体积又是那么渺小，但是由这些能力有限的蚂蚁组成的蚁群却可以做出超越个体蚂蚁能力的超常行为。蚂蚁没有视觉却可以寻觅食物，蚂蚁体积渺小而蚁群却可以搬运比它们个体大十倍甚至百倍的昆虫。这些都说明蚂蚁群体内部的某种机制使得它们具有了群体智能，可以做到蚂蚁个体无法实现的事情。经过生物学家的长时间观察发现，蚂蚁是通过分泌于空间中的信息素进行信息交流，进而实现群体行为的。

下面简要介绍蚁群通过信息素的交流找到最短路径的简化实例，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组成的矩阵表示机器人需要寻找最优路径的地图的地图，其中0表示此处可以通过的，1表示此处为障碍物。

上图的表示矩阵为：

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;

0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;

0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;

0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;

0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0;

0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;

0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;

0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0;

0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;

0 0 1 1 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;

0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 1 1 1 0 1 1 0 0 0 0 0 1 1 0;

0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 1 1 0;

0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;

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

我们设置所有位置的初始信息素相等。

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

概率，并利用轮盘算法选取下一步的初始点。

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

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

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

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

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

4 运行结果

将上述矩阵输入到程序中，画出最短路径的路线，并且输入每一轮迭代的最短路径，查看程序的收敛效果，在程序中设置plotif=1则输出收敛和最短路径图，在程序中设置plotif2=1则输出每一代蚂蚁的路径图。

最终输出的结果如图

路径规划图

对应收敛曲线图

5 代码部分

function m_main() 
G=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0; 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0; 0 0 1 1 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0; 0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 1 0 1 1 0 0 0 0 0 1 1 0; 0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 1 1 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; 
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;    % Alpha 表征信息素重要程度的参数Beta=7;     % Beta 表征启发式因子重要程度的参数Rho=0.3 ;     % Rho 信息素蒸发系数Q=1;          % Q 信息素增加强度系数 
minkl=inf; 
mink=0; 
minl=0; D=G2D(G); 
N=size(D,1);%N表示问题的规模（象素个数）a=1;%小方格象素的边长Ex=a*(mod(E,MM)-0.5);%终止点横坐标if Ex==-0.5 
Ex=MM-0.5; 
end 
Ey=a*(MM+0.5-ceil(E/MM));%终止点纵坐标Eta=zeros(N);%启发式信息，取为至目标点的直线距离的倒数%下面构造启发式信息矩阵for i=1:N ix=a*(mod(i,MM)-0.5); if ix==-0.5 ix=MM-0.5; end 
iy=a*(MM+0.5-ceil(i/MM));  if i~=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只蚂蚁-------------------- 
for k=1:K 
for m=1:M 
%% 第一步：状态初始化W=S;%当前节点初始化为起始点Path=S;%爬行路线初始化
PLkm=0;%爬行路线长度初始化TABUkm=ones(N);%禁忌表初始化TABUkm(S)=0;%已经在初始点了，因此要排除DD=D;%邻接矩阵初始化%% 第二步：下一步可以前往的节点DW=DD(W,:); 
DW1=find(DW); 
for j=1:length(DW1) if TABUkm(DW1(j))==0 DW(DW1(j))=0; end 
end 
LJD=find(DW); 
Len_LJD=length(LJD);%可选节点的个数%% 觅食停止条件：蚂蚁未遇到食物或者陷入死胡同while W~=E&&Len_LJD>=1 
%% 第三步：转轮赌法选择下一步怎么走PP=zeros(Len_LJD); for i=1:Len_LJD PP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta); end 
sumpp=sum(PP); 
PP=PP/sumpp;%建立概率分布
Pcum(1)=PP(1); for i=2:Len_LJD Pcum(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;%蚂蚁移到下一个节点for kk=1:N if TABUkm(kk)==0 DD(W,kk)=0; DD(kk,W)=0; end end 
TABUkm(W)=0;%已访问过的节点从禁忌表中删除DW=DD(W,:); 
DW1=find(DW); 
for j=1:length(DW1) if TABUkm(DW1(j))==0 DW(j)=0; end end 
LJD=find(DW); 
Len_LJD=length(LJD);%可选节点的个数end 
%% 第五步：记下每一代每一只蚂蚁的觅食路线和路线长度ROUTES{k,m}=Path; if Path(end)==E PL(k,m)=PLkm; if PLkm<minkl mink=k;minl=m;minkl=PLkm; end else PL(k,m)=0; end 
end 
%% 第六步：更新信息素
Delta_Tau=zeros(N,N);%更新量初始化for m=1:M if PL(k,m)  ROUT=ROUTES{k,m}; TS=length(ROUT)-1;%跳数PL_km=PL(k,m); for s=1:TS x=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;%是否绘图的控制参数if plotif==1 %绘收敛曲线minPL=zeros(K); for i=1:K PLK=PL(i,:); Nonzero=find(PLK); PLKPLK=PLK(Nonzero); minPL(i)=min(PLKPLK); end 
figure(1) 
plot(minPL); 
hold on 
grid on 
title('收敛曲线（最小路径长度）'); 
xlabel('迭代次数'); 
ylabel('路径长度'); %绘爬行图
figure(2) 
axis([0,MM,0,MM]) 
for i=1:MM 
for j=1:MM 
if G(i,j)==1 
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],[0.2,0.2,0.2]); 
hold on 
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]); 
hold on 
end 
end 
end 
hold on 
ROUT=ROUTES{mink,minl}; 
LENROUT=length(ROUT); 
Rx=ROUT; 
Ry=ROUT; 
for ii=1:LENROUT 
Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); 
if Rx(ii)==-0.5 
Rx(ii)=MM-0.5; 
end 
Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); 
end 
plot(Rx,Ry) 
end 
plotif2=0;%绘各代蚂蚁爬行图
if plotif2==1 
figure(3) 
axis([0,MM,0,MM]) 
for i=1:MM 
for j=1:MM 
if G(i,j)==1 
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],[0.2,0.2,0.2]); 
hold on 
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]); 
hold on 
end 
end 
end 
for k=1:K 
PLK=PL(k,:); 
minPLK=min(PLK); 
pos=find(PLK==minPLK); 
m=pos(1); 
ROUT=ROUTES{k,m}; 
LENROUT=length(ROUT); 
Rx=ROUT; 
Ry=ROUT; 
for ii=1:LENROUT 
Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); 
if Rx(ii)==-0.5 
Rx(ii)=MM-0.5; 
end 
Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); 
end 
plot(Rx,Ry) 
hold on 
end 
end 
function D=G2D(G) 
l=size(G,1); 
D=zeros(l*l,l*l); 
for i=1:l for j=1:l if G(i,j)==0 for m=1:l for n=1:l if G(m,n)==0 im=abs(i-m);jn=abs(j-n); if im+jn==1||(im==1&&jn==1) D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5; end end end end end end 
end