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

1、MATLAB实现基于蚁群算法的机器人路径规划1、问题描述移动机器人路径规划是机器人学的一个重要研究领域。它要求机器人依据某个或某些优化原则(如最 小能量消耗，最短行走路线，最短行走时间等)，在其工作空间中找到一条从起始状态到目标状态的能避开障碍物的最优路径。机器人路径规划问题可以建模为一个有约束的优化问题，都 要完成路径规划、定位和避障等任务。2算法理论蚁群算法(Ant Colony Algorithm，ACA )，最初是由意大利学者Dorigo M.博士于1991年首次提 出，其本质是一个复杂的智能系统，且具有较强的鲁棒性，优良的分布式计算机制等优点。该算法经过十 多年的发展，已被广大的科学

2、研究人员应用于各种问题的研究，如旅行商问题，二次规划问题，生产调度 问题等。但是算法本身性能的评价等算法理论研究方面进展较慢。Dorigo提出了精英蚁群模型(EAS )，在这一模型中信息素更新按照得到当前最优解的蚂蚁所构造的 解来进行，但这样的策略往往使进化变得缓慢，并不能取得较好的效果。次年Dorigo博士给出改进模型 (ACS )，文中改进了转移概率模型，并且应用了全局搜索与局部搜索策略，来得进行深度搜索。Stiitzle与Hoos给出了最大最小蚂蚁系统(MAX-MINAS )，所谓最大-最小即是为信息素设定上限与 下限，设定上限避免搜索陷入局部最优，设定下限鼓励深度搜索。蚂蚁作为一个生物

3、个体其自身的能力是十 分有限的，比如蚂蚁个体是没有视觉的，蚂蚁自身体积又是那么渺小，但是由这些能力有限的蚂蚁组成的 蚁群却可以做出超越个体蚂蚁能力的超常行为。蚂蚁没有视觉却可以寻觅食物，蚂蚁体积渺小而蚁群却可 以搬运比它们个体大十倍甚至百倍的昆虫。这些都说明蚂蚁群体内部的某种机制使得它们具有了群体智能， 可以做到蚂蚁个体无法实现的事情。经过生物学家的长时间观察发现，蚂蚁是通过分泌于空间中的信息素进 行信息交流，进而实现群体行为的。下面简要介绍蚁群通过信息素的交流找到最短路径的简化实例。如图2-1所示，AE之间有两条路 ABCDE与ABHDE，其中AB，DE，HD，HB的长度为1，BC，CD长度

4、为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上

5、的 信息素的浓度是路径BHD上信息素浓度的两倍，这样若再次有蚂蚁选择走BC和BH时，或选择走DC与 DH时，都会以较大的概率选择信息素浓度高的一边。这样的过程反复进行下去，最短的路径上走过的蚂蚁 较多，留下的信息素也越多，蚁群这样就可以找到一条较短的路。这就是它们群体智能的体现。蚁群算法就是模拟蚂蚁觅食过程中可以找到最短的路的行为过程设计的一种仿生算法。在用蚁群算法求 解组合优化问题时，首先要将组合优化问题表达成与信息素相关的规范形式，然后各个蚂蚁独立地根据局部的 信息素进行决策构造解，并根据解的优劣更新周围的信息素，这样的过程反复的进行即可求出组合优化问题 的优化解。归结蚁群算法有如下特点：

6、(1)分布式计算：各个蚂蚁独立地构造解，当有蚂蚁个体构造的解较差时，并不会影响整体的求解结 果。这使得算法具有较强的适应性；(2)自组织性：系统学中自组织性就是系统的组织指令是来自系统的内部。同样的蚁 群算法中的各个蚂 蚁的决策是根据系统内部信息素的分布进行的。这使得算法具有较强的鲁棒性；(3)正反馈机制与负反馈机制结合：若某部分空间上分布的信息素越多，那么在这个空间上走过的蚂蚁 也就越多；走过的蚂蚁越多，在那个空间上留下的信息素也就越多，这就是存在的正反馈机制。但蚁群算 法中解的构造是通过计算转移概率实现的，也就是说构造解的时候可以接受退化解，这限制了正反馈机制，可以使得搜索范围扩大，这是蚁

7、群算法中隐含的负反馈机制。3求解步骤应用蚁群算法求解机器人路径优化问题的主要步骤如下：。表(1)输入由0和1组成的矩阵表示机器人需要寻找最优路径的地图的地图，其中 示此处可以通过的，1表示此处为障碍物。图的表示矩阵为：00000000000000000000:011000000000000 0 0 0 0 0;0 1 1 0 0 0 1 1 1 0000000000 0; 0000001 1 1 0000000000 0; 000000 1 1 1 0000000000 0；01 1 1 00 1 1 1 0000000000 0； 0 1 1 1 0 0 1 1 1 000000000 0

8、0； 0111001110111100000 0; 011100000011 1 1 00000 0; 0000000000 1 1 1 1 00000 0; 000000 0110111100000 0;0000000 1 1 0000000000 0;0000000000011101111 0;0000000000011101111 0;0011000000011101111 0;001 1001 1 10000000000 0;0000001 1 101 1000001 1 0;00000000001 1001001 1 0;00000000000000 1 0000 0;0000000

9、000000000000 0;(2)输入初始的信息素矩阵，选择初始点和终止点并且设置各种参数。在此次计算中，我们设置所有位 置的初始信息素相等。(3)选择从初始点下一步可以到达的节点，根据每个节点的信息素求出前往每个节点的概率，并利用轮 盘算法选取下一步的初始点。ij t ij k N tabuk0ifj N tabukotherwise其中ijt为析取图中弧i,j上的信息素的浓度。ij为与弧i,j相关联的启发式信息。，分别为杜，可的权重参数。(4)更新路径，以及路程长度。(5) 重复(3) ( 4)过程，直到蚂蚁到达终点或者无路可走。(6)重复(3) (4) ( 5)，直到某一代m只蚂蚁迭代

