蚁群算法静态路径规划

蚁群算法静态路径规划
实现蚁群的动态规划算法，首先分析蚁群算法静态路径规划的程序实现过程，忘记代码是在哪里找到的了，非常感谢代码的提供者啦~

1、建立地图：

G = ones(20);
G(3,3:7)=0;
G(3:10,7)=0;
G(10,3:7)=0;
G(17,13:17)=0;
G(10:17,13)=0;
G(10,13:17)=0;
G(14,15)=0;
b = G;
b(end+1,end+1) = 0;
colormap([0 0 0;1 1 1]);  % 创建颜色
pcolor(0.5:size(G,2)+0.5,0.5:size(G,1)+0.5,b); % 赋予栅格颜色

2、建立D矩阵，记录每个点至其邻域点的代价值。列是起点栅格，行是局部终点栅格，将各栅格点至其各邻域无障碍栅格的代价值填入其中，有障碍栅格及非相邻栅格为0。

function D=G2D(G) %邻节点初始化
l=size(G,1);         %l=G矩阵的行数
D=zeros(l*l,l*l);    %D是所有点到所有点的矩阵。纵向将所有点作为起点，横向将所有点作为邻域局部到达点
for i=1:l            
    for j=1:l 
        if G(i,j)==1   %判断局部起始栅格是否为自由栅格
            for m=1:l 
                for n=1:l 
                    if G(m,n)==1  %判断局部终点栅格，即邻域栅格是否为自由栅格
                        im=abs(i-m);jn=abs(j-n); %abs求绝对值
                        if im+jn==1||(im==1&&jn==1)  %判断该栅格是否为当前栅格的邻域栅格
                         D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5; %将所有点到所有邻域点的代价值填入矩阵中。
                                                             %D((i-1)*l+j,(m-1)*l+n)在矩阵中定位邻域矩阵的具体位置
                                                             %(im+jn)^0.5在具体位置中填入具体代价值  
                        end 
                    end 
                end 
            end 
        end 
    end 
end
end

该段程序结果以3*3地图为例：

3、蚁群算法的具体程序流程图如下：

对应代码：
1、初始化参数：

MM=size(G,1);                      % G 地形图为01矩阵 
Tau=ones(MM*MM,MM*MM);        % Tau 初始信息素矩阵，记录每个栅格至其他栅格的信息素量
Tau=8.*Tau;    %初始化每个栅格信息素为8，为什么？？？？
K=100;                             %迭代次数（指蚂蚁出动多少波）
M=50;                              %蚂蚁个数
S=1;                              %最短路径的起始点
E=MM*MM;                        %最短路径的目的点
Alpha=1;                           % Alpha 表征信息素重要程度的参数
Beta=7;                            % Beta 表征启发式因子重要程度的参数
Rho=0.3 ;                          % Rho 信息素蒸发系数
Q=1;                               % Q 信息素增加强度系数 
minkl=inf;    %minkl表示当前最短路径长度
mink=0;       %当前完成最短路径为第几次迭代
minl=0;       %当前完成最短路径为第只蚂蚁
D=G2D(G);     %每个栅格至各自邻域无障碍栅格的代价值
N=MM*MM;               %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)); %终止点纵坐标
Eta=zeros(N);             %启发式信息矩阵，记录启发式信息

2、 构建启发式信息η的矩阵：

%构建启发式信息矩阵。（MM，MM）为终止点的坐标值，计算每个栅格至终点的信息素量
 for i=1:N    %每个栅格的索引号，一共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; %启发信息取为至目标点的直线距离的倒数
   else 
   Eta(i)=100; 
   end 
end 
ROUTES=cell(K,M);     %用细胞结构存储每一代的每一只蚂蚁的爬行路线：
                      %cell(K,M)为K行M列的数组，存储第k次迭代中第M至蚂蚁的路线，
PL=zeros(K,M);         %用矩阵存储每一代的每一只蚂蚁的爬行路线长度
                       %第k次迭代中第M只蚂蚁爬行路线的长度

3、 开始寻路:

