0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看威廉希尔官方网站 视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

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

3D视觉工坊 来源:古月居 2023-03-15 11:40 次阅读

01 数据地图

1.1 地图类型

c6ac878a-c24b-11ed-bfe3-dac502259ad0.png

1.2 栅格地图表示方式

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

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

c6c2d5da-c24b-11ed-bfe3-dac502259ad0.png

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

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

c6d48bcc-c24b-11ed-bfe3-dac502259ad0.png

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是圆圈大小

c6e5fca4-c24b-11ed-bfe3-dac502259ad0.png

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

c6f7c0ce-c24b-11ed-bfe3-dac502259ad0.png

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

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

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

②MATLAB只要用到sub2ind和sub2coord即可

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

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

c706be26-c24b-11ed-bfe3-dac502259ad0.png

03 MATLAB绘制栅格地图

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

c71a358c-c24b-11ed-bfe3-dac502259ad0.png

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函数:

functioncoord=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
functionsub=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++等需要借助循环

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

functionFields=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;产生错误
% 可以采用将行列转成线性索引即函数内该代码下面注释的内容

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

c7346cd6-c24b-11ed-bfe3-dac502259ad0.png

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绘制静态地图

importnumpyasnp
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()

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

c74ca210-c24b-11ed-bfe3-dac502259ad0.png

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

c75abcc4-c24b-11ed-bfe3-dac502259ad0.png

importnumpyasnp
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()

c770161e-c24b-11ed-bfe3-dac502259ad0.png

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

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

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

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

tips:import PathPlanning是必须的

importnumpyasnp
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++编程而言,特殊位置信息的二维数组,作为参数进行传递时候,是不方便编程的。后续优化代码,使其采用线性索引的方式进行编程,实现函数封装,提升通用性。

审核编辑:汤梓红

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • matlab
    +关注

    关注

    185

    文章

    2974

    浏览量

    230405
  • 算法
    +关注

    关注

    23

    文章

    4608

    浏览量

    92844
  • 路径规划
    +关注

    关注

    0

    文章

    78

    浏览量

    15321
  • python
    +关注

    关注

    56

    文章

    4793

    浏览量

    84631
  • 数组
    +关注

    关注

    1

    文章

    417

    浏览量

    25940

原文标题:路径规划算法之栅格地图绘制

文章出处:【微信号:3D视觉工坊,微信公众号:3D视觉工坊】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    动态规划算法

    动态规划算法资料。
    发表于 08-30 20:44

    LCS的动态规划算法

    LCS的动态规划算法(自底向上)
    发表于 05-25 15:06

    你知道有哪几种常见的车辆路径规划算法吗?

    如何去提高汽车的主动安全性和交通安全性?从算法上解读自动驾驶是如何实现的?有哪几种常见的车辆路径规划算法
    发表于 06-17 10:56

    基于插值A算法路径规划

    提出一个基于插值的路径规划算法-插值 A*。此算法可以在每个栅格路径代价不一致的情况下生成一条平滑路径
    发表于 03-03 14:59 16次下载

    基于势场栅格法的机器人全局路径规划

    基于势场栅格法的机器人全局路径规划!资料来源网络,如有侵权,敬请见
    发表于 11-30 11:33 10次下载

    基于实时交通信息的动态路径规划算法性能比较_黄西洲

    基于实时交通信息的动态路径规划算法性能比较_黄西洲
    发表于 03-16 10:04 0次下载

    基于路径跟踪方法的路径规划算法

    为解决拖挂式移动机器人系统路径规划算法精准性低、稳定性差和无法考虑系统间安全性等的问题,提出一种基于路径跟踪方法的路径规划算法。该
    发表于 12-04 14:18 6次下载
    基于<b class='flag-5'>路径</b>跟踪方法的<b class='flag-5'>路径</b><b class='flag-5'>规划算法</b>

    基于滚动窗口的路径规划算法

    自主构建环境地图,具有重要的现实意义。 针对智能机器人如何以最优探测路线自主探测和构建室内环境地图的问题,提出一种基于滚动窗口的路径规划算法。将传统遍历构图的牛耕遍历方式改进为未知环境
    发表于 02-27 10:02 18次下载
    基于滚动窗口的<b class='flag-5'>路径</b><b class='flag-5'>规划算法</b>

    自动驾驶汽车四种常用的路径规划算法解析

    自动驾驶汽车的路径规划算法最早源于机器人的路径规划研究,但是就工况而言却比机器人的路径规划复杂得
    发表于 03-08 17:29 1.7w次阅读

    水下航行器自主巡航的路径规划算法实现

    路径规划算法是自主水下航行器(AUV)完成水下自主巡航的核心算法之一。分别综述了基于环境建模和路径搜索两类AUⅣ路径
    发表于 04-09 16:01 8次下载
    水下航行器自主巡航的<b class='flag-5'>路径</b><b class='flag-5'>规划算法</b>实现

    嵌入式GIS中最优路径规划算法研究与实现

    嵌入式GIS中最优路径规划算法研究与实现(嵌入式开发项目经理)-嵌入式GIS中最优路径规划算法研究与实现                    
    发表于 07-30 12:49 4次下载
    嵌入式GIS中最优<b class='flag-5'>路径</b><b class='flag-5'>规划算法</b>研究与实现

    A星路径规划算法完整代码资料汇总

    A星路径规划算法完整代码资料汇总
    发表于 12-03 17:16 11次下载

    机器人基于搜索和基于采样的路径规划算法

    基于搜索的路径规划算法已经较为成熟且得到了广泛应用,常常被用于游戏中人物和移动机器人的路径规划
    发表于 10-13 14:23 372次阅读
    机器人基于搜索和基于采样的<b class='flag-5'>路径</b><b class='flag-5'>规划算法</b>

    机器人路径基于采样的规划

    路径规划算法主要可分成两种,一种是基于搜索结果的规划,另一类便是本文中将要提及的基于采样的规划。 一般而言,基于搜索的规划(如Astar)通
    的头像 发表于 11-16 15:45 506次阅读
    机器人<b class='flag-5'>路径</b>基于采样的<b class='flag-5'>规划</b>

    全局路径规划RRT算法原理

    通往目的地的安全和无碰撞的路径路径规划问题可以分为两个方面: (一)全局路径规划:全局路径
    的头像 发表于 11-24 15:57 1056次阅读