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

1.1 地圖類(lèi)型

1.2 柵格地圖表示方式

a. 本欄路徑規(guī)劃利用矩陣（二維數(shù)組）來(lái)表示柵格地圖（因?yàn)閷?duì)于矩陣，無(wú)論MATLAB、C++還是Python，矩陣更適合數(shù)組的表達(dá)，更便于編程。）

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

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

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

Tips：線(xiàn)性索引，是從第一列開(kāi)始，沿著行按順序增長(zhǎng)，然后從第二列開(kāi)始…，是豎著沿行增長(zhǎng)的，而非橫著沿著列增長(zhǎng)。理解記憶有點(diǎn)類(lèi)似于Excel表格按行遞增。

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

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

對(duì)于程序

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-紅色-動(dòng)態(tài)障礙
    1 1 0;...            % 4-黃色-起始點(diǎn)
    1 0 1;...            % 5-品紅-目標(biāo)點(diǎn)
    0 1 0; ...           % 6-綠色-到目標(biāo)點(diǎn)的規(guī)劃路徑
    0 1 1];              % 7-青色-動(dòng)態(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是圓圈大小

這是因?yàn)樵跂鸥竦貓D上的坐標(biāo)系X對(duì)應(yīng)于列col，而坐標(biāo)Y對(duì)于與行row

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

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

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

②MATLAB只要用到sub2ind和sub2coord即可

③sub2xy主要是正對(duì)于Python進(jìn)行plt.plot繪圖使用的

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

03 MATLAB繪制柵格地圖

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

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

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

MATLAB繪制柵格步驟：

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

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

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

d. 添加坐標(biāo)。

3.1 colormap函數(shù)

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

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

MATLAB只要用到sub2ind和sub2coord即可

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

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

functioncoord=sub2coord(sub)
%SUB2COORD 將行列式下標(biāo)裝換為坐標(biāo)格式，此時(shí)的坐標(biāo)格式和原本認(rèn)知坐標(biāo)方向也不一致（如下所示）
%        1 2 3 4 5 6 7 .... X坐標(biāo)
%      1|——————————>
%      2|
%      3|
%      4|
%      5|
%  Y坐標(biāo)/


    [l,w] = size(sub);
    % 長(zhǎng)度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 將坐標(biāo)轉(zhuǎn)換為矩陣行列格式，坐標(biāo)格式為下圖所示
%        1 2 3 4 5 6 7 .... X坐標(biāo)
%      1|——————————>
%      2|
%      3|
%      4|
%      5|
%  Y坐標(biāo)/


    [l,w] = size(coord);
    % 長(zhǎng)度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 利用地圖線(xiàn)性值信息繪制柵格圖(FD_DrawRasterMap.m函數(shù))

線(xiàn)性索引值的目的就是省略for循環(huán)，因?yàn)榫仃嘇(線(xiàn)性索引)=int可以很方便的全部賦值，而Python/C++等需要借助循環(huán)

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

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


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


% 起始點(diǎn)和目標(biāo)點(diǎn)
start = startsub;                       % 起點(diǎn)的位置
goal = endsub;                          % 終點(diǎn)的位置
field(start(1),start(2)) = 4;           % 起點(diǎn)在數(shù)組的數(shù)值,數(shù)值用于表示Cmap的顏色
field(goal(1),goal(2)) = 5;             % 終點(diǎn)在數(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-紅色-動(dòng)態(tài)障礙
    1 1 0;...            % 4-黃色-起始點(diǎn) 
    1 0 1;...            % 5-品紅-目標(biāo)點(diǎn)
    0 1 0; ...           % 6-綠色-到目標(biāo)點(diǎn)的規(guī)劃路徑   
    0 1 1; ...           % 7-青色-動(dòng)態(tài)規(guī)劃的路徑
    0 0 1];              % 8-藍(lán)色


