此處機器人原理與應用初學者學習記錄
使用版本:MATLAB2019b
柵格地圖生成
地圖生成部分,此處參考博客link。由於大多柵格地圖生成都是用.txt自己編輯,太麻煩了!不如用MATLAB工具箱裏的現有函數自己畫圖生成地圖。
我特意更新了MATLAB版本,但我自己的Robotic ToolBox工具箱中沒有makemap()函數,可以在該網站下載工具箱,安裝操作很簡單。
鏈接: link.
點擊黑色按鈕即可下載,網頁打開的有點慢,但工具包不大,下載下來後在MATLAB中打開該工具包就能自動安裝。之後控制檯輸入指令rvccheck就能成功調用makemap()函數了。
比如我輸入makemap(20)後會生成20*20的地圖,接下來出現一系列畫圖操作。
我在figure窗口按p,便會提示你畫多邊形,照着指示點擊幾個點再按enter便出現如上結果。
畫完圖記得按q纔會顯示出柵格矩陣,在工作區形成的結果你可以save一下,就會形成.mat文件了。
Dijkstra實現
以下是我根據其他博客代碼作了修改,以.mat形式的柵格圖爲基礎的路徑規劃實現,具體原理可參考其他博客 link
load('E:\作業\機器人原理與應用作業\A.mat'); % import the existed map
cmap = [1 1 1; ...% 1 - white - clear cell
0 0 0; ...% 2 - black - obstacle
1 0 0; ...% 3 - red = visited
0 0 1; ...% 4 - blue - on list
0 1 0; ...% 5 - green - start
1 1 0;% 6 - yellow - destination
0 1 1]; %7
colormap(cmap);
map = zeros(23,23); %本例中makemap(23),map與後面的nrows、ncols數值都要與之對應
% Add an obstacle
map (find(A==1)) = 2; %爲1的點形成障礙物
map(1, 1) = 5; % start_coords起點
map(23, 23) = 7; % dest_coords終點
axis image;
%%
%此處數值與你的柵格圖行列對應
nrows = 23;
ncols = 23;
start_node = sub2ind(size(map), 1, 1); %起點座標
dest_node = sub2ind(size(map), 23, 23); %終點座標
% Initialize distance array
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0;
% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);
% Main Loop
while true
% Draw current map
map(start_node) = 5;
map(dest_node) = 6;
image(1.5, 1.5, map);
grid on;
axis image;
drawnow;
% Find the node with the minimum distance
[min_dist, current] = min(distanceFromStart(:));
if ((current == dest_node) || isinf(min_dist))
break;
end;
map(current) = 3;
distanceFromStart(current) = Inf;
[i, j] = ind2sub(size(distanceFromStart), current);
neighbor = [i-1,j;...
i+1,j;...
i,j+1;...
i,j-1]
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
(neighbor(:,2)<1) + (neighbor(:,2)>ncols )
locate = find(outRangetest>0);
neighbor(locate,:)=[]
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2))
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
map(neighborIndex(i)) = 4;
if distanceFromStart(neighborIndex(i))> min_dist + 1 distanceFromStart(neighborIndex(i)) = min_dist+1;
parent(neighborIndex(i)) = current;
end
end
end
end
%%
if (isinf(distanceFromStart(dest_node)))
route = [];
else
%提取路線座標
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
% 動態顯示出路線
for k = 2:length(route) - 1
map(route(k)) = 7;
pause(0.1);
image(1.5, 1.5, map);
grid on;
axis image;
end
end
A*實現
原理參考同上
% set up color map for display
load('E:\作業\機器人原理與應用作業\A.mat'); % import the existed map
cmap = [1 1 1; ...% 1 - white - clear cell
0 0 0; ...% 2 - black - obstacle
1 0 0; ...% 3 - red = visited
0 0 1; ...% 4 - blue - on list
0 1 0; ...% 5 - green - start
1 1 0;% 6 - yellow - destination
0 1 1]; %7
colormap(cmap);
map = zeros(23,23);
% Add an obstacle
map (find(A==1)) = 2;
map(1, 1) = 5; % start_coords
map(23, 23) = 7; % dest_coords
grid on;
axis image;
%%
nrows = 23;
ncols = 23;
start_node = sub2ind(size(map), 1, 1);
dest_node = sub2ind(size(map), 23, 23);
% Initialize distance array
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0;
%====================
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - 4) + abs(X - 8);
f = Inf(nrows,ncols);
f(start_node) = H(start_node);
%=======================
% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);
% Main Loop
while true
% Draw current map
map(start_node) = 5;
map(dest_node) = 6;
image(1.5, 1.5, map);
grid on;
axis image;
drawnow;
%====================
% Find the node with the minimum distance
[~, current] = min(f(:)); [min_dist, ~] = min(distanceFromStart(:));
%===================
if ((current == dest_node) || isinf(min_dist))
break;
end
map(current) = 3;
%============
f(current) =Inf;
%============
[i, j] = ind2sub(size(distanceFromStart), current);
neighbor = [i-1,j;...
i+1,j;...
i,j+1;...
i,j-1]
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
(neighbor(:,2)<1) + (neighbor(:,2)>ncols )
locate = find(outRangetest>0);
neighbor(locate,:)=[]
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2))
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
map(neighborIndex(i)) = 4;
if distanceFromStart(neighborIndex(i))> min_dist + 1
distanceFromStart(neighborIndex(i)) = min_dist+1;
parent(neighborIndex(i)) = current;
f(neighborIndex(i)) =H(neighborIndex(i));
end
end
end
end
%%
if (isinf(distanceFromStart(dest_node)))
route = [];
else
%提取路線座標
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
% 動態顯示出路線
for k = 2:length(route) - 1
map(route(k)) = 7;
pause(0.1);
image(1.5, 1.5, map);
grid on;
axis image;
end
end
以上代碼借鑑了一些文章,僅供學習交流