foreword

This series will be used to record and share the learning process of related algorithms based on grid map planning in path planning. This article is mainly based on Matlab's two-dimensional and three-dimensional grid map creation. Which should declare:

The creation part of the two-dimensional grid map is:

Gu Yueju ~ A Guide to Robot Path Planning Algorithms Based on Grid Maps • Li Wanhong's course study notes, which are convenient for future consolidation and review. This tutorial is very good, and it is worth recommending! Simultaneous path planning (1): Use Matlab to quickly draw a grid map. This article records in detail the creation process of the two-dimensional grid map in the course. In order to avoid repeated wheel creation, this article does not describe the details here. You can Check out this excellent blogger's post first.

The creation of the three-dimensional space grid is partly for the individual to expand and analyze on the basis of two-dimensional learning, so as to facilitate the subsequent use of path planning in three-dimensional space. At the same time, in the part of the creation function of the three-dimensional grid point, I learned from Zhihu: MATLAB-based three-dimensional grid drawing

1. Creation of two-dimensional grid map

1.1. Construction principle of 2D grid map

The two-dimensional grid map is constructed using the image () function, and the matrix is converted to an image to realize the conversion between digital and image visualization. The position of each element in the matrix represents the coordinates of the grid point, and the element value corresponds to the relevant feature . Different situations in path planning (obstacles, starting point, end point, planned path and final path) are represented by a pre-set colormap. In order to facilitate the operation of the path planning algorithm, it is necessary to use the function conversion of the linear index and the rectangular coordinate, and finally realize the detailed adjustment of the visualization through the image handle.

1.2. Two-dimensional grid map construction routine

% 基于栅格地图的机器人路径规划算法
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;

Please add a picture description

2. Creation of 3D grid map

2.1. Construction principle of 3D grid map

On the basis of the creation of the above-mentioned two-dimensional grid map, the creation of the three-dimensional grid map is not realized by the function image(), but by the Patch function to create and display the space body , create pointCreat(x,y,z, color, alphaValue) function realizes this function mesh() function realizes the construction of spatial grid . At the same time, the three-dimensional Cartesian coordinates represent the position of the midpoint of the path planning. What needs to be explained here is that each three-dimensional coordinate point is created as the lower left vertex of the three-dimensional space body. Similarly, here I created the function DtranTo1D(xmax,ymax,zmax,x , y, z), DtranTo3D(xmax, ymax, zmax, b) realize conversion of three-dimensional coordinates to linear index and linear index to three-dimensional coordinate respectively.

2.2 3D grid map construction routine

% 基于三维栅格地图的机器人路径规划算法
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