路径规划算法之栅格地图绘制

01  数据地图

1.1 地图类型

1.2 栅格地图表示方式

a. 本栏路径规划利用矩阵（二维数组）来表示栅格地图（因为对于矩阵，无论MATLAB、C++还是Python，矩阵更适合数组的表达，更便于编程。）

b. 矩阵表示地图与传统意识中坐标X,Y有所区别。

如图所示的地图，在矩阵表示中，起点位置为[3,1]，终点为[3,5]，三个障碍物分别为[2,3],[3,3]和[4,3]。在坐标表示中，起点位置为(1,2)，终点为(5,2)，三个障碍物分别为(3,1),(3,2)和(3,3)

c. 矩阵表示地图还可以用线性索引来简化，这样将矩阵的二维数组表示成一位数组，编程更方便。

Tips：线性索引，是从第一列开始，沿着行按顺序增长，然后从第二列开始…，是竖着沿行增长的，而非横着沿着列增长。理解记忆有点类似于Excel表格按行递增。

02  位置的表示方法的区分

2.1 在栅格地图上绘制xy点发生的情况

对于程序

 

clear;close all;
rows = 4; cols = 5;                           % 地图行列尺寸


% 设置地图属性
field = ones(rows, cols);
field(1,3) = 4;          % 栅格矩阵表示1,3位置图像


% 颜色表征矩阵
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-青色-动态规划的路径
colormap(cmap);
image(1.5,1.5,field);


% % 设置栅格属性
grid on;hold on;
set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
set(gca, 'XAxisLocation','top')
axis image;


scatter(1,3,200,'filled');  % XY绘制1,3图像，其中200是圆圈大小

‍

 

这是因为在栅格地图上的坐标系X对应于列col，而坐标Y对于与行row

2.2 三种表述位置方法的关系

下面图片是针对于Python绘图表述的，MATLAB绘图转换关系是一致的，但是MATLAB数组从1开始，而Python数组从0开始

①sub2coord和sub2xy均是仿照MATLABsub2ind命名的函数，方便理解和记忆

②MATLAB只要用到sub2ind和sub2coord即可

③sub2xy主要是正对于Python进行plt.plot绘图使用的

几个代码的转换函数见后面的模块

03  MATLAB绘制栅格地图

Tips1：这里为了方便MATLAB语言、Python语言和C++编程的统一性，统一采用按行递增的方式存放的下标特征信息数据（如障碍物坐标[row1,col1;row2,col2;row3,col3;…]），表示地图特征信息的位置==；

Tips2：当你获得线性地图特征信息的线性索引关系或者是XY坐标值，可以将其装换成线性索引值套用程序。MATLAB的数组的应用可以参考： MATLAB数组的一些操作

https://blog.csdn.net/qq_42727752/article/details/119765592

MATLAB绘制栅格步骤：

a. 创建行列大小的二维矩阵，初始状态全为数值1表示空地；

b. 利用矩阵(下标位置) = 赋值的方式，修改对应位置的数值(和colormap的颜色值对应)，表示地图特征；

c. 利用colormap函数和image函数绘制出颜色地图；

d. 添加坐标。

3.1 colormap函数

利用image函数可以画出栅格图像，具体操作可以查看help image的帮助文档。

3.2 几个位置表述转换函数

MATLAB只要用到sub2ind和sub2coord即可

sub2ind和ind2sub将矩阵对应的线性值化成下标，具体操作可以查看 help sub2ind和help ind2sub的帮助文档。

sub2coord.m和coord2sub.m函数：

 

function coord = sub2coord(sub)
%SUB2COORD 将行列式下标装换为坐标格式，此时的坐标格式和原本认知坐标方向也不一致（如下所示）
%        1 2 3 4 5 6 7 .... X坐标
%      1|——————————>
%      2|
%      3|
%      4|
%      5|
%  Y坐标/


    [l,w] = size(sub);
    % 长度l=2表示sub为2*n矩阵
if l == 2
coord(1,:) = sub(2,:);
        coord(2,:) = sub(1,:);
    end


if w == 2
        coord(:,1) = sub(:,2);
        coord(:,2) = sub(:,1);
    end


end

function sub = coord2sub(coord)
%COORD2SUB 将坐标转换为矩阵行列格式，坐标格式为下图所示
%        1 2 3 4 5 6 7 .... X坐标
%      1|——————————>
%      2|
%      3|
%      4|
%      5|
%  Y坐标/


    [l,w] = size(coord);
    % 长度l=2表示sub为2*n矩阵
if l == 2
sub(1,:) = coord(2,:);
sub(2,:) = coord(1,:);
    end


