90 lines
3.2 KiB
Matlab
90 lines
3.2 KiB
Matlab
% ！！！ 运行代码需将Astar和Dijkstra文件夹添加到设置路径中 ！！！
|
||
% 创建地图
|
||
% 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
|
||
map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0; % 1
|
||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0; % 2
|
||
0 0 0 1 1 1 1 0 0 1 0 0 0 0 0 1 1 0 0 0; % 3
|
||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0; % 4
|
||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0; % 5
|
||
1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 1 0 0 1 1; % 6
|
||
1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 1; % 7
|
||
1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0; % 8
|
||
1 0 0 0 1 1 1 1 1 1 0 0 1 1 0 0 0 0 0 0; % 9
|
||
1 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0; %10
|
||
0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0; %11
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0; %12
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; %13
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %14
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %15
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %16
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1; %17
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; %18
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; %19
|
||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; %20
|
||
|
||
% 设置起点和终点
|
||
start = [2, 2];
|
||
goal = [19, 19];
|
||
|
||
% 调用A*算法
|
||
path_Astar = Astar(map, start, goal);
|
||
% 调用Dijkstra算法
|
||
path_Dijkstra = Dijkstras(map, start, goal);
|
||
%调用RTT算法
|
||
path_RTT = RTT(map, start, goal);
|
||
|
||
visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT);
|
||
|
||
function visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT)
|
||
% 可视化地图，0为空地，1为障碍
|
||
imagesc(map);
|
||
colormap(flipud(gray)); % 灰度图（0白 1黑）
|
||
axis equal tight;
|
||
hold on;
|
||
|
||
% 蓝色起点
|
||
plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
|
||
% 红色终点
|
||
plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
|
||
|
||
% 绘制 A* 路径
|
||
if ~isempty(path_Astar)
|
||
if ~isequal(path_Astar(1,:), start)
|
||
path_Astar = [start; path_Astar];
|
||
end
|
||
if ~isequal(path_Astar(end,:), goal)
|
||
path_Astar = [path_Astar; goal];
|
||
end
|
||
plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2);
|
||
plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r');
|
||
end
|
||
|
||
% 绘制 Dijkstra 路径
|
||
if ~isempty(path_Dijkstra)
|
||
plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2);
|
||
plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g');
|
||
end
|
||
% 绘制 RTT 路径
|
||
if ~isempty(path_RTT)
|
||
plot(path_RTT(:,2), path_RTT(:,1), 'b-', 'LineWidth', 2);
|
||
end
|
||
|
||
% 网格线
|
||
[rows, cols] = size(map);
|
||
for i = 1:rows+1
|
||
line([0.5, cols+0.5], [i-0.5, i-0.5], 'Color', 'k'); % 横线
|
||
end
|
||
for j = 1:cols+1
|
||
line([j-0.5, j-0.5], [0.5, rows+0.5], 'Color', 'k'); % 竖线
|
||
end
|
||
|
||
set(gca, ...
|
||
'XTick', 1:cols, ...
|
||
'YTick', 1:rows, ...
|
||
'XTickLabel', 1:cols, ...
|
||
'YTickLabel', 1:rows, ...
|
||
'XAxisLocation', 'top', ... % x轴
|
||
'YDir', 'reverse'); % y轴
|
||
title('地图');
|
||
hold off;
|
||
end |