tic;  % tic-toc: Functions for Elapsed Time
for k=1:K            %第k次迭代
  for m=1:M          %第M只蚂蚁
  %状态初始化
      W=S;                  %当前节点的索引号初始化为起始点
      Path=S;                %爬行路线初始化
      PLkm=0;               %爬行路线长度初始化
      TABUkm=ones(N);       %禁忌表大小为地图像素个数，全部初始化为1
                            %禁忌表记录走过的位置，将走过的位置由1变0
      TABUkm(S)=0;          %将禁忌表起点位置置为0
      DD=D;                 %邻节点初始化
  %筛选下一步可以前往的节点
      DW=DD(W,:); %取G2D矩阵中以当前点为局部起始点的一行
      DW1=find(DW); %DW1矩阵存储该行中所有无障碍邻节点的索引位置：
                    %find()返回矩阵中所有不为0的元素（未走过的）的索引位置：
      for j=1:length(DW1) %length(DW1)返回DW1矩阵的长度
         if TABUkm(DW1(j))==0 %判断TABUkm禁忌表中，该位置是否为之前走过的点
            DW(DW1(j))=0; %删除DW中所有之前已经走过的邻节点
                         %如果禁忌表中记录该位置已经走过，则取出的该行对应点位置由非0置为0，之后的寻路不再重复记录该点
         end 
      end %现在DW中为当前节点可以选择的所有邻节点了
      % 计算各可选择邻节点的选择概率
      LJD=find(DW); %LJD记录未走过的点的索引号，即下一步可选的节点
      Len_LJD=length(LJD);%Len_LJD为可选节点的个数
      %蚂蚁未遇到食物或者陷入死胡同或者觅食停止
      while W~=E&&Len_LJD>=1 %起始点不等于终止点，且可选节点个数大于等于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;%蚂蚁从当前城市转移到各相邻城市的概率
         Pcum(1)=PP(1); %Pcum记录从当前城市转移到各城市的概率
         %轮盘赌选择下一步
         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+DD(W,to_visit);    %路径长度增加
         W=to_visit;                   %蚂蚁移到下一个节点，W代表当前节点
         %对应禁忌表修改G2D中可选择邻节点的值
         for kk=1:N      
         if TABUkm(kk)==0 %若禁忌表中该点为以走过的点
            DD(W,kk)=0;     %将G2D中表示起始点到终止点的对应栅格置零，以后不再对该点进行寻路。
            DD(kk,W)=0; 
          end 
      end
      %本只蚂蚁继续下一步节点继续寻路
      TABUkm(W)=0;  %更新禁忌表：
                    %将当前的W点在紧急表中置为以访问过的节点。禁忌表中以访问过的节点为0，未访问过的节点为1
      DW=DD(W,:);   
      DW1=find(DW);
      for j=1:length(DW1) 
         if TABUkm(DW1(j))==0 
            DW(j)=0; 
         end 
      end 
      LJD=find(DW); 
      Len_LJD=length(LJD);%可选节点的个数
   end %本次迭代的当前蚂蚁寻路完毕
   
   %退出当前蚂蚁的寻路过程，记下本次迭代当前蚂蚁的觅食路线和路线长度
   ROUTES{k,m}=Path; %记录本次迭代中的当前蚂蚁的行走路线
   if Path(end)==E  %判断本只蚂蚁寻找路径的最后一个节点是否为终点。
                    %Path(end)返回矩阵Path中的最后一个元素
      PL(k,m)=PLkm; %若该蚂蚁到达终点，将本次路线长度放到PL的第k行m列
      if PLkm<minkl %若本次路径长度<当前已知的最短路径长度
          mink=k; %记录完成本次最短路径的迭代次数
          minl=m;%记录完成本次最短路程的哪只蚂蚁
          minkl=PLkm; %记录本次最短路线的长度
      end 
   else 
      PL(k,m)=0; %若该蚂蚁没有到达终点长，则本次路径长度为0
   end 
end %返回进行下一只蚂蚁的寻路
%更新信息素
Delta_Tau=zeros(N,N);%初始化信息素增量，N是像素个数
   for m=1:M     %M是蚂蚁数
     if PL(k,m)   % 本次迭代的m只蚂蚁的路径长度
        ROUT=ROUTES{k,m}; %ROUT取本次迭代的所有蚂蚁的行走路线
        TS=length(ROUT)-1;%length(ROUT)返回ROUT的列数，即蚂蚁个数
                          %少最后一只蚂蚁的路线，是因为后面需要y=ROUT(s+1)
        PL_km=PL(k,m); %PL_km取本次迭代当前蚂蚁的路径长度
        for s=1:TS   %s为蚂蚁个数
          x=ROUT(s); %前一条路径
          y=ROUT(s+1); %后一条路径
          Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km; %PL_km为本次迭代的当前蚂蚁所走过的路径长度
          Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km; 
        end 
     end 
  end 
