基于栅格地图的路径规划（一）基于Matlab二维、三维栅格地图的构建

前言

这个系列将会用来记录和分享关于路径规划中基于栅格地图规划的相关算法学习过程，本文主要是基于Matlab的二维、三维栅格地图创建。其中应该声明的是：

1. 二维栅格地图的创建部分内容为：

• 古月居~基于栅格地图的机器人路径规划算法指南 • 黎万洪 课程学习的笔记，方便自己日后的巩固与复习，这个教程讲的很好，值得推荐！同时路径规划（一）：使用Matlab快速绘制栅格地图这篇文章较为详细的记录了课程中二维栅格图的创建过程，为了避免重复造轮子，本文对此处不做细节描述，大家可以先看这位优秀博主的帖子。

2. 三维空间栅格的创建部分为个人在二维学习基础上拓展分析而来，以便后续三维空间的路径规划使用。同时，在三维格点的创建函数部分借鉴了知乎：基于MATLAB的三维网格绘制

1、二维栅格地图的创建

1.1、二维栅格地图构建原理

二维栅格地图的构建利用image()函数，矩阵转图像实现数字与图像可视化之间的转化，矩阵中每元素的位置代表栅格点的坐标，元素值则对应相关的特征。通过预先设定好的colormap表征路径规划中不同的情况（障碍、起点、终点、规划中的路径以及最终的路径）。为了便于路径规划算法的运行，需要用到线性索引与直角坐标的函数转化，最后通过图像句柄实现可视化的细节调整。

1.2、二维栅格地图构建例程

% 基于栅格地图的机器人路径规划算法
clc
clear
close all

%% 构建颜色MAP图
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);

%% 构建栅格地图场景
% 栅格界面大小:行数和列数
rows = 20;
cols = 20; 

% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);

% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

% 起始点和目标点
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;

%% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;

2、三维栅格地图的创建

2.1、三维栅格地图构建原理

在上述二维栅格地图的创建基础上，三维栅格的地图的创建并不利用函数image()进行实现，而是通过Patch函数进行空间体的创建显示，创建pointCreat(x,y,z,color,alphaValue)函数实现此功能mesh()函数实现空间网格的构建。同时，三维直角坐标表示路径规划中点的位置，这里需要说明的一点是每个三维坐标点作为三维空间体的左下顶点实现创建，同样的这里我创建了函数DtranTo1D(xmax,ymax,zmax,x,y,z)、DtranTo3D(xmax,ymax,zmax,b)分别实现三维坐标转线性索引、线性索引转三维坐标。

2.2 三维栅格地图构建例程

% 基于三维栅格地图的机器人路径规划算法
clc
clear
close all

%% 构建栅格地图场景

%构建颜色MAP图
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);
% 栅格界面大小:行数和列数
rows = 10;
cols = 10; 
heigh=10;

x=1:rows+1;
y=1:cols+1;
z=1:heigh+1;

[X,Y]=meshgrid(x,y);
Z=ones(size(X));
camp1=[0.7 0.7 0.7];
colormap(camp1);
for(i=1:length(z))
    colormap(camp1);
    a=mesh(X,Y,i*ones(size(X)));
    a.FaceAlpha=0;
    hold on;
    b=mesh(i*ones(size(X)),X,Y);
    b.FaceAlpha=0;
end
% 起点信息
[xstar,ystar,zstar]=DtranTo3D(rows,cols,heigh,1);
pointCreat(xstar,ystar,zstar,cmap(4,:),1);
% 终点信息
goalPos=rows*cols*heigh-2;
[xend,yend,zend]=DtranTo3D(rows,cols,heigh,goalPos);
pointCreat(xend,yend,zend,cmap(3,:),1);
% 障碍物区域
obsRate = 0.05;
obsNum = floor(rows*cols*heigh*obsRate);
startPos=1;
obsIndex = randi([2,rows*cols*heigh-3],obsNum,startPos);
for(i=1:length(obsIndex))
    [xobs,yobs,zobs]=DtranTo3D(rows,cols,heigh,obsIndex(i));
    pointCreat(xobs,yobs,zobs,cmap(2,:),1);
end
%显示区域设置
xlim([0,rows+1]);
ylim([0,cols+1]);
zlim([0,heigh+1]);

function [x,y,z]=DtranTo3D(xmax,ymax,zmax,b)

jishu=1;
for(i=1:xmax)
    for(j=1:ymax)
        for(k=1:zmax)
            index(jishu,1)= DtranTo1D(xmax,ymax,zmax,i,j,k);
            index(jishu,2)= i;
            index(jishu,3)= j;
            index(jishu,4)= k;
            jishu=jishu+1;
        end
    end
end

x=index(find(index(:,1)==b),2);
y=index(find(index(:,1)==b),3);
z=index(find(index(:,1)==b),4);
end

function b=DtranTo1D(xmax,ymax,zmax,x,y,z)
a=sub2ind([xmax,ymax],x,y);
b=a+(z-1)*xmax*zmax;
end
  
function pointCreat(x,y,z,color,alphaValue)
z=z+1;

points=[x,y,z;

x+1,y,z;

x+1,y+1,z;

x,y+1,z;

x,y,z-1;

x+1,y,z-1;

x+1,y+1,z-1;

x,y+1,z-1;];%节点信息

mesh=[1,2,3,4,5,6,7,8];%网格信息

for i=1:length(mesh(:,1))%绘图

%六面体单元结点坐标

vertices_matrix = [points(mesh(i,:),1),points(mesh(i,:),2),points(mesh(i,:),3)];

%六面体单元结点顺序

faces_matrix=[1 2 3 4;2 6 7 3;6 5 8 7;5 1 4 8; 4 3 7 8; 5 6 2 1];%给出每个面节点序号，顺时针或者逆时针排列

h=patch('vertices', vertices_matrix,'faces',faces_matrix,'facecolor',color);
h.FaceAlpha=alphaValue;
hold on%绘图

end
axis equal
end