【路径规划】基于matalb遗传算法机器人栅格地图路径规划【含Matlab源码 175期】

一、遗传算法及栅格地图简介

1.1 引言

1.2 遗传算法理论

1.2.1 遗传算法的生物学基础

1.2.2 遗传算法的理论基础

1.2.3 遗传算法的基本概念

1.2.4 标准的遗传算法

1.2.5 遗传算法的特点

1.2.6 遗传算法的改进方向

1.3 遗传算法流程

1.4 关键参数说明

2 栅格地图

2.1 栅格法应用背景

路径规划时首先要获取环境信息， 建立环境地图， 合理的环境表示有利于建立规划方法和选择合适的搜索算法，最终实现较少的时间开销而规划出较为满意的路径。一般使用栅格法在静态环境下建立环境地图。

2.2 栅格法实质

将AGV的工作环境进行单元分割， 将其用大小相等的方块表示出来，这样栅格大小的选取是影响规划算法性能的一个很重要的因素。栅格较小的话，由栅格地图所表示的环境信息将会非常清晰，但由于需要存储较多的信息，会增大存储开销，同时干扰信号也会随之增加，规划速度会相应降低，实时性得不到保证；反之，由于信息存储量少，抗干扰能力有所增强，规划速随之增快，但环境信息划分会变得较为模糊，不利于有效路径的规划。在描述环境信息时障碍物所在区域在栅格地图中呈现为黑色，地图矩阵中标为1，可自由通行区域在栅格地图中呈现为白色，地图矩阵中标为0。路径规划的目的就是在建立好的环境地图中找到一条最优的可通行路径，所以使用栅格法建立环境地图时，栅格大小的合理设定非常关键。

2.3 10乘10的静态环境地图