Tau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分。
                           %当所有蚂蚁寻路完成后，各路径上的信息素进行更新？？？
end%本次迭代完成，返回进行下一次迭代

这样蚁群寻路的程序就完成了，下面就是Matlab的画图部分。

4、绘图的基本步骤为：

创建图像窗口→添加坐标轴→添加栅格→设置填充图框→转换坐标→绘制路线
（1）输出程序运行时间
（2）绘制收敛曲线
（3）绘制行走路线图
（1）

%输出运行时间及路径长度
fprintf('processing time=%d \n', toc);%输出处理时间processing time=，%d对应于toc的输出时间，\n回车换行
                                      %tic与toc成对使用，计算程序运行时间
                                      %fprintf(fid,format,variables)
plotif=1;%是否绘图的控制参数
if plotif==1 %绘收敛曲线
   minPL=zeros(K); %K为迭代次数
   %选出每次迭代的最短路径
   for i=1:K 
     PLK=PL(i,:); %取第i次迭代的所有路径长度
                  %PL为K*M的矩阵，记录每次迭代中每只蚂蚁行走的路径长度
     Nonzero=find(PLK); %提取本次迭代路径长度不为0的索引号存储至Nonzero
     PLKPLK=PLK(Nonzero); %将路径长度不为0的路径索引号对应的路径长度存储至PLKPLK
     minPL(i)=min(PLKPLK); %选出本次迭代最短路径存储至minPL的相应迭代位置
   end
   Pathlength=min(minPL(1));%在所有迭代中选出最短路径
   fprintf('processing PathLength=%d \n', Pathlength);% 输出最短路径 

(2)

%绘制“收敛曲线变化趋势”图，将每次迭代的最短路径放在图中表示
   figure(1) %生成图形窗口
   plot(minPL); %plot生成的图横坐标为1至矩阵行数，纵坐标为0到矩阵中最大的数，矩阵中的每一列对应坐标中的一条曲线
   hold on %保持当前图
   grid on %加栅格
   title('收敛曲线变化趋势'); 
   xlabel('迭代次数'); 
   ylabel('最小路径长度');
   end

(3)

figure(2) %建立图形窗口
   axis([0,MM,0,MM]) %设置图的横纵坐标，MM为地图矩阵的行数或列数
   for i=1:MM 
      for j=1:MM 
       %此处的行纵坐标从0开始，为对应于矩阵地图从1开始且pcolor加了一行，所以此处横纵坐标要换算
       %具体操作之后在优化
         if G(i,j)==1           %0是黑色代表障碍，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],[1,1,1]); %将1234点所围成的图形进行白色填充，fill为颜色填充
            hold on 
         else 
            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 on 
           set(gca,'YDir','reverse');%地图的矩阵的行列与正常坐标轴的行列一致
                                   % set(gca,'propertyname','propertyvalue'......)命令可以调整图形的坐标属性
                                   % ydir：y轴方向；reverse：反转
         end
      end
   end
   hold on
   title('机器人运动轨迹'); 
   xlabel('坐标x'); 
   ylabel('坐标y');
   ROUT=ROUTES{mink,minl}; %ROUT取最短行走路线，mink为该路线的迭代号,minl为该路线的蚂蚁号
   LENROUT=length(ROUT); %提取路线长度，即走了几个栅格
   Rx=ROUT; %Rx与Ry中分别存储具体的该路线
   Ry=ROUT; 
%将该路线的栅格索引号转换为横纵坐标
   for ii=1:LENROUT 
     Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); 
     if Rx(ii)==-0.5 
        Rx(ii)=MM-0.5; 
     end 
     Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); 
   end 
plot(Rx,Ry) %绘制路线

以上为蚁群静态路径规划的所有matlab代码及解释。
生成行走路线的地图有些问题，之后在稍做修改，但此处并不影响其运行。