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

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

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

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

4、E 之间有两条路 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与D

5、C的蚂蚁分别走过了 BCD和DCB，而选择BH与 DH 的蚂蚁都走到了 H 点。所有的蚂蚁都在所走过的路上留下了相同浓度的信息素，那么 路径 BCD 上的信息素的浓度是路径 BHD 上信息素浓度的两倍，这样若再次有蚂蚁选择走 BC 和 BH 时，或选择走 DC 与 DH 时，都会以较大的概率选择信息素浓度高的一边。这 样的过程反复进行下去， 最短的路径上走过的蚂蚁较多， 留下的信息素也越多， 蚁群这样就 可以找到一条较短的路。这就是它们群体智能的体现。蚁群算法就是模拟蚂蚁觅食过程中可以找到最短的路的行为过程设计的一种仿生算法。 在用蚁群算法求解组合优化问题时，首先要将组合优化问题表达成与信息素

6、相关的规范形 式，然后各个蚂蚁独立地根据局部的信息素进行决策构造解，并根据解的优劣更新周围的信息素，这样的过程反复的进行即可求出组合优化问题的优化解。归结蚁群算法有如下特点：（1）分布式计算：各个蚂蚁独立地构造解，当有蚂蚁个体构造的解较差时，并不会影 响整体的求解结果。这使得算法具有较强的适应性；（2）自组织性：系统学中自组织性就是系统的组织指令是来自系统的内部。同样的蚁这使得算法具有较强的鲁群算法中的各个蚂蚁的决策是根据系统内部信息素的分布进行的。 棒性；这就也就是说构造解这是蚁群算法中隐（3）正反馈机制与负反馈机制结合：若某部分空间上分布的信息素越多，那么在这个 空间上走过的蚂蚁也就越多；

7、走过的蚂蚁越多，在那个空间上留下的信息素也就越多，是存在的正反馈机制。 但蚁群算法中解的构造是通过计算转移概率实现的， 的时候可以接受退化解， 这限制了正反馈机制， 可以使得搜索范围扩大， 含的负反馈机制。3求解步骤 应用蚁群算法求解机器人路径优化问题的主要步骤如下：（1）输入由0和1组成的矩阵表示机器人需要寻找最优路径的地图的地图，其中 示此处可以通过的，1表示此处为障碍物。100 000000000;100 000000000;100 000000000;100 000000000;100 000000000;1 0 10 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0

8、;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 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 10 0 0 0 0 0 1 0 0 0 0 0 0 10 10 10 10 10 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

9、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) 选择从初始点下一步可以到达的节点，根据每个节

