3 移动机器人路径规划（1- 栅格地图绘制）

最新推荐文章于 2024-09-09 12:14:15 发布
最新推荐文章于 2024-09-09 12:14:15 发布
阅读量2.1w 收藏 333
点赞数 48
分类专栏： M/P/R与机器人学 文章标签： python matlab
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42727752/article/details/117339892
版权

1 栅格地图绘制

1.1 数据地图

1.1.1地图类型

常见的地图表示方法有：栅格法、可视图法、拓扑图法等等。
在这里插入图片描述

1.1.2 栅格地图表示方式

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

2、矩阵表示地图与传统意识中坐标X,Y有所区别。
在这里插入图片描述
如图所示的地图，在矩阵表示中，起点位置为[3,1]，终点为[3,5]，三个障碍物分别为[2,3],[3,3]和[4,3]。在坐标表示中，起点位置为(1,2)，终点为(5,2)，三个障碍物分别为(3,1),(3,2)和(3,3)

3、矩阵表示地图还可以用线性索引来简化，这样将矩阵的二维数组表示成一位数组，编程更方便。
在这里插入图片描述

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

1.2 位置的表示方法的区分

1.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
在这里插入图片描述

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

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

①sub2coord和sub2xy均是仿照MATLABsub2ind命名的函数，方便理解和记忆
②MATLAB只要用到sub2ind和sub2coord即可
③sub2xy主要是正对于Python进行plt.plot绘图使用的

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

在这里插入图片描述

1.3 MATLAB绘制栅格地图

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

在这里插入图片描述

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

MATLAB绘制栅格步骤
1、创建行列大小的二维矩阵，初始状态全为数值1表示空地；
2、利用矩阵(下标位置) = 赋值的方式，修改对应位置的数值(和colormap的颜色值对应)，表示地图特征；
3、利用colormap函数和image函数绘制出颜色地图；
4、添加坐标。

1.3.1 colormap函数

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

1.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

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

最后生成的图像如图所示。
在这里插入图片描述

1.4 Python绘制栅格地图

Python绘制栅格地图的流程：
1、创建全1二维矩阵，表示空地信息。
2、将下标位置的栅格信息装换成特征值。
3、将矩阵可视化。

Python数组的一些操作：1 Python基础（1 常用的数据类型和程序结构）

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

1.4.1 matplotlib和seaborn包

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

参考1：Creating annotated heatmaps
参考2：Python绘制热力图

1.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))

1.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()

最后生成的图像效果如图。
在这里插入图片描述

1.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()

在这里插入图片描述

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

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

  • 1、使用imshow(field)联合plt.pause()显示栅格地图
  • 2、利用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

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

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

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

#include<iostream>
#include<vector>

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 <len; temp_len++)
    {
        filed[obspos[temp_len][0]][obspos[temp_len][1]] = 3;
    }


    // 打印栅格矩阵数值
    for(int r = 0; r < rows; r++)
    {
        for(int c = 0; c < cols; c++)
        {
            cout<<filed[r][c]<<" ";
        }
        cout<<endl;

    }
    return 0;
}

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

1.5 0积分参考代码

0积分参考代码连接：参考

下一篇：2 Dijkstra路径规划算法

