matlab_蚁群算法_机器人路径优化问题
用AＣＯ算法求解机器人路径优化问题
4.1问题描述
移动机器人路径规划是机器人学的一个重要研究领域。它要求机器人依据某个或某些优化原则(如最小能量消耗,最短行走路线，最短行走时间等),在其工作空间中找到一条从起始状态到目标状态的能避开障碍物的最优路径。机器人路径规划问题可以建模为一个有约束的优化问题，都要完成路径规划、定位和避障等任务。
4.２算法理论
蚁群算法（Aｎt ＣｏｌonｙＡｌｇoｒithm，ACA)，最初是由意大利学者Ｄｏｒigo M. 博士于１991 年首次提出，其本质是一个复杂的智能系统,且具有较强的鲁棒性,优良的分布式计算机制等优点。该算法经过十多年的发展,已被广大的科学研究人员应用于各种问题的研究，如旅行商问题,二次规划问题,生产调度问题等。但是算法本身性能的评价等算法理论研究方面进展较慢。
Ｄorigｏ提出了精英蚁群模型（ＥAS）,在这一模型中信息素更新按照得到当前最优解的蚂蚁所构造的解来进行，但这样的策略往往使进化变得缓慢,并不能取得较好的效果。次年Dorigｏ博士在文献［3０]中给出改进模型(ACＳ),文中
改进了转移概率模型，并且应用了全局搜索与局部搜索策略,来得进行深度搜索。
Stüｔzｌe 与Hoｏｓ给出了最大－最小蚂蚁系统（MAX-MINAＳ),所谓最大－最小即是为信息素设定上限与下限，设定上限避免搜索陷入局部最优,设定下限鼓励深度搜索。
蚂蚁作为一个生物个体其自身的能力是十分有限的，比如蚂蚁个体是没有视觉的,蚂蚁自身体积又是那么渺小，但是由这些能力有限的蚂蚁组成的蚁群却可以做出超越个体蚂蚁能力的超常行为。蚂蚁没有视觉却可以寻觅食物,蚂蚁体积渺小而蚁群却可以搬运比它们个体大十倍甚至百倍的昆虫。这些都说明蚂蚁群体内部的某种机制使得它们具有了群体智能,可以做到蚂蚁个体无法实现的事情。经过生物学家的长时间观察发现，蚂蚁是通过分泌于空间中的信息素进行信息交流，进而实现群体行为的。
下面简要介绍蚁群通过信息素的交流找到最短路径的简化实例。如图 2-1 所示,AE 之间有
两条路AＢCDＥ与ABHDE，其中AB,DE,ＨＤ，HB 的长度为1,BC，CD 长度为０.5,并且，假设路上信息素浓度为０,且各个蚂蚁行进速度相同,单位时间所走的长度为１,每个单位时间内在走过路径上留下的信息素的量也相同。当t=0时，从Ａ点，E 点同时各有30 只蚂蚁从该点出发。当t=1,从A 点出发的蚂蚁走到B 点时,由于两条路ＢＨ与BC 上的信息素浓度相同，所以蚂蚁以相同的概率选择ＢH 与ＢＣ,这样就有15 只蚂蚁选择走ＢH,有15 只蚂蚁选择走ＢC。同样的从E 点出发的蚂蚁走到Ｄ点，分别有1５只蚂蚁选择DH 和DＣ。当t＝2 时,选择BC 与DC的蚂蚁分别走过了ＢＣＤ和DCB，而选择BH 与DH 的蚂蚁都走到了H 点。所有的蚂蚁都在所走过的路上留下了相同浓度的信息素,那么路径BCD 上的信息素的浓度是路径BHD 上信息素浓度的两倍,这样若再次有蚂蚁选择走BC 和BH 时，或选择走DC 与DＨ时,都会以较大的概率选择信息素浓度高的一边。这样的过程反复进行下去，最短的路径上走过的蚂蚁较多，留下的信息素也越多,蚁群这样就可以找到一条较短的路。这就是它们群体智能的体现。
蚁群算法就是模拟蚂蚁觅食过程中可以找到最短的路的行为过程设计的一种仿生算法。在用蚁群算法求解组合优化问题时，首先要将组合优化问题表达成与信息素相关的规范形式,然后各个蚂蚁独立地根据局部的信息素进行决策构造解,并根据解的优劣更新周围的信息素，这样的过程反复的进行即可求出组合优化问题的优化解。
归结蚁群算法有如下特点:
(1）分布式计算：各个蚂蚁独立地构造解，当有蚂蚁个体构造的解较差时,并不会影响整体的求解结果。这使得算法具有较强的适应性;
（２）自组织性:系统学中自组织性就是系统的组织指令是来自系统的内部。同样的蚁群算法中的各个蚂蚁的决策是根据系统内部信息素的分布进行的。这使得算法具有较强的鲁棒性;
(3）正反馈机制与负反馈机制结合:若某部分空间上分布的信息素越多，那么在这个空间上走过的蚂蚁也就越多；走过的蚂蚁越多，在那个空间上留下的信息素也就越多,这就是存在的正反馈机制。但蚁群算法中解的构造是通过计算转移概率实现的,也就是说构造解的时候可以接受退化解，这限制了正反馈机制，可以使得搜索范围扩大,这是蚁群算法中隐含的负反馈机制。
4.３求解步骤
应用蚁群算法求解机器人路径优化问题的主要步骤如下:
（１)输入由0和1组成的矩阵表示机器人需要寻找最优路径的地图的地图,其中0表示此处可以通过的,１表示此处为障碍物。
上图的表示矩阵为：０ 0 00 0 0 0 ０0 0 ００0 0 ０ 000 00;
０１１0 0 0 0 00 0 0 00 ０ 0 00 ０ 0 0;
01 1 0 0 0 １ 1 10 ０ 0 0 ０0 ０ 0 0 00;
０ 0 0 00 ０１１ 1 0 0 0 ０ 0 0 0 ０ 000;
00 ００00 １ 110 00 0 ０00 0 0 0 ０;
0 1 1 １００ 1 1 1 0 0 ０0 0 000０ 0 0；
0 １ 1 １ 0 0１ 1 １０ 0 0 0 ００００00 0;
０ 1 １１ 0 0 1 1 1０ 1 １１ 1 0０ 0 ０0 0;
0 １１１ 0０0 0 0 ０ 1 1 1 １ 0０00 0 0;
0 ０ 0 ０ 0 0 0 0 0 ０ 1 １ 1 １ 0 0 0 0 0 0;
０ ０ 0 0 ０ ０ ０ 1 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 ０ 1 1 1 0 1 1 1 1 0; ０ 0 0 0 ０ 0 0 0 0 0 ０ １ １ １ 0 1 1 1 1 0;
０ 0 １ 1 0 ０ 0 0 0 0 0 1 1 １ 0 １ 1 1 1 0； 0 0 １ 1 0 0 1 1 １ 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 0； 0 0 0 0 ０ 0 0 0 0 ０ １ 1 0 0 1 ０ 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 0 0 0 0 0 0；
（2）输入初始的信息素矩阵,选择初始点和终止点并且设置各种参数。在此次计算中，我们设置所有位置的初始信息素相等。
（3)选择从初始点下一步可以到达的节点，根据每个节点的信息素求出前往每个节点的概率,并利用轮盘算法选取下一步的初始点。
{}[()][],if {}[()][]()0 otherwise k ij ij k k ij ij ij k N tabu t j N tabu t p t αβαβτητη∈-??∈-???=????∑
其中τij (t )为析取图中弧(i , ｊ)上的信息素的浓度。ηij 为与弧(i , j )相关联的启发式信息。α ,β 分别为τij （t ） , ηij 的权重参数。
(４)更新路径,以及路程长度。
(5) 重复（3)（4)过程,直到蚂蚁到达终点或者无路可走。
(6）复（3)(4)（５），直到某一代m 只蚂蚁迭代结束。
（7）更新信息素矩阵，其中没有到达的蚂蚁不计算在内。
(1)(1)()ij ij ij t t τρττ+=-?+?
,k i j ()()0k i j k ij Q L t t τ???=???
如果蚂蚁经过，，蚂蚁不经过几点，
其中 为信息素挥发系数。Ｑ为信息量增加强度。()
k
L t
为路径长度。
(8)重复(３）-（7），直至n代蚂蚁迭代结束。
4．4 运行结果（图、表等）
将上述矩阵输入到程序中，画出最短路径的路线,并且输入每一轮迭代的最短路径,查看程序的收敛效果,在程序中设置ploｔif＝1则输出收敛和最短路径图，在程序中设置plo ｔif2=1则输出每一代蚂蚁的路径图。
最终输出的结果如图
funｃｔionｍ_mａin(）
Ｇ=［0 0 0 0 00 0 0 0 ０0 ０0 ００0０0 0 0;
0 1 １0 0 0 0 0 ０0 0 000 ００0 0 0０；
０1 1 0 0 0 １ 1 1 0 00 0 0 00 00 0 ０;
０0 0 0 00 １1 1 0 00 00 ０0 ０00 0;
０0００0 0 1 1 1 0 ００0 ００0 ０0 0 ０；
0 １1１0 0 1 1 １０00 ０000 0 0 0 0；
0 １１1 0０1 1 1 0 ００00 ０0 0 0 0 0;
０1 1 1 ０0 1 1 1 0 １1 1 1 ０0 0 0 00;
０1 1 １00 00 00 1 1 １1０0 0 0 0 0;
00 0 00 0 0 00 01１1 1 0 00 0 00;
０0 0 0 0 0 0 1 １0 1 1 1 1０00 ０0 ０;
００0 0 ０0 ０1 １0 0０0 0 ０0 ０00 0;
000 ０0 ０0 000 0 1 1 １0 1１１ 1 ０;
0 00００00００00 1 1 1 ０1 1 1 １0；
0 0 1 1 0 ０00 0 0 ０1 1 1 0 1 1 １1 0;
00 1 1 0 0 1 1 1 0 ００0 0 0 0 ０0 0 0;
0 0 0 ００0 1 1 1 ０1 1 0 0 00０１１0；
00 0 0 0 0 000 0 1 1 ０0 1 0 0 1 １0;
0 0 0 0 0 0 0 ０００0 0 00 10 ０0 ０0;
0 0 ０0０0 ０0 0 0 0 00 0 0 0 0 00 ０;];
ＭM=size(G,1);% G 地形图为01矩阵,如果为1表示障碍物
Tａu＝onｅs(MM*MM,ＭM*MM);%Tau初始信息素矩阵（认为前面的觅食活动中有残留的信息素)
Ｔaｕ=8.＊Tａu;
K=100; % K 迭代次数（指蚂蚁出动多少波)
M=5０; % Ｍ蚂蚁个数（每一波蚂蚁有多少个)
Ｓ＝1 ；％S 起始点(最短路径的起始点)
E＝MM*MM; % Ｅ终止点(最短路径的目的点）
Alpha=1;% Ａlpha表征信息素重要程度的参数
Ｂetａ=7; % Ｂeta 表征启发式因子重要程度的参数
Rhｏ＝0.3 ; ％Ｒhｏ信息素蒸发系数
Q＝1；% Q信息素增加强度系数
ｍinkl=inｆ;
mｉnｋ=０;
ｍinｌ=0；
Ｄ=G2Ｄ(G）;
N＝sizｅ（Ｄ,１）;％N表示问题的规模（象素个数）
a=1；%小方格象素的边长
Eｘ=ａ*(mod(Ｅ,MM)-0.５);％终止点横坐标
iｆEｘ=＝-０.５
Ex=MM-０.５;
eｎｄ
Ey=a*(MM+0．5－ceil（E/ＭM))；％终止点纵坐标
Eta＝zerｏs（Ｎ);%启发式信息,取为至目标点的直线距离的倒数
%下面构造启发式信息矩阵
ｆor i=1:N
ｉx＝a＊(ｍoｄ(i，ＭM)－0.5);
iｆｉx==－０.5
ｉx=MM-0．5；
ｅnd
iy=a＊(MM+0．５-ｃeｉl(i/MＭ));
if i~=E
Ｅｔa(i）=1/((ix-Ｅｘ)＾2+（iｙ-Ey)^2)^0.5；
elsｅ
Eｔa(i)=1０0;
end
end
ROＵＴES＝ｃell(Ｋ,M);%用细胞结构存储每一代的每一只蚂蚁的爬行路线
ＰL=zeｒos(K,Ｍ）;％用矩阵存储每一代的每一只蚂蚁的爬行路线长度
％％-----------启动K轮蚂蚁觅食活动,每轮派出M只蚂蚁－-------－--－－------- foｒk=1:K
ｆor ｍ=1:Ｍ
%%第一步：状态初始化
Ｗ=S;％当前节点初始化为起始点
Paｔｈ=S;%爬行路线初始化
PLkｍ＝0;％爬行路线长度初始化
TABUkｍ=ｏnｅs(N)；%禁忌表初始化
ＴＡBUkm(S）=０；%已经在初始点了，因此要排除
DD=Ｄ;%邻接矩阵初始化
％％第二步：下一步可以前往的节点
DW=DD（W，:）;
DW1=ｆinｄ(DW)；
foｒj=1：length(DW1)
ifＴABUkm(DW1（j））==0
DW(ＤW1(j)）＝0;
ｅnd
eｎd
LJD=find(DW);
Ｌｅn_ＬJD=lｅｎgth（LJD);%可选节点的个数
%％觅食停止条件:蚂蚁未遇到食物或者陷入死胡同
wｈｉle W~=Ｅ&&Lｅn_LＪD>=1
％%第三步：转轮赌法选择下一步怎么走
PＰ=zeros（Len＿LJD);
ｆｏr ｉ＝１：Lｅn_LJD
ＰＰ(ｉ)=(Tau（W,LJD(i))^Alｐha)＊（(Eｔa（LJD（ｉ)）)^Beta); end
sｕmpｐ=sｕm(PＰ）；
ＰP=PP/sｕmpp;%建立概率分布
Pcum(１)=PＰ(１);
foｒi=2:Lｅn_LJD
Pcｕｍ（ｉ)=Ｐcum(i－1)+PP（i)；
end
Selecｔ=ｆiｎd（Ｐｃｕm>=rａnd)；
to_visit＝LJD（Ｓelect(１）);
%% 第四步:状态更新和记录
Pａth=[Path,to_ｖisit];％路径增加
PLｋｍ=PLkm+DD(W,tｏ_vｉsit);%路径长度增加
Ｗ=to＿vｉsit；％蚂蚁移到下一个节点
ｆoｒｋk=1:Ｎ
if TABUkｍ(ｋk)==0
DD(W,kk)=0；
DＤ(kk，W）=０;
ｅnd
end
TABUkm（Ｗ）=０;％已访问过的节点从禁忌表中删除
ＤW=DD（W,:);
DＷ1=fｉnd（ＤＷ);
fｏr ｊ=1：leｎgｔh（ＤW１)
if TABUｋｍ(DＷ1(j))=＝0
DW(j)＝0;
end
end
LJD＝fｉnd（DＷ)；
Leｎ_LJＤ＝lenｇth(LJD);%可选节点的个数
end
％% 第五步:记下每一代每一只蚂蚁的觅食路线和路线长度
ＲOUTＥS{ｋ,m｝=Paｔh;
iｆPａtｈ(ｅnd)==Ｅ
PL(ｋ，ｍ)=ＰLkm；
if PＬkm＜mｉnkl
mink=k;minl=m;miｎkl=ＰＬkｍ;
eｎｄ
eｌsｅ
PＬ(ｋ，m)=0；
eｎd
ｅnd
%% 第六步:更新信息素
Dｅｌta＿Tau＝ｚeros(N，Ｎ);%更新量初始化
ｆor m=１:M
iｆＰＬ(k,m)
ROUT=ROUTES{k,m};
TS=length(ROUＴ)-1;%跳数
PＬ_kｍ=PL(k，m);
for s=1:ＴＳ
ｘ=ROUT(ｓ);
y=RＯＵT（s+1）;
Delta＿Taｕ(x,y)=Deltａ_Tａu(ｘ,y)+Q/PL_ｋm;
Delta_Tau（ｙ，ｘ）=Delta_Tau(y,x）+Q/PL＿km;
end
enｄ
end
Tau=（１-Rhｏ).*Ｔau+Deltａ_Tau;%信息素挥发一部分,新增加一部分
end
％% ----－-－---------－--－-－----－绘图－－-----－----－---－-－--－-－------－- ｐlotiｆ=１;%是否绘图的控制参数
if ｐlｏｔiｆ==1
％绘收敛曲线
minPL＝zeroｓ(K);
for i＝１:K
PＬK=PL（i,:);
Nonzero=find(PLK）;
PLKＰLK=PＬK(Nonzｅrｏ）;
minPL（i)＝miｎ(ＰLＫPＬK);
end
figｕｒe(1)
pｌot(ｍinPL)；
hｏlｄon
grid on
title('收敛曲线（最小路径长度）＇);
xlａbeｌ('迭代次数＇);
ylabeｌ('路径长度')；
％绘爬行图
fｉｇurｅ(２)
axｉｓ([０,ＭM,0,ＭＭ])
ｆoｒi=1:MM
ｆoｒj=1:ＭＭ
iｆＧ（i,j）==1
ｘ1=ｊ－1;y１＝MM-i;
ｘ2＝j;y２=ＭM－i；
ｘ3=j;ｙ３=MM-i+1;
ｘ4=j-１;y4=MM-i+1;
filｌ(［ｘ１，x2，x3,ｘ４],[y1，ｙ２，y3,y4],[0.２,0.2,0.2］); ｈold on
ｅlse
x１=j－1;ｙ1＝MM-i;
x２＝j;y2＝ＭM-i;
x3=j;y3＝MM-i+1;
x4=j-1;y4=MＭ-i+1;
fill([ｘ1,x2,x3,ｘ4],［y1，y2,y3,ｙ４］，［1,1,1］);
hｏld ｏn
end
ｅnｄ
eｎd
hｏldｏn
RＯＵT＝ROUTES｛minｋ,minｌ};
LＥNRＯＵT=lengｔh（RＯＵT);
Rx=ROUT;
Ry=ＲOUＴ;
for ii=1:ＬＥＮＲＯUT
Rx（ｉi）＝a*(moｄ(ROUＴ(iｉ),MＭ）-０．5);
if Rx(ii）==-0.５
Ｒｘ(ii)=MM－0.５;
end
Ry(ii)=a*（MＭ+0.5-ceil（ＲOUT(ii）/MM）);
eｎｄ
pｌoｔ（Rｘ,Ry）
end