% 構(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

測(cè)試代碼：生成行為4列為5的矩陣，起點(diǎn)位置在3，終點(diǎn)在19，障礙物位置在10、11和12。主要目的是為了理解image函數(shù)的線(xiàn)性值和ROW和COL的坐標(biāo)關(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
% 如果出線(xiàn)障礙物信息在地圖上顯示不對(duì)
% 可能是在函數(shù)內(nèi)的field(obsIndex(:,1),obsIndex(:,2)) = 2;產(chǎn)生錯(cuò)誤
% 可以采用將行列轉(zhuǎn)成線(xiàn)性索引即函數(shù)內(nèi)該代碼下面注釋的內(nèi)容

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

04 Python繪制柵格地圖

Python繪制柵格地圖的流程：

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

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

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

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

4.1 matplotlib和seaborn包

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

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








'''
# sub2coord和coord2sub函數(shù)，行列坐標(biāo)系內(nèi)，行列位置和坐標(biāo)位置相互轉(zhuǎn)換
# 其中的【行列坐標(biāo)系】形式如圖所示，從0開(kāi)始，因?yàn)镻ython數(shù)組下標(biāo)從0開(kāi)始，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ù)，坐標(biāo)系XY位置轉(zhuǎn)換為矩陣格式的轉(zhuǎn)換函數(shù)
# ++++sz為坐標(biāo)尺寸，即元素個(gè)數(shù)，非最大坐標(biāo)值
# 其中的XY形式如圖所示，從0開(kāi)始，到[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)系，對(duì)應(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ù)，用于通過(guò)柵格地圖的信息，繪制出彩色地圖
# 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ù)
# 其實(shí)默認(rèn)為fig,ax = plt.figure(),后續(xù)發(fā)現(xiàn)fig沒(méi)有用上
# 但是ax需要頻繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(12, 8))
    ax = plt.gca()


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


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


# 將列標(biāo)簽移動(dòng)到圖像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')


# 設(shè)置圖標(biāo)的數(shù)字個(gè)數(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))


# 修改柵格地圖中起始點(diǎn)和終點(diǎn)的數(shù)值，其中起點(diǎn)以數(shù)值4表征，終點(diǎn)以數(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的熱力圖進(jìn)行繪制
# 設(shè)置色條的范圍，從0~8
    cmap = colors.ListedColormap(['none','white','black','red','yellow','magenta','green','cyan','blue'])


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




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


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


# 將列標(biāo)簽移動(dòng)到圖像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')




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


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


return field


'''
# # -------------------------------------這是是測(cè)試的主函數(shù)-------------------------------------------------
'''
rows = 4
cols = 5


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


drawmap(rows,cols,startSub,goalSub,obsSub)
# 采用熱力圖這種，坐標(biāo)系簡(jiǎn)歷和MATLAB是一致的，X=col，Y=rows，且遞增方向都是一致的，因此繪制XY時(shí)候比較難理解
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坐標(biāo)填充矩形繪制地圖

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


# 修改柵格地圖中起始點(diǎn)和終點(diǎn)的數(shù)值，其中起點(diǎn)以數(shù)值4表征，終點(diǎn)以數(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è)置畫(huà)布屬性
    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'))


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

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

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

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

b. 利用plt.plot()聯(lián)合plt.pause()和plt.cla()進(jìn)行刷新顯示

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




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


# 新建畫(huà)布指定大小
fig = plt.figure(figsize=(9,6))
# 新建子圖
ax = fig.add_subplot(111)


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






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


# 動(dòng)態(tài)刷新地圖次數(shù)
if draw:
for i in range(10):
# 隨機(jī)選擇修改地圖信息的位置
        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()
# 利用隨機(jī)函數(shù)，隨機(jī)選擇地圖位置改變數(shù)值
'''
if not draw:
# plt.plot都是建立在xy坐標(biāo)系上的內(nèi)容可以利用pathplanning的sub2xy函數(shù)轉(zhuǎn)換
# 將行列轉(zhuǎn)成xy坐標(biāo)系的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):
# 隨機(jī)選擇修改地圖信息的位置
        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. 將下標(biāo)位置的柵格信息裝換成特征值。 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ù)組，默認(rèn)初始化為0
int filed[rows][cols] = {0};


// 修改起點(diǎn)和終點(diǎn)在柵格數(shù)組中的數(shù)值，其中起點(diǎn)為1，終點(diǎn)為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：對(duì)C++編程而言，特殊位置信息的二維數(shù)組，作為參數(shù)進(jìn)行傳遞時(shí)候，是不方便編程的。后續(xù)優(yōu)化代碼，使其采用線(xiàn)性索引的方式進(jìn)行編程，實(shí)現(xiàn)函數(shù)封裝，提升通用性。

	審核編輯：湯梓紅