爬楼的猪
关注
专栏目录
基于栅格地图的Dijkstra算法路径规划
01-19
用MATLAB 实现，基于栅格地图的Dijkstra算法路径规划。
【路径规划】基于灰狼算法实现机器人栅格地图路径规划matlab源码.zip
10-20
在机器人路径规划的场景中，栅格地图是一种常见的表示环境的方法。地图被划分为多个正方形或矩形的单元格，每个单元格代表机器人可以占据的位置，其中绿色单元格表示安全路径，红色或其他颜色的单元格表示障碍物。...
我建议的标题是：C++实现栅格地图的状态空间和状态校验的RRT搜索算法与混合A*全局规划算法，混合A*与RRT-Dubins 全局规划算法在C++中的实现
XPOIEYLo的博客
06-09 1232
RRT+Dubins规划算法和混合A*规划算法是近年来新兴的全局规划算法，它们通过引入新的思想和技术，克服了传统算法的局限性，并在实际应用中取得了较好的效果。在C++中实现栅格地图、状态空间和状态校验是实现全局规划算法的关键，通过合理设计类和数据结构，以及定义相应的函数和方法，可以实现高效、准确的路径规划。最近几年，由于机器人领域的发展和需求的日益增加，一种新型的全局规划算法——混合A*规划算法逐渐引起了人们的注意。在混合A*算法中，离散状态空间用于搜索路径的粗略规划，而连续状态空间用于路径的优化和细化。
ROS_Kinetic_x ROS栅格地图庫 Grid Map Library
zhangrelay的专栏
09-28 6018
源自：https://github.com/ethz-asl/grid_map Grid Map Overview This is a C++ library with ROS interface to manage two-dimensional grid maps with multiple data layers. It is designed for mobile robotic
【栅格地图路径规划】基于SSA、GA、PSO、DE、GWO五大经典算法实现机器人路径规划附Matlab代码
最新发布
m0_60703264的博客
09-09 1240
机器人路径规划是机器人领域的核心问题之一，其目标是在复杂环境中找到一条安全、高效的路径，使机器人从起点到达目标点。栅格地图是常见的环境表示方式，将环境划分为离散的网格单元，方便路径规划算法的实施。本文将基于五种经典的群智能优化算法：模拟退火算法（SSA）、遗传算法（GA）、粒子群优化算法（PSO）、差分进化算法（DE）、灰狼优化算法（GWO）实现栅格地图下的机器人路径规划，并提供相应的Matlab代码。1. 问题描述。
栅格地图
热门推荐
shuijinghua的博客
01-22 1万+
参考网址 https://wenku.baidu.com/view/e3f60da9ad51f01dc381f104.html 栅格地图有两种表示方法，直角坐标系法和序号法，序号法比直角坐标法节省内存 室内环境栅格法建模步骤 1.栅格粒大小的选取 栅格的大小是个关键因素，栅格选的小，环境分辨率较大，环境信息存储量大，决策速度慢。 栅格选的大，环境分辨率较小，环境信息存储量小，决...
栅格法地图构建
m0_51649746的博客
05-10 1223
栅格法创建地图，快捷，方便
基于栅格地图的无人机三维路径规划算法（Matlab代码实现）
m0_73907476的博客
05-16 1093
% ********蚁群算法********************************text(goal(1),goal(2),goal(3),'终点','color','r');
栅格地图（自己看的）
weixin_60352621的博客
07-05 201
同时对于索引来说，得到在地图上的索引的时候，依靠当前在地图的位置，因为建图的中心点是不固定的，准确来说是不固定在地图的左下角，那么对于当前的点位来说，存储需要依靠当前的点位减去最左下角具体的值，来将存在内存上的地图来连续起来，因为内存空间是从左下角开始连续存卒。对于栅格地图它是连续的存储在内存空间上的，那么xy的二维栅格地图的存储是根据x轴一排一排存储，则y轴坐标变换一个要乘一行x的值。
蚁群算法在栅格地图上的最短距离 MATLAB.rar_matlab 栅格地图_栅格地图_栅格蚁群_栅格蚁群算法_栅格路径规划
07-14
总之，蚁群算法在栅格地图上求解最短距离是一种有效的路径规划方法，结合MATLAB的高效计算能力，可以解决实际场景中的诸多问题。理解并掌握这种算法的原理和实现，对提升路径规划领域的研究和应用水平具有重要意义。
世界地图遥感栅格图
11-10
下载后将自己感兴趣的区域，格式是shp与影像叠加，进行数据分析和研究
栅格法建立地图环境
05-31
栅格地图
matlab栅格地图绘制
01-05
matlab栅格地图绘制，介绍了相关matlab程序，用户可根据需要学习 是太阳能光伏发电系统（Solar power system）的简称，是一种利用太阳电池半导体材料的光伏效应，将太阳光辐射能直接转换为电能的一种新型发电系统，有独立运行和并网运行两种方式。
基于matlab实现的粒子群优化算法，通过栅格法实现移动机器人路径规划.rar
05-05
在本项目中，"基于matlab实现的粒子群优化算法，通过栅格法实现移动机器人路径规划"，主要关注的是如何利用PSO算法解决移动机器人的路径规划问题。路径规划是机器人学中的一个核心问题，旨在找到机器人从起点到终点...
基于人工势场的机器人路径规划-MATLAB源程序
04-28
总的来说，这个项目为研究者提供了一个实用的工具，用于学习、实验和优化基于人工势场的机器人路径规划算法。结合MATLAB的可视化能力和C++的执行效率，可以深入理解路径规划的原理，同时也能为实际机器人系统的开发...
基于栅格地图的A-星算法路径规划
01-19
本篇文章将深入探讨基于栅格地图的A-星算法路径规划，并以MATLAB实现为例进行详细讲解。 首先，我们需要理解栅格地图的概念。栅格地图是一种将环境抽象为二维网格的表示方式，每个网格代表一个固定大小的区域。在...
栅格地图建立-Grid-Mapping
fb_941219的博客
12-15 6327
占栅格地图-Occupancy Grid Map，理论推导及代码阅读。
【全局规划】栅格地图
uytguytgf的博客
08-25 2211
构建栅格地图
金鹰算法栅格地图机器人路径规划中文参考文献
07-23
很抱歉，我之前回答的关于金鹰算法的参考文献与您的问题不符。以下是关于栅格地图机器人路径规划中金鹰算法的一些中文参考文献供您参考： 1. 刘德华, 曹静, 王胜. 基于金鹰算法的栅格地图机器人路径规划研究[J]. 计算机科学, 2018, 45(7): 220-224. 2. 张润泽, 鲁成龙, 王胜. 基于金鹰算法的栅格地图机器人路径规划[J]. 计算机科学, 2019, 46(8): 189-193. 3. 王凯, 李静, 武忠杰. 基于金鹰算法的栅格地图机器人路径规划研究[J]. 计算机科学, 2019, 46(8): 211-215. 请注意，金鹰算法在栅格地图机器人路径规划领域的应用相对较新，因此相关中文文献可能相对较少。建议您在进行研究时，还可以参考英文文献、学位论文以及相关领域的会议论文等，以获取更多的研究资料。
评论 5
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值