if w == 2
sub(:,1) = coord(:,2);
sub(:,2) = coord(:,1);
    end
end

‍

 

3.3 利用地图线性值信息绘制栅格图(FD_DrawRasterMap.m函数)

线性索引值的目的就是省略for循环，因为矩阵A(线性索引)=int可以很方便的全部赋值，而Python/C++等需要借助循环

其次是线性索引作为输入，只要一个参数即可，同时在表达矩阵上，可以简化成一维，而行列是二维较为复杂

 

function Fields = DrawRasterMap(rows,cols,startsub,endsub,obssub)
%DRAWRASTERMAO 绘制随机障碍物的栅格图,存在一个返回内容；输入格式（行数 列数 起点行列 终点行列置 障碍物行列）；
                                                       % 输出内容:数值化的栅格矩阵


% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols); %初始化空白区域 数值全为1,数值用于表示Cmap的颜色(因为Cmap数组索引只能从1开始)


% 起始点和目标点
start = startsub;                       % 起点的位置
goal = endsub;                          % 终点的位置
field(start(1),start(2)) = 4;           % 起点在数组的数值,数值用于表示Cmap的颜色
field(goal(1),goal(2)) = 5;             % 终点在数组的数值,数值用于表示Cmap的颜色


% 障碍物区域
obs = obssub;
field(obs(:,1),obs(:,2)) = 2;       %障碍物在数组的数值,数值用于表示Cmap的颜色


% ObsR = obssub(:,1);ObsC = obssub(:,2);
% obs = sub2ind([rows,cols],ObsR,ObsC);
% field(obs) = 2;                         % 障碍物在数组的数值,数值用于表示Cmap的颜色


% 函数返回值为数值化的栅格矩阵
Fields = field;


%构建数组的栅格图 可以查看帮助 help image
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;
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;


end

 

测试代码：生成行为4列为5的矩阵，起点位置在3，终点在19，障碍物位置在10、11和12。主要目的是为了理解image函数的线性值和ROW和COL的坐标关系。

 

%%%% TestMain.m
%%%% 脚本文件 用于直接调用函数 熟悉概念
clc;
clear;
close all;
rows = 4;cols = 5;


[startpos(1),startpos(2)] = ind2sub([rows,cols],3);
[endpos(1),endpos(2)] = ind2sub([rows,cols],19);
[obspos(:,1),obspos(:,2)] = ind2sub([rows,cols],[10;11;12]);


field = FD_DrawRasterMap(rows,cols,startpos,endpos,obspos);
field
% 如果出线障碍物信息在地图上显示不对
% 可能是在函数内的field(obsIndex(:,1),obsIndex(:,2)) = 2;产生错误
% 可以采用将行列转成线性索引即函数内该代码下面注释的内容

 

最后生成的图像如图所示。

04  Python绘制栅格地图

Python绘制栅格地图的流程：

a. 创建全1二维矩阵，表示空地信息。
b. 将下标位置的栅格信息装换成特征值。
c. 将矩阵可视化。

Python数组的一些操作：

https://blog.csdn.net/qq_42727752/article/details/108368891

输入数据依旧采用按列递增的方式（如障碍物坐标[[row1,col1],][row2,col2],[row3,col3];…]）

4.1 matplotlib和seaborn包

绘制栅格地图前需要pip install matplotlib和seaborn两个绘图功能包。

4.2 几种坐标转换函数PathPlanning.py

 

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
import random
import copy
from matplotlib import colors








'''
# --------------------------------PATHPLANNING函数包------------------------------------
# =====Tips1:输入参数矩阵尺寸为[0-->rows]的所有个数，非rows的最大下标
# =====Tips2:区别行列矩阵，行列坐标，和XY坐标
# =====Tips3:在不同表示系，sz均为行列尺寸，即sz=[rows,cols],在XY坐标坐标系中sz=[leny,lenx]
#
# # sub2coord(possub)函数，输入单个行列位置，输出单个行列坐标位置(数组)
# # coord2sub(posxy)函数，输入单个行列坐标位置，输出单个行列位置(数组)
# # xy2sub(sz,x,y)函数，输入尺寸、x坐标和y坐标，输出行列位置(数组)
# # sub2xy(sz,r,c)函数，输入尺寸、XY坐标行位置和列位置，输出xy位置(数组)
# # sub2ind(sz,r,c)函数，输入尺寸，行位置和列位置，输出对应的线性索引值(从0开始)
# # ind2sub(sz,ind)函数，输入尺寸和线性索引值，输出矩阵位置(数组)
# # DrawHeatMap(field)函数，输入地图矩阵绘制热力图，无输出
# ----------------------------------------------------------------------------------------
'''








