01 數(shù)據(jù)地圖

1.1 地圖類型

1.2 柵格地圖表示方式

a. 本欄路徑規(guī)劃利用矩陣（二維數(shù)組）來表示柵格地圖（因為對于矩陣，無論MATLAB、C++還是Python，矩陣更適合數(shù)組的表達，更便于編程。）

b. 矩陣表示地圖與傳統(tǒng)意識中坐標X,Y有所區(qū)別。

如圖所示的地圖，在矩陣表示中，起點位置為[3,1]，終點為[3,5]，三個障礙物分別為[2,3],[3,3]和[4,3]。在坐標表示中，起點位置為(1,2)，終點為(5,2)，三個障礙物分別為(3,1),(3,2)和(3,3)

c. 矩陣表示地圖還可以用線性索引來簡化，這樣將矩陣的二維數(shù)組表示成一位數(shù)組，編程更方便。

Tips：線性索引，是從第一列開始，沿著行按順序增長，然后從第二列開始…，是豎著沿行增長的，而非橫著沿著列增長。理解記憶有點類似于Excel表格按行遞增。

02 位置的表示方法的區(qū)分

2.1 在柵格地圖上繪制xy點發(fā)生的情況

對于程序

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


% 設(shè)置地圖屬性
field = ones(rows, cols);
field(1,3) = 4;          % 柵格矩陣表示1,3位置圖像


% 顏色表征矩陣
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-靜態(tài)障礙
    1 0 0; ...           % 3-紅色-動態(tài)障礙
    1 1 0;...            % 4-黃色-起始點
    1 0 1;...            % 5-品紅-目標點
    0 1 0; ...           % 6-綠色-到目標點的規(guī)劃路徑
    0 1 1];              % 7-青色-動態(tài)規(guī)劃的路徑
colormap(cmap);
image(1.5,1.5,field);


% % 設(shè)置柵格屬性
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對應(yīng)于列col，而坐標Y對于與行row

2.2 三種表述位置方法的關(guān)系

下面圖片是針對于Python繪圖表述的，MATLAB繪圖轉(zhuǎn)換關(guān)系是一致的，但是MATLAB數(shù)組從1開始，而Python數(shù)組從0開始

①sub2coord和sub2xy均是仿照MATLABsub2ind命名的函數(shù)，方便理解和記憶

②MATLAB只要用到sub2ind和sub2coord即可

③sub2xy主要是正對于Python進行plt.plot繪圖使用的

幾個代碼的轉(zhuǎn)換函數(shù)見后面的模塊

03 MATLAB繪制柵格地圖

Tips1：這里為了方便MATLAB語言、Python語言和C++編程的統(tǒng)一性，統(tǒng)一采用按行遞增的方式存放的下標特征信息數(shù)據(jù)（如障礙物坐標[row1,col1;row2,col2;row3,col3;…]），表示地圖特征信息的位置==；

Tips2：當(dāng)你獲得線性地圖特征信息的線性索引關(guān)系或者是XY坐標值，可以將其裝換成線性索引值套用程序。MATLAB的數(shù)組的應(yīng)用可以參考： MATLAB數(shù)組的一些操作

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

MATLAB繪制柵格步驟：

a. 創(chuàng)建行列大小的二維矩陣，初始狀態(tài)全為數(shù)值1表示空地；

b. 利用矩陣(下標位置) = 賦值的方式，修改對應(yīng)位置的數(shù)值(和colormap的顏色值對應(yīng))，表示地圖特征；

c. 利用colormap函數(shù)和image函數(shù)繪制出顏色地圖；

d. 添加坐標。

3.1 colormap函數(shù)

利用image函數(shù)可以畫出柵格圖像，具體操作可以查看help image的幫助文檔。

3.2 幾個位置表述轉(zhuǎn)換函數(shù)

MATLAB只要用到sub2ind和sub2coord即可

sub2ind和ind2sub將矩陣對應(yīng)的線性值化成下標，具體操作可以查看 help sub2ind和help ind2sub的幫助文檔。

sub2coord.m和coord2sub.m函數(shù)：

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 將坐標轉(zhuǎn)換為矩陣行列格式，坐標格式為下圖所示
%        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函數(shù))

