《Motion Planning》【0】格栅地图绘制

个人学习使用仅供参考

1.Function_map.m

%Function_map返回的是一个随机的格栅地图 ipnut格式为(行， 列，开始的位置--可以看成是一个vector后面同理，终点位置， 障碍物的位置)
%返回的是一个设置好上述参数的map
function field = Function_map(rows, cols, start_sub, end_sub)
field = ones(rows, cols);

%起始点
field(start_sub(1), start_sub(2)) = 4; % start_sub(1), start_sub(2)是起始点的行、列存到start_sub（容器）中，4对应的起始点的颜色

%终点
field(end_sub(1), end_sub(2)) = 5; % end_sub(1), end_sub(2)是终点的行列、存到end_sub（容器）中，5对应的起始点的颜色

%障碍物写法1 这里有个问题就是当传入9 13 14的时候10也会被填充
%field(obs_sub(:, 1), obs_sub(:, 2)) = 2; % 把障碍物的行存到obs_sub数组中的第1列，把障碍物的列存到obs_sub数组中的第2列, 设置对应的颜色2黑色

%障碍物写法2
%随机障碍物
rand_r = randi([1, rows], 100, 1);
rand_c = randi([1, cols], 100, 1);
obs_sub = [rand_r, rand_c];
obs_r = obs_sub(:, 1);
obd_c = obs_sub(:, 2);
obs_ind = sub2ind([rows cols], obs_r, obd_c);
field(obs_ind) = 2;
%固定障碍物
%obs_row = obs_sub(:, 1);
%obs_col = obs_sub(:, 2);
%obs = sub2ind([rows cols], obs_row, obs_col);
%field(obs) = 2;

%第一列    第二列
%  2         3      这是第1个obs_sub坐标(2, 3) 对应的ind坐标10
%  3         3      这是第2个obs_sub坐标(3, 3) 对应的ind坐标11
%  4         3      这是第3个obs_sub坐标(4, 3) 对应的ind坐标12

%************************************************构建数组的栅格图************************************************
%这里的所有不能放在上面
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-青色-动态规划的路径
        0 0 1];              % 8-蓝色

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

% 绘制图像
image(1.5, 1.5, field);
grid on;
hold on;
set(gca, 'gridline', '-', 'gridcolor', 'k', 'linewidth', 2, 'GridAlpha', 0.5);
set(gca,'xtick', 1:cols + 1, 'ytick', 1:rows + 1);
set(gca, 'XAxisLocation', 'top')
axis image;

% 散点图可以当做是动态障碍物
%scatter(1.5, 3.5, 300, 'filled', MarkerEdgeColor='blue', MarkerFaceColor='red');

end

2.Test_main.m

clc;
clear;
close all;
rows = 20; % 行
cols = 20; % 列
sz = [rows cols];
[startpos(1), startpos(2)] = ind2sub(sz, 20); % 起始点位置
[endpos(1), endpos(2)] = ind2sub(sz, 381); % 终点位置
%[obspos(:, 1), obspos(:, 2)] = ind2sub(sz, [27 47 28 45 45 59 57 70 86 89]); % 障碍物位置
%field = Function_map(rows, cols, startpos, endpos, obspos); % 调用function
field = Function_map(rows, cols, startpos, endpos); % 调用function

3.文件结构

4.结果（障碍物是随机的）

测试文件Test_main.m运行

 

---

OVER