'''
# sub2coord和coord2sub函数，行列坐标系内，行列位置和坐标位置相互转换
# 其中的【行列坐标系】形式如图所示，从0开始，因为Python数组下标从0开始，Y方向沿下递增
#
#                   0 1 2 3 4
#                0+---------->X(X=cols)
#                1|
#                2|
#                3|
#               Y(Y=rows)
'''
def sub2coord(possub):
    posx = possub[1]
    posy = possub[0]
return [posx, posy]




def coord2sub(posxy):
    posr = posxy[1]
    posc = posxy[0]
return [posr, posc]




'''
# xy2sub和sub2xy函数，坐标系XY位置转换为矩阵格式的转换函数
# ++++sz为坐标尺寸，即元素个数，非最大坐标值
# 其中的XY形式如图所示，从0开始，到[sz]-1为止
#
#                Y/
#                3|
#                2|
#                1|
#                0+---------->X
#                   0 1 2 3 4
'''
def xy2sub(sz, x, y):
    r = sz[0]-1-y
    c = x
return [r, c]




def sub2xy(sz, r, c):
    x = c
    y = sz[0]-1-r
return [x, y]




'''
# sub2ind和ind2sub函数，将行列位置转换为索引位置
# 行列位置和索引关系，对应如图所示
#
#                   0 1 2 3 4
#                0+---------->cols
#                1| 0 3 6 ...
#                2| 1 4 7 ...
#                3| 2 5 8 ...
#               rows
'''


def sub2ind(sz,r,c):
    ind = c*sz[0]+r
return ind




def ind2sub(sz,ind):
    c = int(ind/sz[0])
    r = ind-c*sz[0]
return [r,c]








'''
# DrawHeatMap函数，用于通过栅格地图的信息，绘制出彩色地图
# INPUT：栅格电子地图矩阵
# OUTPUT:NONE
#
'''
def DrawHeatMap(field):
    rows = len(field)
    cols = len(field[0])
    cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue'])


# 绘图函数
# 其实默认为fig,ax = plt.figure(),后续发现fig没有用上
# 但是ax需要频繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(12, 8))
    ax = plt.gca()


# 绘制热力图
# 其中vmin和vmax对应栅格地图数值的颜色与cmap一一对应
# cbar设置false将色条设置为不可见
    ax = sns.heatmap(field, cmap=cmap, vmin=0, vmax=8, linewidths=0.8,linecolor='black', ax=ax, cbar=False)


# 设置图标题
    ax.set_ylabel('rows')
    ax.set_xlabel('cols')


# 将列标签移动到图像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')


# 设置图标的数字个数文字，放在plt.show下面能居中
    ax.set_xticks(np.arange(cols))
    ax.set_yticks(np.arange(rows))

 

4.3 采用seaborn绘制静态地图

 

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import colors
from matplotlib import animation
import matplotlib.patches as pc
import copy




def sub2coord(possub):
    posx = possub[1]
    posy = possub[0]
return [posx,posy]




def coord2sub(posxy):
    posr = posxy[1]
    posc = posxy[0]
return [posr,posc]




'''
# # -------------------------------------这里定义绘图函数-------------------------------------------------
'''
def drawmap(rows,cols,startSub,goalSub,obsSub):
# 创建全部为空地的地图栅格，其中空地以数字1表征
    field = np.ones((rows,cols))


# 修改栅格地图中起始点和终点的数值，其中起点以数值4表征，终点以数值5表示
    field[startSub[0],startSub[1]] = 4
    field[goalSub[0],goalSub[1]] = 5


# 修改栅格地图中障碍物的数值，其中以数值5表示
for i in range(len(obsSub)):
        field[obsSub[i][0],obsSub[i][1]] = 2


# 绘制图像，利用matplotlib的热力图进行绘制
# 设置色条的范围，从0~8
    cmap = colors.ListedColormap(['none','white','black','red','yellow','magenta','green','cyan','blue'])


# 绘图函数
# 其实默认为fig,ax = plt.figure(),后续发现fig没有用上
# 但是ax需要频繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(cols,rows))
    ax = plt.gca()




# 绘制热力图
# 其中vmin和vmax对应栅格地图数值的颜色与cmap一一对应
# cbar设置false将色条设置为不可见
    sns.heatmap(field, cmap = cmap,vmin = 0,vmax = 8, linewidths = 1.25, linecolor= 'black', ax = ax, cbar = False)


# 设置图标题
    ax.set_ylabel('rows')
    ax.set_xlabel('cols')


# 将列标签移动到图像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')




# 设置图标的数字个数文字，放在plt.show下面能居中
    ax.set_xticks(np.arange(cols))
    ax.set_yticks(np.arange(rows))


