路径规划基于matlab蚁群算法求解机器人栅格地图最短路径规划问题含Matlab源码 1580期

Posted 2022-01-02 紫极神光

一、简介

路径规划是实现移动机器人自主导航的关键技术，是指在有障碍物的环境中，按照一定的评价标准（如距离、时间、能耗等），寻找到一条从起始点到目标点的无碰撞路径，这里选取最短距离路径规划的评价标准，即最短路径规划问题。

1．路径规划数学模型的建立
将移动机器人周围环境用一组数据进行抽象表达，建立二维或三维的环境模型，得到移动机器人能够理解分析的环境数据，是机器人路径规划的基本前提。我这里用的是栅格法，其原理是将周围环境看成一个二维平面，将平面分成一个个等面积大小的具有二值信息的栅格，每个栅格中存储着周围环境信息量，下图我给出了一个栅格法地图，方便大家更好的理解栅格地图。这里设计的栅格地图为一个20×20的地形矩阵，黑色的地方表示有障碍，白色的地方表示没有障碍。

图1　栅格法地图
在用栅格法建立环境模型时，为了将环境信息转换成移动机器人可以识别的数据，一般采用序号法标记环境地图信息，即将栅格地图中一个个栅格从序号１依次累加直到标记到最后一个栅格。如图2所示。


图3　八叉树搜索策略
那么，怎么判断一个栅格点是否为另一个栅格点的相邻栅格点呢，另外，又怎么判断是否为有障碍栅格呢。这就需建立矩阵D，记录每个栅格点至其相邻栅格点的代价值。本例中栅格地图有20×20个栅格点，则D的大小为400×400，其中列是起点栅格，行是局部终点栅格，各栅格点至其各相邻无障碍栅格点的代价值非零，而有障碍栅格及非相邻栅格设为0。

2．机器人最短路径规划的实现步骤
蚁周模型实现机器人最短路径规划的流程图

为了方便大家更好地理解蚁群算法的原理及实现过程，其流程图如图4所示。（流程图较长，我截图了两段。）


图4　基于蚁群算法的机器人最小路径规划流程图
图中公式（3）（4）的具体表达在下边的具体步骤里。

蚁周模型实现机器人最短路径规划的具体步骤

**步骤1：**给出栅格地图的地形矩阵；初始化信息素矩阵 Tau（记录每个栅格至其他栅格的信息素量），最大迭代次数K，蚂蚁个数M，表征信息素重要程度的参数 、表征启发式信息重要程度的参数 ，信息素蒸发系数 ，信息素增加强度系数Q及启发式信息矩阵
**步骤2：**构建启发式信息矩阵。按式（1）和式（2）计算每个栅格至目标点的距离，启发式信息素取为至目标点距离的倒数，距离越短，启发式因子越大，障碍物处的启发式信息为0。建立矩阵D，用以存储每个栅格点至各自相邻无障碍栅格点的代价值。
**步骤3：**对于每一只蚂蚁，初始化蚂蚁爬行的路径及路径长度，将禁忌列表全部初始化为1；蚂蚁从起始点出发开始搜索路径，找出当前栅格点的所有无障碍相邻栅格点（即矩阵D中相应元素不为0的栅格点），再根据禁忌列表筛选出当前可选择的栅格点。
**步骤4：**如果起始点是目标点，且可选栅格点个数大于等于1，则根据式（3）计算蚂蚁从当前栅格点转移到各相邻栅格点的概率，

二、部分源代码

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

%地图矩阵
n=size(G,1);%n表示地图大小
m=50;    %% m 蚂蚁个数
Alpha=2;  %% Alpha 表征信息素重要程度的参数
Beta=6;  %% Beta 表征启发式因子重要程度的参数
Rho=0.1; %% Rho 信息素蒸发系数
NC_max=200; %%最大迭代次数
Q=1;         %%信息素增加强度系数
Tau=ones(n,n);     %Tau为信息素矩阵
NC=1;               %迭代计数器，记录迭代次数
r_e=1;  c_e=20;%地图终点在矩阵中的位置%可以通过position2rc函数产生
s=n;%路径起始点在矩阵中的位置
position_e=n*(n-1)+1;%路径终点在矩阵中的位置
min_PL_NC_ant=inf;%%蚂蚁最短的行进距离
min_ant=0;%%最短行进距离的蚂蚁坐标
min_NC=0;%%最短行进距离的迭代次数
% 计算邻接矩阵及启发因子%%邻接矩阵作用是计算启发因子
z=1;
for i=1:n
    for j=1:n
        if G(i,j)==0 
            D(i,j)=((i-r_e)^2+(j-c_e)^2)^0.5;
        else
            D(i,j)=inf;      %i=j时不计算，应该为0，但后面的启发因子要取倒数，用eps（浮点相对精度）表示
        end
    end
