Cx330_HomeWork/Path_Planning
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增RTT算法

Browse Source
This commit is contained in:
张梦南 2025-06-02 19:49:22 +08:00
parent 53270692e9
commit 5d2b4d951a
1 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
homework_map.m
View File

@ -13,7 +13,7 @@ map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0; % 1
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 0 1 0; %13
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
@ -30,10 +30,12 @@ goal = [19, 19];
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);
visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT);
function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
function visualize_path(map, start, goal, path_Astar, path_Dijkstra, path_RTT)
% 可视化地图，0为空地，1为障碍
imagesc(map);
colormap(flipud(gray)); % 灰度图（0白 1黑）
@ -42,25 +44,29 @@ function visualize_path(map, start, goal, path_Astar, path_Dijkstra)
% 蓝色起点
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
% 显示路径线
plot(path_Astar(:,2), path_Astar(:,1), 'r-', 'LineWidth', 2); % 绿色路径线
plot(path_Astar(:,2), path_Astar(:,1), 'ro', 'MarkerSize', 4, 'MarkerFaceColor', 'r'); % 每个点画圈
plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
plot(path_Dijkstra(:,2), path_Dijkstra(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
% 绘制 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
% 网格线