線性索引值的目的就是省略for循環(huán)，因為矩陣A(線性索引)=int可以很方便的全部賦值，而Python/C++等需要借助循環(huán)

其次是線性索引作為輸入，只要一個參數(shù)即可，同時在表達矩陣上，可以簡化成一維，而行列是二維較為復(fù)雜

functionFields=DrawRasterMap(rows,cols,startsub,endsub,obssub)
%DRAWRASTERMAO 繪制隨機障礙物的柵格圖,存在一個返回內(nèi)容；輸入格式（行數(shù) 列數(shù) 起點行列 終點行列置 障礙物行列）；
                                                       % 輸出內(nèi)容:數(shù)值化的柵格矩陣


% 定義柵格地圖全域，并初始化空白區(qū)域
field = ones(rows, cols); %初始化空白區(qū)域 數(shù)值全為1,數(shù)值用于表示Cmap的顏色(因為Cmap數(shù)組索引只能從1開始)


% 起始點和目標點
start = startsub;                       % 起點的位置
goal = endsub;                          % 終點的位置
field(start(1),start(2)) = 4;           % 起點在數(shù)組的數(shù)值,數(shù)值用于表示Cmap的顏色
field(goal(1),goal(2)) = 5;             % 終點在數(shù)組的數(shù)值,數(shù)值用于表示Cmap的顏色


% 障礙物區(qū)域
obs = obssub;
field(obs(:,1),obs(:,2)) = 2;       %障礙物在數(shù)組的數(shù)值,數(shù)值用于表示Cmap的顏色


% ObsR = obssub(:,1);ObsC = obssub(:,2);
% obs = sub2ind([rows,cols],ObsR,ObsC);
% field(obs) = 2;                         % 障礙物在數(shù)組的數(shù)值,數(shù)值用于表示Cmap的顏色


% 函數(shù)返回值為數(shù)值化的柵格矩陣
Fields = field;


%構(gòu)建數(shù)組的柵格圖 可以查看幫助 help image
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-靜態(tài)障礙
    1 0 0; ...           % 3-紅色-動態(tài)障礙
    1 1 0;...            % 4-黃色-起始點 
    1 0 1;...            % 5-品紅-目標點
    0 1 0; ...           % 6-綠色-到目標點的規(guī)劃路徑   
    0 1 1; ...           % 7-青色-動態(tài)規(guī)劃的路徑
    0 0 1];              % 8-藍色


% 構(gòu)建顏色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函數(shù)的線性值和ROW和COL的坐標關(guān)系。

%%%%TestMain.m
%%%% 腳本文件 用于直接調(diào)用函數(shù) 熟悉概念
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
% 如果出線障礙物信息在地圖上顯示不對
% 可能是在函數(shù)內(nèi)的field(obsIndex(:,1),obsIndex(:,2)) = 2;產(chǎn)生錯誤
% 可以采用將行列轉(zhuǎn)成線性索引即函數(shù)內(nèi)該代碼下面注釋的內(nèi)容

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

04 Python繪制柵格地圖

Python繪制柵格地圖的流程：

a. 創(chuàng)建全1二維矩陣，表示空地信息。
b. 將下標位置的柵格信息裝換成特征值。
c. 將矩陣可視化。

Python數(shù)組的一些操作：

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

輸入數(shù)據(jù)依舊采用按列遞增的方式（如障礙物坐標[[row1,col1],][row2,col2],[row3,col3];…]）

4.1 matplotlib和seaborn包

繪制柵格地圖前需要pip install matplotlib和seaborn兩個繪圖功能包。