# 直接显示图像，如要类似于MATLAB的hold on建议在主函数最后使用plt.show()
# plt.show()


return field


'''
# # -------------------------------------这是是测试的主函数-------------------------------------------------
'''
rows = 4
cols = 5


startSub = [2,0]
goalSub = [2,4]
obsSub = [[1,2],[2,2],[3,2]]


drawmap(rows,cols,startSub,goalSub,obsSub)
# 采用热力图这种，坐标系简历和MATLAB是一致的，X=col，Y=rows，且递增方向都是一致的，因此绘制XY时候比较难理解
pointxy = [2,3]
plt.scatter(pointxy[0],pointxy[1],s = 200,c = 'r')
pointsub = coord2sub(pointxy)
plt.scatter(pointsub[0],pointsub[1],s = 200,c = 'y')
plt.show()

 

最后生成的图像效果如图。

4.4 采用plot在XY坐标填充矩形绘制地图

 

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from matplotlib import colors
import matplotlib.patches as pc




def xy2sub(len,x,y):
    r = len - y -1
    c = x
return [r,c]


def sub2xy(len,r,c):
    x = c
    y = len - r - 1
return [x,y]




def drawmap_xy(Xs,Ys,startxy,goalxy,obsxy):
# 其中X和矩阵地图的cols对应
    rows = Ys
    cols = Xs
# 创建全部为空地的地图栅格，其中空地以数字1表征
# ！！！注意ones(行列个数，因此rows需要+1)
    field = np.ones([rows,cols])


# 修改栅格地图中起始点和终点的数值，其中起点以数值4表征，终点以数值5表示
    startsub = xy2sub(rows,startxy[0],startxy[1])
    goalsub = xy2sub(rows,goalxy[0],goalxy[1])
    field[startsub[0],startsub[1]] = 4
    field[goalsub[0],goalsub[1]] = 5


# 修改栅格地图中障碍物的数值，其中以数值5表示
for i in range(len(obsxy)):
        obssub = xy2sub(rows,obsxy[i][0],obsxy[i][1])
        field[obssub[0],obssub[1]] = 2




# 设置画布属性
    plt.figure(figsize=(cols,rows))
    plt.xlim(-1, cols)
    plt.ylim(-1, rows)
    plt.xticks(np.arange(Xs))
    plt.yticks(np.arange(Ys))


# 绘制障碍物XY位置
for i in range(len(obsxy)):
        plt.gca().add_patch(pc.Rectangle((obsxy[i][0] - 0.5, obsxy[i][1] - 0.5), 1,1,color='k'))


# 绘制起点，终点
    plt.gca().add_patch(pc.Rectangle((startxy[0] - 0.5, startxy[1] - 0.5), 1,1,color='yellow'))
    plt.gca().add_patch(pc.Rectangle((goalxy[0] - 0.5, goalxy[1] - 0.5), 1,1,color='m'))


return field








'''
这里是主函数，将下列地图，用以为坐标XY形式绘制出
# Y/|1. 1. 1. 1. 1.
#   |1. 1. 2. 1. 1.
#   |4. 1. 2. 1. 5.
#   |1. 1. 2. 1. 1.---->X
'''


startxy = [0,1]
goalxy = [4,1]
obsxy = [[2,0],[2,1],[2,2]]


Ys = 4
Xs = 5


drawmap_xy(Xs,Ys,startxy,goalxy,obsxy)


# 采用XY坐标的优势在于绘制其他参数时候，如在X=2，Y=3 绘制一个大圆点，但是地图矩阵和XY又需要相互转变一下
plt.scatter(2,3,s = 200,c = 'r')
plt.show()

 

4.5 绘制动态地图信息的方法

绘制动态地图方法提供两种参考：

a. 使用imshow(field)联合plt.pause()显示栅格地图

b. 利用plt.plot()联合plt.pause()和plt.cla()进行刷新显示

tips：import PathPlanning是必须的

 

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import colors
from matplotlib import animation
import matplotlib.patches as pc
import copy
import random


import PathPlanning




'''
# ----------------------------------------------------------------------------------------------
# 绘制动态地图方法提供两种参考：
# 1、使用imshow(field)联合plt.pause()显示栅格地图
# 2、利用plt.plot()联合plt.pause()和plt.cla()进行刷新显示
# tips：import PathPlanning是必须的
# -----------------------------------------------------------------------------------------------
'''