10乘10的静态环境地图代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境地图%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DrawMap(map)
n = size(map);
step = 1;
a = 0 : step :n(1);
b = 0 : step :n(2);
figure(1)
axis([0 n(2) 0 n(1)]); %设置地图横纵尺寸
set(gca,'xtick',b,'ytick',a,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
hold on
r = 1;
for(i=1:n(1))         %设置障碍物的左下角点的x,y坐标
    for(j=1:n(2))
        if(map(i,j)==1)
            p(r,1)=j-1;
            p(r,2)=i-1;
            fill([p(r,1) p(r,1) + step p(r,1) + step p(r,1)],...
                 [p(r,2) p(r,2) p(r,2) + step p(r,2) + step ],'k');
            r=r+1;
            hold on
        end
    end
end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%栅格数字标识%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_text = 1:1:n(1)*n(2); %产生所需数值.
for i = 1:1:n(1)*n(2)
    [row,col] = ind2sub([n(2),n(1)],i);
    text(row-0.9,col-0.5,num2str(x_text(i)),'FontSize',8,'Color','0.7 0.7 0.7');
end
hold on
axis square

建立环境矩阵，1代表黑色栅格，0代表白色栅格，调用以上程序，即可得到上述环境地图。

map=[0 0 0 1 0 0 1 0 0 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 1 0 0 0 1 1 0 0;
     0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 1 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 0 1 0 0 0 0 0 0;
     1 1 1 0 0 0 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;];
     DrawMap(map);         %得到环境地图

2.4 栅格地图中障碍栅格处路径约束

移动体栅格环境中多采用八方向的移动方式，此移动方式在完全可通行区域不存在运行安全问题，当

移动体周围存在障碍栅格时此移动方式可能会发生与障碍物栅格的碰撞问题，为解决此问题加入约束

条件，当在分别与障碍物栅格水平方向和垂直方向的可行栅格两栅格之间通行时，禁止移动体采用对

角式移动方式。

约束条件的加入，实质是改变栅格地图的邻接矩阵，将障碍栅格(数字为“1”的矩阵元素)的对角栅格

设为不可达， 即将对角栅格的距离值改为无穷大。其实现MATLAB代码如下：

代码：

%约束移动体在障碍栅格对角运动
%通过优化邻接矩阵实现
%%%%%%%%%%%%%%%%%% 约束移动体移动方式 %%%%%%%%%%%%%%%%%
function W=OPW(map,W)
% map 地图矩阵  % W 邻接矩阵
n = size(map);
num = n(1)*n(2);
for(j=1:n(1))
    for(z=1:n(2))
       if(map(j,z)==1)
          if(j==1)                  %若障碍物在第一行
             if(z==1)               %若障碍物为第一行的第一个
                W(j+1,j+n(2)*j)=Inf;
                W(j+n(2)*j,j+1)=Inf;
             else
                if(z==n(2))         %若障碍物为第一行的最后一个
                   W(n(2)-1,n(2)+n(1)*j)=Inf;
                   W(n(2)+n(1)*j,n(2)-1)=Inf;
                else                %若障碍物为第一行的其他
                    W(z-1,z+j*n(2))=Inf;
                    W(z+j*n(2),z-1)=Inf;
                    W(z+1,z+j*n(2))=Inf;
                    W(z+j*n(2),z+1)=Inf;
                end
             end
          end
          if(j==n(1))               %若障碍物在最后一行
             if(z==1)               %若障碍物为最后一行的第一个
                W(z+n(2)*(j-2),z+n(2)*(j-1)+1)=Inf;
                W(z+n(2)*(j-1)+1,z+n(2)*(j-2))=Inf;
             else
             if(z==n(2))            %若障碍物为最后一行的最后一个
                W(n(1)*n(2)-1,(n(1)-1)*n(2))=Inf;
                W((n(1)-1)*n(2),n(1)*n(2)-1)=Inf;
             else                   %若障碍物为最后一行的其他
                W((j-2)*n(2)+z,(j-1)*n(2)+z-1)=Inf;
                W((j-1)*n(2)+z-1,(j-2)*n(2)+z)=Inf;
                W((j-2)*n(2)+z,(j-1)*n(2)+z+1)=Inf;
                W((j-1)*n(2)+z+1,(j-2)*n(2)+z)=Inf;
             end
             end
          end
          if(z==1)              
             if(j~=1&&j~=n(1))       %若障碍物在第一列非边缘位置 
                W(z+(j-2)*n(2),z+1+(j-1)*n(2))=Inf;
                W(z+1+(j-1)*n(2),z+(j-2)*n(2))=Inf;
                W(z+1+(j-1)*n(2),z+j*n(2))=Inf;
                W(z+j*n(2),z+1+(j-1)*n(2))=Inf;
             end
          end
         if(z==n(2))
            if(j~=1&&j~=n(1))         %若障碍物在最后一列非边缘位置 
               W((j+1)*n(2),j*n(2)-1)=Inf;
               W(j*n(2)-1,(j+1)*n(2))=Inf;
               W(j*n(2)-1,(j-1)*n(2))=Inf;
               W((j-1)*n(2),j*n(2)-1)=Inf;
            end
         end
         if(j~=1&&j~=n(1)&&z~=1&&z~=n(2))   %若障碍物在非边缘位置
            W(z+(j-1)*n(2)-1,z+j*n(2))=Inf;
            W(z+j*n(2),z+(j-1)*n(2)-1)=Inf;
            W(z+j*n(2),z+(j-1)*n(2)+1)=Inf;
            W(z+(j-1)*n(2)+1,z+j*n(2))=Inf;
            W(z+(j-1)*n(2)-1,z+(j-2)*n(2))=Inf;
            W(z+(j-2)*n(2),z+(j-1)*n(2)-1)=Inf;
            W(z+(j-2)*n(2),z+(j-1)*n(2)+1)=Inf;
            W(z+(j-1)*n(2)+1,z+(j-2)*n(2))=Inf;
         end
       end
     end
   end
end

2.5 栅格法案例

下面以Djkstra算法为例， 其实现如下：

map=[0 0 0 1 0 0 1 0 0 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 1 0 0 0 1 1 0 0;
     0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 1 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 0 1 0 0 0 0 0 0;
     1 1 1 0 0 0 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境矩阵map%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DrawMap(map); %得到环境地图
W=G2D(map);   %得到环境地图的邻接矩阵
W(W==0)=Inf;  %邻接矩阵数值处理
W=OPW(map,W); %优化邻接矩阵
[distance,path]=dijkstra(W,1,100);%设置起始栅格，得到最短路径距离以及栅格路径
[x,y]=Get_xy(distance,path,map);   %得到栅格相应的x，y坐标
Plot(distance,x,y);   %画出路径

运行结果如下：

其中函数程序：

DrawMap(map) 详见建立栅格地图

W=G2D(map) ； 详见建立邻接矩阵

[distance， path] =dijkstra(W， 1， 100) 详见Djk stra算法

[x， y] =Get_xy(distance， path， map) ；

Plot(distance， x， y) ；

二、部分源代码

% 基于遗传算法的栅格法机器人路径规划的MATLAB实现
clc;
clear;
% 输入数据,即栅格地图
G=  [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 0 0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 1 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 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 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 1 1 1 0 1 0 1 1 0 0 0 0 0 0;
     0 1 1 1 0 0 0 0 0 0 1 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 1 1 0 1 1 1 1 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 1 1 1 0 1 1 1 1 0;
     0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
     0 0 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0;
     0 0 1 1 0 0 1 1 1 0 1 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 1 1 1 0 1 0 0 0 0 0 0 1 1 0; 
     0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 1 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];
p_start = 0;        % 起始序号
p_end = 399;        % 终止序号
NP = 200;           % 种群数量
max_gen = 50;       % 最大进化代数
percross = 0.8;     % 交叉概率
permutate = 0.2;    % 变异概率
perlenth = 1;       % 路径长度比重
persmooth = 7;      % 路径顺滑度比重
%init_path = [];
z = 1; 
new_pop1 = {}; % 元胞类型数组，啥都可以存，类似C语言中的结构
new_pop2 = {};
[y, x] = size(G); % x = 20, y = 20，行数和列数
% 起点所在列（从左到右编号1.2.3...）
xstart = mod(p_start, x) + 1; 
% 起点所在行（从上到下编号行1.2.3...）
ystart = fix(p_start / x) + 1;
% 终点所在列、行
xend = mod(p_end, x) + 1; % mod函数取余函数，前除以后取余数
yend = fix(p_end / x) + 1; % fix函数取整函数，四舍五入取整数

% 种群初始化第一步，必经节点,从起始点所在行开始往上，在每行中挑选一个自由栅格，构成必经节点
pass_num = yend - ystart + 1; % 等于地图总共的行数，表示
pop = zeros(NP, pass_num); % pop初始化置零，等于最大迭代次数*地图总行数的矩阵，表示所有可能的路径
for global1_i = 1 : NP
    pop(global1_i, 1) = p_start; % 循环NP次，把起点放入pop首列
    global1_j = 1;
    for global_iyk = ystart+1 : yend-1 % 除去起点和终点
        global1_j = global1_j + 1;
        can = []; % 每一行无障碍的点组成can数组
        for global_ixk = 1 : x
            global_ino = (global_ixk - 1) + (global_iyk - 1) * x; % 给每个栅格编序号
            if G(global_iyk, global_ixk) == 0
                can = [can global_ino]; % 把无障碍的点加入can数组中
            end
        end
        can_num = length(can);  % can数组的长度
        index = randi(can_num); % 产生随机的整数
        pop(global1_i, global1_j) = can(index); % 每一行表示的一条路径中加入一个无障碍的点
    end
    pop(global1_i, end) = p_end; % 最后把终点放入pop最后一列

    % 种群初始化第二步，将上述必经节点联结成无间断路径
    % single_new_pop = generate_continuous_path(pop(i, :), G, x);
    %@@@@@@@@@@ 将上述随机的点连接成连续路径
    generate_i = 1;
    single_new_pop = pop(global1_i,:); % 取pop矩阵第global1_i行而不是第generate_i行所有数据，表示取出一条路径
    [~, single_path_num] = size(single_new_pop);
    while generate_i ~= single_path_num
        value_x_now = mod(single_new_pop(1, generate_i), x) + 1;  % 当前点所在列
        value_y_now = fix(single_new_pop(1, generate_i) / x) + 1; % 当前点所在行
        value_x_next = mod(single_new_pop(1, generate_i + 1), x) + 1;  % 下一个点所在列
        value_y_next = fix(single_new_pop(1, generate_i + 1) / x) + 1; % 下一个点所在行
        max_iteration = 0; % 初始化平滑路径最大迭代次数
        while max(abs(value_x_next - value_x_now), abs(value_y_next - value_y_now)) ~= 1 % 判断点generate_i和generate_i+1是否连续（挨在一起）,若否就插入中间点
            x_insert = floor((value_x_next + value_x_now) / 2);
            y_insert = floor((value_y_next + value_y_now) / 2);
            if G(y_insert, x_insert) == 0 % 当插入栅格为自由栅格（不在障碍物上）时
                num_insert = (x_insert - 1) + (y_insert - 1) * x; % 将栅格位置转化为序号
                single_new_pop = [single_new_pop(1, 1:generate_i), num_insert, single_new_pop(1, generate_i+1:end)]; % 将栅格序号插入single数组里        
            else % 插入栅格为障碍物栅格(在障碍物上）时
                % 往左走
                if G(y_insert, x_insert - 1) == 0 && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, generate_i)) && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, generate_i+1))
                    x_insert = x_insert - 1;
                    num_insert = (x_insert - 1) + (y_insert - 1) * x; % 将栅格位置转化为序号
                    single_new_pop = [single_new_pop(1, 1:generate_i), num_insert, single_new_pop(1, generate_i+1:end)]; % 将栅格序号插入single数组里               
                % 往右走    
                elseif G(y_insert, x_insert + 1) == 0 && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, generate_i)) && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, generate_i+1))
                    x_insert = x_insert + 1;
                    num_insert = (x_insert - 1) + (y_insert - 1) * x; % 将栅格位置转化为序号
                    single_new_pop = [single_new_pop(1, 1:generate_i), num_insert, single_new_pop(1, generate_i+1:end)]; % 将栅格序号插入single数组里                
                % 向上走
                elseif G(y_insert + 1, x_insert) == 0 && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, generate_i)) && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, generate_i+1))
                    y_insert = y_insert + 1;
                    num_insert = (x_insert - 1) + (y_insert - 1) * x; % 将栅格位置转化为序号
                    single_new_pop = [single_new_pop(1, 1:generate_i), num_insert, single_new_pop(1, generate_i+1:end)]; % 将栅格序号插入single数组里
                % 向下走
                elseif  G(y_insert - 1, x_insert) == 0 && ((x_insert - 1) + (y_insert - 2) * x ~= single_new_pop(1, generate_i)) && ((x_insert - 1) + (y_insert-2) * x ~= single_new_pop(1, generate_i+1))
                    y_insert = y_insert - 1;
                    num_insert = (x_insert - 1) + (y_insert - 1) * x; % 将栅格位置转化为序号
                    single_new_pop = [single_new_pop(1, 1:generate_i), num_insert, single_new_pop(1, generate_i+1:end)]; % 将栅格序号插入single数组里
                % 其他情况舍去此路径
                else                
                    single_new_pop = [];
                    break
                end    
            end        
            value_x_next = x_insert;
            value_y_next = y_insert;
            max_iteration = max_iteration + 1;
            if max_iteration > 20000 % 最多执行20000次，是为平滑路径最大迭代次数
                single_new_pop = [];
                break
            end    
        end    
        if isempty(single_new_pop)
            break
        end
        [~, single_path_num] = size(single_new_pop);
        generate_i = generate_i + 1;
    end
    if ~isempty(single_new_pop)
       new_pop1(z, 1) = {single_new_pop};
        z = z + 1;
    end
end

三、运行结果

四、matlab版本及参考文献

1 matlab版本

2014a

2 参考文献

《智能优化算法及其MATLAB实例（第2版）》包子阳 余继周 杨杉著 电子工业出版社