10、点的信息素求出前往每个节点的 概率，并利用轮盘算法选取下一步的初始点。kpij =无L(tij卩 kN -tabUk /0ifj n -tabUkotherwise（4）（6）（7）其中Uj为与弧（i, j湘关联的启发式信其中Tj （t为析取图中弧（i, j ）上的信息素的浓度。息。a，P分别为Tij （t ）, n j的权重参数。更新路径，以及路程长度。重复（3）（ 4）过程，直到蚂蚁到达终点或者无路可走。 重复（3）（ 4）（ 5），直到某一代 m只蚂蚁迭代结束。 更新信息素矩阵，其中没有到达的蚂蚁不计算在内。Tij(t +1 )=(1 - P 卜 Tj(t )+人g（t金），如果蚂蚁k经

11、过i，j0,如果蚂蚁k不经过i，jP为信息素挥发系数。Q为信息量增加强度。Lk（t ）为路径长度。重复（3） - （7）,直至n代蚂蚁迭代结束。（8） 4运行结果（图、表等）将上述矩阵输入到程序中，画出最短路径的路线，并且输入每一轮迭代的最短路径，查看程序的收敛效果，在程序中设置plotif=1则输出收敛和最短路径图，在程序中设置plotif2=1则输出每一代蚂蚁的路径图。最终输出的结果如图204518161412108642068101214161820度长径路:iI11II il 111 1II I收敛曲线（最小路径长度）4035302520151050010203040506070809

12、0100迭代次数5 MATLAB 程序fun ctio n m_mai n()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 1Tau 初始信息素矩阵（认为前面的觅食活动中有残留的信K=100;M=50;S=1 ;指蚂蚁出动多少波） 每一波蚂蚁有多少个）E=MM*MM; Alpha=1;Beta=7;Rho=0.3 ; Q=1;minkl=inf; mink=0; minl=0;D=G2D(G);N=size(D,1);%N% K 迭代次数% M 蚂蚁个

13、数% S 起始点（最短路径的起始点）% E 终止点（最短路径的目的点） % Alpha 表征信息素重要程度的参数 % Beta 表征启发式因子重要程度的参数% Rho 信息素蒸发系数% Q 信息素增加强度系数表示问题的规模（象素个数）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 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;1 0 0 0 0 0 0;1 0 0 0 0 0 0;1 0 0 0 0 0 0;1 00 0 00 0;0 0 0 000 01 1

14、00 000 00 0 00 0;0 0 0 000 00 0 00 111 01 1 11 0;0 0 0 000 00 0 00 111 01 1 11 0;0 0 1 100 00 0 00 111 01 1 11 0;0 0 1 100 11 1 00 000 00 0 00 0;0 0 0 000 11 1 01 100 00 0 11 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=

15、size(G,1); % G 地形图为 01 矩阵，如果为 1 表示障碍物 Tau=ones(MM*MM,MM*MM);% 息素)Tau=8.*Tau;a=1;%小方格象素的边长 Ex=a*(mod(E,MM)-0.5);% 终止点横坐标 if Ex=-0.5Ex=MM-0.5;endEy=a*（MM+0.5-ceil（E/MM）;% 终止点纵坐标Eta=zeros（N）;% 启发式信息，取为至目标点的直线距离的倒数%下面构造启发式信息矩阵for i=1:Nix=a*(mod(i,MM)-0.5);if ix=-0.5ix=MM-0.5;endiy=a*(MM+0.5-ceiI(i/MM);i

16、f i=E Eta(i)=1/(ix-Ex)A2+(iy-Ey)A2)A0.5; eIseEta(i)=100;endendROUTES=cell(K,M);% 用细胞结构存储每一代的每一只蚂蚁的爬行路线 PL=zeros(K,M);% 用矩阵存储每一代的每一只蚂蚁的爬行路线长度 %启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁for k=1:K for m=1:M% 第一步：状态初始化W=S;% 当前节点初始化为起始点P ath=S;%爬行路线初始化PLkm=0;% 爬行路线长度初始化TABUkm=ones(N);% 禁忌表初始化TABUkm(S)=0;% 已经在初始点了，因此要排除DD=D;% 邻

17、接矩阵初始化% 第二步：下一步可以前往的节点DW=DD(W,:);DW1=find(DW);for j=1:length(DW1)if TABUkm(DW1(j)=0DW(DW1(j)=0;endendLJD=find(DW);Len_LJD=length(LJD);% 可选节点的个数% 觅食停止条件：蚂蚁未遇到食物或者陷入死胡同while W=E&&Len_LJD>=1% 第三步：转轮赌法选择下一步怎么走PP=zeros(Len_LJD);for i=1:Len_LJDPP (i)=(Tau(W,LJD(i)AAI pha)*(Eta(LJD(i)ABeta);ends

18、umpp=sum(PP);PP=PP/su mpp;%建立概率分布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); % 第四步：状态更新和记录 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 endTABU

19、km(W)=0;% 已访问过的节点从禁忌表中删除 DW=DD(W,:);DW1=find(DW);for j=1:length(DW1)if TABUkm(DW1(j)=0 DW(j)=0;endend LJD=find(DW); Len_LJD=length(LJD);% 可选节点的个数 end% 第五步：记下每一代每一只蚂蚁的觅食路线和路线长度 ROUTESk,m=Path;if Path(end)=E PL(k,m)=PLkm; if PLkm<minklmink=k;minl=m;minkl=PLkm;endelsePL(k,m)=0; endend% 第六步：更新信息素Delt

20、a_Tau=zeros(N,N);% 更新量初始化 for m=1:Mif PL(k,m)ROUT=ROUTESk,m;TS=length(ROUT)-1;% 跳数 PL_km=PL(k,m);for s=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; endendendTau=(1-Rho).*Tau+Delta_Tau;% 信息素挥发一部分，新增加一部分 end% 绘图plotif=1;% 是否绘图的控制参数if plotif=1 %

21、 绘收敛曲线 minPL=zeros(K);for i=1:K PLK=PL(i,:); Nonzero=find(PLK); PLKPLK=PLK(Nonzero); minPL(i)=min(PLKPLK);endfigure(1) plot(minPL); hold on grid on title(' 收敛曲线(最小路径长度) '); xlabel(' 迭代次数 ');ylabelC路径长度');%绘爬行图figure(2) axis(0,MM,0,MM) for i=1:MM for j=1:MM if G(i,j)=1 x1=j-1;y1=M

22、M-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,y3,y4,1,1,1);hold on end end end hold onROUT=ROUTESmink,minl;LENROUT=length(ROUT);Rx=ROUT;Ry=ROUT;for ii=1:LENROUTRx(ii)=a*(mod(ROUT(ii),MM)-0.5);if Rx(ii)=-0.5Rx(ii)=MM-0.5;endRy(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM);end plot(Rx,Ry) endplotif2=0;% 绘各代蚂蚁爬行图 if plotif2=1figure(3) axis(0,MM,0,MM)