10、结束。(7)更新信息素矩阵，其中没有到达的蚂蚁不计算在内。ijt 1 1 ijtijLkQt，如果蚂蚁k经过i，0，如果蚂蚁k不经过i其中为信息素挥发系数。Q为信息量增加强度。Lk t为路径长度。(8)重复(3)(7)，直至n代蚂蚁迭代结束。4运行结果(图、表等)将上述矩阵输入到程序中，画出最短路径的路线，并且输入每一轮迭代的最短路查看程序的收敛效果，在程序中设置plotif=1则输出收敛和最短路径图，在程序中设置plotif2=1则输出每 代蚂蚁的路径图。最终输出的结果如图1820收敛曲线（最小路径长度）度长径路5 O5 0 5 0 53 3 2 2 15 MA TLAB 程序 functi

11、on m_main()G=0 0 0 0 0 0 0 0 0 0 0 10 20 30 40 50 60 70 80 90 100 迭代次数0000000 0；011000000000000000000110001000000100000010111001011100101110010 111111111I 1II0000000000000000000000000000000000000000000000000000000110111111 11 10000001000000000010000000110110000001000000100000010000000000000110000000

12、00000000000000011101111000000000000111011110001100000001110111100011001110000000000000000011101100000110000000000011001001100000000000000010000000000000000000000000MM=size(G,1); % G地形图为01矩阵，如果为1表示障碍物Tau=ones(MM*MM,MM*MM);% Tau初始信息素矩阵(认为前面的觅食活动中有残留的信 息素) Tau=8.*Tau;K=100; % K迭代次数(指蚂蚁出动多少波)M=50; % M蚂蚁

13、个数(每一波蚂蚁有多少个)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.5Ex=MM-0.5; endEy=a*(MM+0.5ceil(E/MM)

14、;% 终止点纵坐标Eta=zeros(N);%启发式信息，取为至目标点的直线距离的倒数下面构造启发式信息矩阵 for i=1 :N ix=a*(mod(i,MM)-0.5);if ix=-0.5ix=MM-0.5;end iy=a*(MM+0.5-ceil(i/MM); if i=EEta(i)=1/(ix-Ex)A2+(iy-Ey)A2)A0.5; else Eta(i)=100; end endROUTES=cell(K,M);%用细胞结构存储每一代的每一只蚂蚁的爬行路线PL=zeros(K,M);%用矩阵 存储每一代的每一只蚂蚁的爬行路线长度% 启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁 .

15、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 endLJD=find(DW);Len_LJD=length(LJD);%可选节点的个数%觅食停止条件：蚂蚁未遇到食

16、物或者陷入死胡同while W=E&&Len_LJD>=1%第三步：转轮赌法选择下一步怎么走PP=zeros(Len_LJD);for i=1 :Len_LJD PP(i)=(Tau(W,LJD(i)AAlpha)*(Eta(LJD(i)ABeta);endsumpp=sum(PP);PP=PP/sumpp;%建立概率分布Pcum(1)=PP(1);for i=2:Len_LJD Pcum(i)=Pcum(i-1 )+PP(i); endSelect=find(Pcum>=rand); to_visit=LJD(Select(1); % 第四步：状态更新和记录Pat

17、h=Path,to_visit;% 路径增加PLkm=PLkm+DD(W,to_visit);%路径长度增加 W=to_visit;%蚂蚁移到下一个节点 for kk=1:N if TABUkm(kk)=O DD(W,kk)=0; DD(kk,W)=0; end endTABUkm(W)=0;%已访问过的节点从禁忌表中删除DW=DD(W,:);DW1=find(DW);for j=1 :length(DW1)if TABUkm(DW1(j)=0DW(j)=0;endend LJD=find(DW); Len_LJD=length(LJD);% 可选节点的个数 end%第五步：记下每一代每一只蚂

18、蚁的觅食路线和路线长度ROUTESk,m=Path;if Path(end)=E PL(k,m)=PLkm; if PLkm<minkl mink=k;minl=m;minkl=PLkm; endelse PL(k,m)=O;endend%第六步：更新信息素Delta_Tau=zeros(N,N);%更新量初始化form=1:Mif PL(k,m) ROUT=ROUTESk,m;TS=length(R0UT)-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

19、_km; Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;endendendTau=(1Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分end% 绘图plotif=1;%是否绘图的控制参数if plotif=1 %绘收敛曲线 minPL=zeros(K);for i=1:KPLK=PL(i,:);Nonzero=find(PLK);PLKPLK=PLK(Nonzero);minPL(i)=min(PLKPLK);endfigure(1)plot(minPL);hold ongrid onMtle('收敛曲线(最小路径长度)；xlabe

20、l('迭代次数1);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 onelsex1=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,

21、y3,y4,1,1,1);hold on end end end hold onROUT=ROUTESmink,minl;LENROUT=length(ROUT);Rx=ROUT;Ry=ROUT;forii=1:LENROUTRx(ii)=a*(mod(ROUT(ii),MM)-0.5); if Rx(ii)=-0.5Rx(ii)=MM-0.5; end Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM); endplot(Rx,Ry) endplotif2=0;%绘各代蚂蚁爬行图if plotif2=1 figure(3) axis(0,MM,0,MM) for i=1:MM forj=1:MM if G(i,j)=1 x1 =j-1 ;y1 =MM-i;x2=j;