路径规划基于matlab双向蚁群算法栅格地图机器人路径规划【含Matlab源码 3277期】.zip

%%蚂蚁双向行走，取正向走的前半段与反向走的后半段， %%每当前向蚂蚁遇到后项蚂蚁时截取路径，当都没有遇 %%到且都没到终点时略去路径，当前向路径找不到终点， %%后向路线找到终点时，取后项路径，PL存储结合后的 %%每代每只蚂蚁行走的路径长度 function antcycle_TEST1() clc;clear all; G=[0 0 0 0 0 1 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 0 0 0 0 0 0 0 0 0; 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0; 0 0 1 1 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 0 0 1 1 0 1 1 1 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 1 1 0 1 1 1 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 1 1 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 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 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 0 0 1 1 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 0 0 0 0 0 0 0 0 1 1 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 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 1 0 0; 0 0 1 1 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 0 0 0 0 0 0]; %G = repmat(G,2,2); MM=size(G,1); %G地形图为01矩阵，求矩阵G的行数 Tau=ones(MM*MM,MM*MM); %Tau初始信息素矩阵 Tau=1.*Tau; K=50; %迭代次数（指蚂蚁出动多少波） M=35; %蚂蚁个数 S=1; %最短路径的起始点 E=MM*MM; %最短路径的目标点 Alpha_Min=0.9; Beta_Max=6; Alpha_D=0.2; Beta_D=2; Alpha=Alpha_Min; %Alpha表征信息素重要程度的参数 Beta=Beta_Max; %Beta表征启发式因子重要程度的参数 Rho=0.4; %Rho信息素蒸发系数 Q=100; %Q信息素增加强度系数 minkl=inf; %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)); %终止点纵坐标 %%%%加1 Ex1=a*(mod(S,MM)-0.5); %终止点横坐标 if Ex==-0.5 Ex1=MM-0.5; end Ey1=a*(MM+0.5-ceil(S/MM)); %终止点纵坐标 %%%%%end Eta=zeros(N,1); %启发式信息，取至目标点的直线距离的倒数 %%%%%加2 Eta1=zeros(N,1); %%%%%end %以下启发式信息矩阵 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 该值变大，倾向于最短到终点路径，当前点i到目标点E的直线距离的倒数 else Eta(i)=100; end end %%%%加3 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~=S Eta1(i)=1/((ix-Ex1)^2+(iy-Ey1)^2); %^0.5 该值变大，倾向于最短到终点路径，当前点i到目标点E的直线距离的倒数 else Eta1(i)=100; end end %%%%end ROUTES=cell(K,M);%用细胞结构存储每一代的每一只蚂蚁的爬行路线 ROUTES1=cell(K,M);%加 ROUTES2=cell(K,M); PL=zeros(K,M); %用矩阵存储每一代的每一只蚂蚁的爬行路线长度 %（横着存储的） %启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁 for k=1:K for m=1:M %状态初始化 W=S; %当前节点初始化为起始点 W当前节点 W1=E;%加 Path=S; %爬行路线初始化 Path1=E;%加 PLkm=[0]; %爬行路线长度初始化 PLkm1=[0];%加 TABUkm=ones(N,2); %禁忌表初始化 TABUkm(S,1)=0; %已经在初始点了，因此要排除 TABUkm(E,2)=0;%加 DD=D; %邻接矩阵初始化 DDD=D; %下一步可以前往的节点 DW=DD(W,:); DW1=find(DW); %发现所有不为零的位置 DWE=DDD(E,:);%加 DWE1=find(DWE);%加 for j=1:length(DW1) if TABUkm(DW1(j),1)==0 DW(DW1(j))=0; end end %%%加 for j=1:length(DWE1) if TABUkm(DWE1(j),2)==0 DWE(DWE1(j))=0; end end %%%end LJD=find(DW); Len_LJD=length(LJD); %可选节点的个数 %蚂蚁未遇到食物或者陷入死胡同或者觅食停止 LJD1=find(DWE);%加 Len_LJD1=length(LJD1);%加 while isempty(find(Path1==W))&&(Len_LJD>=1||Len_LJD1>=1)&&(W~=E&&W1~=S) %轮转赌法选择下一步怎么走 if 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; %建立概率分布 % [~, select]=max(PP); % to_visit=LJD(select); %选择概率最大的 Pcum =zeros(1,Len_LJD); %添加 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,PLkm(end)+DD(W,to_visit)]; %路径长度增加 W=to_visit; %蚂蚁移到下一个节点 for kk=1:N if TABUkm(kk,1)==0 DD(W,kk)=0; DD(kk,W)=0; end end TABUkm(W,1)=0; %已访问过的节点从禁忌表中删除 DW=DD(W,:); DW1=find(DW); for j=1:length(DW1) if TABUkm(DW1(j),1)==0 DW(DW1(j))=0; %修改原DW(j)=0; end end LJD=find(DW); Len_LJD=length(LJD); %可选节点的个数 end %%%加 if Len_LJD1>=1 PP=zeros(Len_LJD1); for i=1:Len_LJD1 PP(i)=(Tau(W1,LJD1(i))^Alpha)*((Eta1(LJD1(i)))^Beta); end sumpp=sum(PP); PP=PP/sumpp; %建立概率分布 % [~, select]=max(PP); % to_visit=LJD(select); %选择概率最大的 PP=flipud(PP); Pcum =zeros(1,Len_LJD1); %添加 Pcum(1)=PP(1); for i=2:Len_LJD1 Pcum(i)=Pcum(i-1)+PP(i); end Select=find(Pcum>=rand); LJD1=flipud(LJD1'); to_visit=LJD1(Select(1)); %下一个移动节点 %状态更新和记录 Path1=[Path1,to_visit]; %路径增加 PLkm1=[PLkm1,PLkm1(end)+DDD(W1,to_visit)]; %路径长度增加 W1=to_visit; %蚂蚁移到下一个节点 for kk=1:N if TABUkm(kk,2)==0 DDD(W1,kk)=0; DDD(kk,W1)=0; end end TABUkm(W1,2)=0; %已访问过的节点从禁忌表中删除 DWE=DDD(W1,:); DWE1=find(DWE); for j=1:length(DWE1) if TABUkm(DWE1(j),2)==0 DWE(DWE1(j))=0; %修改原DW(j)=0; end end LJD1=find(DWE); Len_LJD1=length(LJD1); %可选节点的个数 end end %%%end %记下每一代每一只蚂蚁的觅食路线和路线长度 ROUTES{k,m}=Path; ROUTES1{k,m}=Path1; if isempty(find(Path1==W))==0 Path=Path(1:find(Path==W));%找到前一半的路径 Path1=Path1(1:(find(Path1==W)));%找到后一半的路径 PL(k,m)=PLkm(find(Path==W))+PLkm1(find(Path1==W));%合并路径长度 Path1=Path1(1:(length(Path1)-1)); ROUTES2{k,m}=[Path,flipud(Path1')'];%合并路径 else if Path1(end)==S ROUTES2{k,m}=flipud(Path1')'; PL(k,m)=PLkm1(end); else ROUTES2{k,m}=0; PL(k,m)=0; end if PL(k,m)~=0&&PL(k,m)<minkl mink=k;minl=m;minkl=PL(k,m); end end end %%%%加 %更新信息素 PL1=PL(find(PL)); RMAXk=ceil(find(PL==min(PL1))/K);%找到本次循环的最小距离路径 Delta_Tau=zeros(N,N); %更新量初始化 for m=1:M if PL(k,m) ROUT=ROUTES2{k,m}; TS=length(ROUT)-1; %跳数 PL_km=PL(k,m); for s=1:TS x=ROUT(s); y=ROUT(s+1); if m~RMAXk; Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km; Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km; else Delta_Tau(x,y)=Delta_Tau(x,y)+2*Q/PL_km;%对最小距离增加额外信息素 Delta_Tau(y,x)=Delta_Tau(y,x)+2*Q/PL_km; end end end end % % Delta_Tau = (0.75+k/(2*K))*Delta_Tau; % 根据时间更新 % Rho = 0.4+0.2*k/K;%sqrt(4-(k/K-2)^2)/sqrt(3);%一开始保留信息素少，最后保留多，初始信息素不重要 Alpha=Alpha_Min+Alpha_D*sqrt(4-(k/K-2)^2)/sqrt(3);%0.8+0.4*(k/K);%开始信息素作用小，后期变大 Beta=Beta_Max-Beta_D*sqrt(4-(k/K-2)^2)/sqrt(3);%6-2*(k/K);