end
D(r_e,c_e)=0.05;
Eta=1./D;          %Eta为启发因子，这里设为到终点距离的倒数
Tau=10.*Eta;%%%%%创新点%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%计算移动矩阵
 D_move=zeros(n*n,8);%%D_move每一行代表与行标对应元素，可以前往的下一个节点的位置
 for point=1:n*n
     if G(point)==0
         [r,c]=position2rc(point);
         move=1;
         for k=1:n
             for m1=1:n
                 im=abs(r-k);
                 jn=abs(c-m1); 
                 if im+jn==1||(im==1&&jn==1) 
                     if G(k,m1)==0
                         D_move(point,move)=(m1-1)*n+k;
                         move=move+1;
                     end
                 end
             end
         end
     end 
 end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%移动矩阵和邻接矩阵计算完成，检查无误%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%开始迭代
routes=cell(NC_max,m);%%%%存储每次迭代每个蚂蚁的路径
PL=zeros(NC_max,m); %%%%%存储每次迭代每个蚂蚁的路径长度
while NC<=NC_max
    NC
    for ant=1:m
        current_position=s;%%%当前位置为起始点
        path=s;%%路径初始化
        PL_NC_ant=0;%%长度初始化
        Tabu=ones(1,n*n);   %%%%禁忌表，排除已经走过的位置
        Tabu(s)=0;%%排除已经走过的初始点
        D_D=D_move;%%%%D_D是D_move的中间矩阵，作用是为了不让D_move参与计算，也可不用D_D矩阵，直接用D_move
        D_work=D_D(current_position,:);%%%把当前点可以前往的写一个节点的信息传送给D_work
        nonzeros_D_work=find(D_work);%%%找到不为0的元素的位置
        for i1=1:length(nonzeros_D_work)
            if Tabu(D_work(i1))==0
                D_work(nonzeros_D_work(i1))=[];%%将禁忌表中已走过的元素删除，防止走已经走过的位置
                D_work=[D_work,zeros(1,8-length(D_work))];%%%保证D_work向量长度为8（每个点最多能往周围的8个点走），为后面for循环做准备
            end
        end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%排除走过的第一点（排除起点）%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        len_D_work=length(find(D_work));
        while current_position~=position_e&&len_D_work>=1%%当前点是否为终点或者走进死胡同
            p=zeros(1,len_D_work);
            for j1=1:len_D_work
                [r1,c1]=position2rc(D_work(j1));%%利用自己编的函数把可以前进的点计算为行列表示
                p(j1)=(Tau(r1,c1)^Alpha)*(Eta(r1,c1)^Beta);%%%%计算每个可以前往的节点的概率
            end
            p=p/sum(p);%%%归一化
            pcum=cumsum(p);%%%概率累加
            select=find(pcum>=rand);%%%%轮盘赌法选择下个节点
            to_visit=D_work(select(1));%%%前往下一个节点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%到达下一个节点%%%%%%%%%%%%%%%%%%%%%%%%
            path=[path,to_visit];%%%路径累加
            dis=distance(current_position,to_visit);%%%计算到下个节点的距离
            PL_NC_ant=PL_NC_ant+dis;%%距离累加
            current_position=to_visit;%%%当前点设为前往点
            D_work=D_D(current_position,:);%%%%把当前节点可以前往的下一个节点的信息传给D_work
            Tabu(current_position)=0;%%%禁忌表中排除已经到的点
            for kk=1:400
                if Tabu(kk)==0
                    for i3=1:8
                        if D_work(i3)==kk
                           D_work(i3)=[];%%%%排除禁忌表中已经走过的节点
                           D_work=[D_work,zeros(1,8-length(D_work))];%%保证长度为8
                        end
                    end
                end
            end
            len_D_work=length(find(D_work));%%%计算当前点可以前往的下一个节点的数量
        end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.