'''
# ------------------------声明地图信息和某些固定信息，地图样式如图所示
# [[1. 1. 1. 1. 1. 1. 1.]
#  [1. 1. 1. 2. 1. 1. 1.]
#  [1. 4. 1. 2. 1. 5. 1.]
#  [1. 1. 1. 2. 1. 1. 1.]
#  [1. 1. 1. 1. 1. 1. 1.]
#  [1. 1. 1. 1. 1. 1. 1.]]
#
'''
rows = 6
cols = 7
startSub = [2,1]
goalSub = [2,5]
obsSub = [[1,3],[2,3],[3,3]]




# 栅格地图属性
field = np.ones((rows, cols))


field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5


for i in range(len(obsSub)):
    field[obsSub[i][0], obsSub[i][1]] = 2


# 新建画布指定大小
fig = plt.figure(figsize=(9,6))
# 新建子图
ax = fig.add_subplot(111)


# 没有用的变量，主要是区分绘制方法是第一种还是第二种
draw = False






'''
# 方法一利用imshow绘制动态地图
# 利用随机函数，随机选择地图位置改变数值
'''
cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue'])


# 动态刷新地图次数
if draw:
for i in range(10):
# 随机选择修改地图信息的位置
        temp_r = random.randint(0,rows-1)
        temp_c = random.randint(0,cols-1)


# 修改地图信息
        field[temp_r][temp_c] = 3


# 显示图像并设置图像属性
        ax.imshow(field, cmap=cmap, vmin=0, vmax=8)
        ax.set_ylabel('rows')
        ax.set_xlabel('cols')
        ax.xaxis.tick_top()
        ax.xaxis.set_label_position('top')
        ax.set_xticks(np.arange(cols))
        ax.set_yticks(np.arange(rows))
        plt.pause(0.05)


# 重置回白色
        field[temp_r][temp_c] = 1




'''
# 方法二：采用plt.plot联合plt.pause()和plt.cla()
# 利用随机函数，随机选择地图位置改变数值
'''
if not draw:
# plt.plot都是建立在xy坐标系上的内容可以利用pathplanning的sub2xy函数转换
# 将行列转成xy坐标系的xy值
    startXY = PathPlanning.sub2xy([rows,cols],startSub[0],startSub[1])
    goalXY = PathPlanning.sub2xy([rows,cols],goalSub[0],goalSub[1])
    obsX = []
    obsY = []


for i in range(len(obsSub)):
        obsxy = PathPlanning.sub2xy([rows,cols],obsSub[i][0],obsSub[i][1])
        obsX.append(obsxy[0])
        obsY.append(obsxy[1])


for i in range(100):
# 随机选择修改地图信息的位置
        temp_r = random.randint(0,rows-1)
        temp_c = random.randint(0,cols-1)
        temp_xy = PathPlanning.sub2xy([rows,cols],temp_r,temp_c)


# 修改地图信息
        field[temp_r][temp_c] = 3


# 显示图像并设置图像属性
        plt.plot(startXY[0],startXY[1],'r+')
        plt.plot(goalXY[0],goalXY[1],'b+')
        plt.plot(obsX,obsY,'sk')
        plt.plot(temp_xy[0],temp_xy[1],'sr')
        ax.set_xlim([-1,cols])
        ax.set_ylim([-1,rows])
        ax.set_xticks(np.arange(cols))
        ax.set_yticks(np.arange(rows))
        plt.pause(1)
        plt.cla()


# 重置回白色
        field[temp_r][temp_c] = 1

05  C++创建栅格地图矩阵

 

C++创建栅格地图矩阵的流程：

    a. 创建全0二维数组，表示空地信息。     b. 将下标位置的栅格信息装换成特征值。     c. C++虽然有matplotlib包，但是可视化效果并不是很好，这里不使用C++绘制栅格地图，难度较大。

C++数组的一些基本操作：

 

#include
#include


using namespace std;


int main()
{
int const rows = 4;
int const cols = 5;
int startpos[2] = {2,0};
int endpos[2] = {2,4};
int obspos[3][2] = {{1,2},{2,2},{3,2}};


// 定义栅格地图二维数组，默认初始化为0
int filed[rows][cols] = {0};


// 修改起点和终点在栅格数组中的数值，其中起点为1，终点为2
    filed[startpos[0]][startpos[1]] = 1;
    filed[endpos[0]][endpos[1]] = 2;


// 循环修改障碍物在栅格矩阵的数值，其中障碍物数值为3
int len = sizeof(obspos) / sizeof(obspos[0]);
for(int temp_len = 0;temp_len 

	 

	Tips：对C++编程而言，特殊位置信息的二维数组，作为参数进行传递时候，是不方便编程的。后续优化代码，使其采用线性索引的方式进行编程，实现函数封装，提升通用性。

	　　审核编辑：汤梓红