4.2 幾種坐標轉(zhuǎn)換函數(shù)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函數(shù)包------------------------------------
# =====Tips1:輸入?yún)?shù)矩陣尺寸為[0-->rows]的所有個數(shù)，非rows的最大下標
# =====Tips2:區(qū)別行列矩陣，行列坐標，和XY坐標
# =====Tips3:在不同表示系，sz均為行列尺寸，即sz=[rows,cols],在XY坐標坐標系中sz=[leny,lenx]
#
# # sub2coord(possub)函數(shù)，輸入單個行列位置，輸出單個行列坐標位置(數(shù)組)
# # coord2sub(posxy)函數(shù)，輸入單個行列坐標位置，輸出單個行列位置(數(shù)組)
# # xy2sub(sz,x,y)函數(shù)，輸入尺寸、x坐標和y坐標，輸出行列位置(數(shù)組)
# # sub2xy(sz,r,c)函數(shù)，輸入尺寸、XY坐標行位置和列位置，輸出xy位置(數(shù)組)
# # sub2ind(sz,r,c)函數(shù)，輸入尺寸，行位置和列位置，輸出對應(yīng)的線性索引值(從0開始)
# # ind2sub(sz,ind)函數(shù)，輸入尺寸和線性索引值，輸出矩陣位置(數(shù)組)
# # DrawHeatMap(field)函數(shù)，輸入地圖矩陣繪制熱力圖，無輸出
# ----------------------------------------------------------------------------------------
'''








'''
# sub2coord和coord2sub函數(shù)，行列坐標系內(nèi)，行列位置和坐標位置相互轉(zhuǎn)換
# 其中的【行列坐標系】形式如圖所示，從0開始，因為Python數(shù)組下標從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函數(shù)，坐標系XY位置轉(zhuǎn)換為矩陣格式的轉(zhuǎn)換函數(shù)
# ++++sz為坐標尺寸，即元素個數(shù)，非最大坐標值
# 其中的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函數(shù)，將行列位置轉(zhuǎn)換為索引位置
# 行列位置和索引關(guān)系，對應(yīng)如圖所示
#
#                   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函數(shù)，用于通過柵格地圖的信息，繪制出彩色地圖
# 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'])


# 繪圖函數(shù)
# 其實默認為fig,ax = plt.figure(),后續(xù)發(fā)現(xiàn)fig沒有用上
# 但是ax需要頻繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(12, 8))
    ax = plt.gca()


# 繪制熱力圖
# 其中vmin和vmax對應(yīng)柵格地圖數(shù)值的顏色與cmap一一對應(yīng)
# cbar設(shè)置false將色條設(shè)置為不可見
    ax = sns.heatmap(field, cmap=cmap, vmin=0, vmax=8, linewidths=0.8,linecolor='black', ax=ax, cbar=False)


# 設(shè)置圖標題
    ax.set_ylabel('rows')
    ax.set_xlabel('cols')


# 將列標簽移動到圖像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')


# 設(shè)置圖標的數(shù)字個數(shù)文字，放在plt.show下面能居中
    ax.set_xticks(np.arange(cols))
    ax.set_yticks(np.arange(rows))

4.3 采用seaborn繪制靜態(tài)地圖

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]




'''
# # -------------------------------------這里定義繪圖函數(shù)-------------------------------------------------
'''
def drawmap(rows,cols,startSub,goalSub,obsSub):
# 創(chuàng)建全部為空地的地圖柵格，其中空地以數(shù)字1表征
    field = np.ones((rows,cols))


# 修改柵格地圖中起始點和終點的數(shù)值，其中起點以數(shù)值4表征，終點以數(shù)值5表示
    field[startSub[0],startSub[1]] = 4
    field[goalSub[0],goalSub[1]] = 5


# 修改柵格地圖中障礙物的數(shù)值，其中以數(shù)值5表示
for i in range(len(obsSub)):
        field[obsSub[i][0],obsSub[i][1]] = 2


# 繪制圖像，利用matplotlib的熱力圖進行繪制
# 設(shè)置色條的范圍，從0~8
    cmap = colors.ListedColormap(['none','white','black','red','yellow','magenta','green','cyan','blue'])


# 繪圖函數(shù)
# 其實默認為fig,ax = plt.figure(),后續(xù)發(fā)現(xiàn)fig沒有用上
# 但是ax需要頻繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(cols,rows))
    ax = plt.gca()




# 繪制熱力圖
# 其中vmin和vmax對應(yīng)柵格地圖數(shù)值的顏色與cmap一一對應(yīng)
# cbar設(shè)置false將色條設(shè)置為不可見
    sns.heatmap(field, cmap = cmap,vmin = 0,vmax = 8, linewidths = 1.25, linecolor= 'black', ax = ax, cbar = False)


