基于蚁群算法的机器人路径规划MATLAB源码

1、基于蚁群算法的机器人路径规划MATLAB源码使用网格离散化的方法对带有障碍物的环境建模，使用邻接矩阵存储该环境，使得问题转化为蚁群算法寻找最短路径。functionROUTES,PL,Tau=ACASPS(G,Tau,K,M,S,E,Alpha,Beta,Rho,Q)%ACASP.m%蚁群算法动态寻路算法%GreenSim团队原创作品，转载请注明%GreenSim团队长期从事算法设计、代写程序等业务%欢迎访问GreenSim算法仿真团队-%输入参数列表%G地形图为01矩阵，如果为1表示障碍物%Tau初始信息素矩阵(认为前面的觅食活动中有残留的信息素)%K迭代次数(指蚂蚁出动多少波)%M蚂蚁个数

2、(每一波蚂蚁有多少个)%S起始点(最短路径的起始点)%E终止点(最短路径的目的点)%Alpha表征信息素重要程度的参数%Beta表征启发式因子重要程度的参数%Rho信息素蒸发系数%Q信息素增加强度系数%输出参数列表%ROUTES每一代的每一只蚂蚁的爬行路线%PL每一代的每一只蚂蚁的爬行路线长度%Tau输出动态修正过的信息素%变量初始化%loadD=G2D(G);N=size(D,1);%N表示问题的规模(象素个数)MM=size(G,1);a=1;%小方格象素的边长Ex=a*(mod(E,MM)-0.5);%终止点横坐标ifEx=-0.5Ex=MM-0.5;endEy=a*(MM+0.5-ce

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

4、rk=1:K%disp(k);form=1:M%第一步：状态初始化W=S;%当前节点初始化为起始点Path=S;%爬行路线初始化PLkm=0;%爬行路线长度初始化TABUkm(S)=0;%已经在初始点了，因此要排除DD=D;%邻接矩阵初始化%第二步：下一步可以前往的节点DW=DD(W,:);DW1=find(DW<inf);forj=1:length(DW1)ifTABUkm(DW1(j)=0endendLJD=find(DW<inf);%可选节点集Len_LJD=length(LJD);%可选节点的个数%觅食停止条件：蚂蚁未遇到食物或者陷入死胡同whileW=E&&

5、;Len_LJD>=1%第三步：转轮赌法选择下一步怎么走PP=zeros(1,Len_LJD);fori=1:Len_LJDendPP=PP/(sum(PP);%建立概率分布Pcum=cumsum(PP);Select=find(Pcum>=rand);to_visit=LJD(Select(1);%下一步将要前往的节点%第四步：状态更新和记录Path=Path,to_visit;%路径增加PLkm=PLkm+DD(W,to_visit);%路径长度增加W=to_visit;%蚂蚁移到下一个节点forkk=1:NifTABUkm(kk)=0DD(W,kk)=inf;DD(kk,W)

6、=inf;endendTABUkm(W)=0;%已访问过的节点从禁忌表中删除DW=DD(W,:);LJD=find(DW<inf);%可选节点集Len_LJD=length(LJD);%可选节点的个数end%第五步：记下每一代每一只蚂蚁的觅食路线和路线长度ROUTESk,m=Path;ifPath(end)=EPL(k,m)=PLkm;elsePL(k,m)=inf;endend%第六步：更新信息素Delta_Tau=zeros(N,N);%更新量初始化form=1:MifPL(k,m)<infROUT=ROUTESk,m;TS=length(ROUT)-1;%跳数PL_km=PL

7、(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;endendendTau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分end%绘图plotif=0;%是否绘图的控制参数ifplotif=1%绘收敛曲线meanPL=zeros(1,K);minPL=zeros(1,K);fori=1:KPLK=PL(i,:);Nonzero=find(PLK<inf);PLKPLK=PLK(Non

8、zero);meanPL(i)=mean(PLKPLK);minPL(i)=min(PLKPLK);endfigure(1)plot(minPL);holdonplot(meanPL);gridontitle('收敛曲线(平均路径长度和最小路径长度)');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(x

9、1,x2,x3,x4,y1,y2,y3,y4,0.2,0.2,0.2);holdonelsex1=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);holdonendendendholdonROUT=ROUTESK,M;Rx=ROUT;Ry=ROUT;forii=1:LENROUTRx(ii)=a*(mod(ROUT(ii),MM)-0.5);ifRx(ii)=-0.5Rx(ii)=MM-0.5;endRy(ii)=a*(MM+0.5-ceil(ROUT(ii

10、)/MM);endplot(Rx,Ry)endplotif2=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;x4=j-1;y4=MM-i+1;fill(x1,x2,x3,x4,y1,y2,y3,y4,0.2,0.2,0.2);holdonelsex1=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);holdonendendendfork=1:KPLK=PL(k,:);minPLK=min(PLK);pos=find(PLK=minPLK);m=pos(1);ROUT=ROUTESk,m;LENROUT=leng