基于蚁群算法的栅格地图路径规划-考虑机器人体积

基于蚁群算法的栅格地图路径规划-考虑机器人体积
**

一、基础栅格地图路径规划*

clc;clear all;
G=[0 0 1 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 0 0 0 0 1 1 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 0 0 0 0 1 0 0 1 0 0 0 0; 
   0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0;
   0 1 1 1 0 0 0 0 1 1 0 0 0 1 0 1 0 0 0 0; 
   0 1 1 0 0 0 0 0 0 0 0 0 1 1 0 1 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 0 0 0 1 1 0 0 0 0 0 0; 
   0 0 0 0 1 1 0 0 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 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; 
   0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; 
   0 0 1 1 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 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;];


MM=size(G,1);                 % G 地形图为01矩阵，如果为1表示障碍物 ，size()返回矩阵G的行数，MM=20。如果size中的1改为2，则返回矩阵G的列数
Tau=ones(MM*MM,MM*MM);        % Tau 初始信息素矩阵，规模为400*400,。记录每个栅格到其他栅格的信息素量。ones产生一个全是1的矩阵
Tau=8.*Tau;                   %矩阵全为8
K=100;                        %迭代次数（指蚂蚁出动多少波）
M=50;                         %蚂蚁个数
S=1;                         %最短路径的起始点，第一个栅格点为起始地，这里用1-400的序号代表点的位置，默认左上角是第一个
E=400;                         %最短路径的目的点
Alpha=1;                      % Alpha 表征信息素重要程度的参数
Beta=8;                       % Beta 表征启发式因子重要程度的参数
Rho=0.4;                     % Rho 信息素蒸发系数
Q=1;                          % Q 信息素增加强度系数 
minkl=inf;                    %inf代表无穷大量
mink=1; 
minl=1; 
D=G2D(G);                    %G2D栅格矩阵转换成邻接矩阵的函数，D用于存储每个点到各自相邻无障碍栅格点的代价值（该点到其他点的可行性）

N=size(D,1);                 %N表示问题的规模（像素个数），N=400
a=1;                         %小方格像素的边长
Ex=a*(mod(E,MM)-0.5);        %终止点横坐标，这一行E=-0.5。这是栅格地图的直角坐标表示法和序号表示法的映射关系。
 
if Ex==-0.5 
  Ex=MM-0.5;                 %这一行Ex=19.5
end 

Ey=a*(MM+0.5-ceil(E/MM));    %终止点纵坐标，栅格地图的直角坐标表示法和序号表示法的映射关系。
 Eta=zeros(N);               %启发式信息矩阵，取为至目标点的直线距离的倒数，N=400，Eta大小为400*400
 %以下启发式信息矩阵
 %下面的for循环就是得到每个栅格点的坐标，并构建启发式信息矩阵（每个栅格点到目标点的距离）
 for i=1:N 
 ix=a*(mod(i,MM)-0.5);       %mod求i除以MM的余数
   if ix==-0.5  
   ix=MM-0.5;                %得到每一个栅格点的横坐标，一共400个
   end 
iy=a*(MM+0.5-ceil(i/MM));    %得到每一个栅格点的纵坐标，一共400个，这里ix，iy都是有400个元素的向量
   if i~=E                   %~=不等号，E代表目标点这里是400
   Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;%计算该点到目标的距离长度，并输入到启发式信息矩阵Eta，一共400个
   else 
   Eta(i)=100; 
   end 
 end 
%到这里，步骤2完成
 
ROUTES=cell(K,M);     %用细胞结构存储每一代的每一只蚂蚁的爬行路线。K迭代次数，M蚂蚁个数。cell类型每个单元可以储存任何数据
                      %把ROUTES初始化为一个K行M列的空cell类型数据
                      %比如ROUTES{1,1}=rand(5)，就是ROUTES的1行1列的单元中存储的就是一个随机的5×5的方阵
PL=zeros(K,M);         %用矩阵存储每一代的每一只蚂蚁的爬行路线长度

%启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁
for k=1:K 
for m=1:M 
%状态初始化
W=S;                  %当前节点初始化为起始点，S=1是最短路径起始点，W用于表示当前所在节点
Path=S;                %爬行路线初始化
PLkm=0;               %爬行路线长度初始化
TABUkm=ones(N);       %禁忌表初始化，N=400
TABUkm(S)=0;          %已经在初始点了，因此要排除
DD=D;                 %邻接矩阵初始化，DD是400*400大小

%下一步可以前往的节点，蚂蚁走出第一步
DW=DD(W,:);           %取行操作，取矩阵DD第W行的全部元素，矩阵DD的第一行代表第一个栅格点的邻接矩阵（也就是第一个栅格点能走的下一个点存储在第一行）
                       %第二行就是存储第二个栅格点到其他栅格点的可行性，DW大小是400*1
DW1=find(DW);         %find函数，返回向量DW中非0元素的位置（就是非0元素是第几个数）
                       %DW1就得到了所以可以去的栅格点的序号，这样就筛选出当前可选择的点
                       
for j=1:length(DW1) 
   if TABUkm(DW1(j))==0    %将可选择的点与禁忌表对比，如果这个点在禁忌表在是0，则这个点不能走
      DW(DW1(j))=0;        %更新这一行的邻接矩阵（蚂蚁当前所在点与其他所有点的邻