# 設(shè)置圖標題
    ax.set_ylabel('rows')
    ax.set_xlabel('cols')


# 將列標簽移動到圖像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')




# 設(shè)置圖標的數(shù)字個數(shù)文字，放在plt.show下面能居中
    ax.set_xticks(np.arange(cols))
    ax.set_yticks(np.arange(rows))


# 直接顯示圖像，如要類似于MATLAB的hold on建議在主函數(shù)最后使用plt.show()
# plt.show()


return field


'''
# # -------------------------------------這是是測試的主函數(shù)-------------------------------------------------
'''
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坐標填充矩形繪制地圖

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對應(yīng)
    rows = Ys
    cols = Xs
# 創(chuàng)建全部為空地的地圖柵格，其中空地以數(shù)字1表征
# ！！！注意ones(行列個數(shù)，因此rows需要+1)
    field = np.ones([rows,cols])


# 修改柵格地圖中起始點和終點的數(shù)值，其中起點以數(shù)值4表征，終點以數(shù)值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


# 修改柵格地圖中障礙物的數(shù)值，其中以數(shù)值5表示
for i in range(len(obsxy)):
        obssub = xy2sub(rows,obsxy[i][0],obsxy[i][1])
        field[obssub[0],obssub[1]] = 2




# 設(shè)置畫布屬性
    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








'''
這里是主函數(shù)，將下列地圖，用以為坐標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坐標的優(yōu)勢在于繪制其他參數(shù)時候，如在X=2，Y=3 繪制一個大圓點，但是地圖矩陣和XY又需要相互轉(zhuǎn)變一下
plt.scatter(2,3,s = 200,c = 'r')
plt.show()

4.5 繪制動態(tài)地圖信息的方法

繪制動態(tài)地圖方法提供兩種參考：

a. 使用imshow(field)聯(lián)合plt.pause()顯示柵格地圖

b. 利用plt.plot()聯(lián)合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




'''
# ----------------------------------------------------------------------------------------------
# 繪制動態(tài)地圖方法提供兩種參考：
# 1、使用imshow(field)聯(lián)合plt.pause()顯示柵格地圖
# 2、利用plt.plot()聯(lián)合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)


# 沒有用的變量，主要是區(qū)分繪制方法是第一種還是第二種
draw = False






'''
# 方法一利用imshow繪制動態(tài)地圖
# 利用隨機函數(shù)，隨機選擇地圖位置改變數(shù)值
'''
cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue'])


# 動態(tài)刷新地圖次數(shù)
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


# 顯示圖像并設(shè)置圖像屬性
        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聯(lián)合plt.pause()和plt.cla()
# 利用隨機函數(shù)，隨機選擇地圖位置改變數(shù)值
'''
if not draw:
# plt.plot都是建立在xy坐標系上的內(nèi)容可以利用pathplanning的sub2xy函數(shù)轉(zhuǎn)換
# 將行列轉(zhuǎn)成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


# 顯示圖像并設(shè)置圖像屬性
        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

C++創(chuàng)建柵格地圖矩陣的流程：

a. 創(chuàng)建全0二維數(shù)組，表示空地信息。 b. 將下標位置的柵格信息裝換成特征值。 c. C++雖然有matplotlib包，但是可視化效果并不是很好，這里不使用C++繪制柵格地圖，難度較大。

C++數(shù)組的一些基本操作：

#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}};


// 定義柵格地圖二維數(shù)組，默認初始化為0
int filed[rows][cols] = {0};


// 修改起點和終點在柵格數(shù)組中的數(shù)值，其中起點為1，終點為2
    filed[startpos[0]][startpos[1]] = 1;
    filed[endpos[0]][endpos[1]] = 2;


// 循環(huán)修改障礙物在柵格矩陣的數(shù)值，其中障礙物數(shù)值為3
int len = sizeof(obspos) / sizeof(obspos[0]);
for(int temp_len = 0;temp_len 

	

	Tips：對C++編程而言，特殊位置信息的二維數(shù)組，作為參數(shù)進行傳遞時候，是不方便編程的。后續(xù)優(yōu)化代碼，使其采用線性索引的方式進行編程，實現(xiàn)函數(shù)封裝，提升通用性。

	審核編輯：湯梓紅