clear
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 地图建模
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MAX0 = [
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 0;
0 0 0 0 0 0 1 1 0 0 0 0 1 0 0 1 0 0 0 0;
0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 1 0 0 0 0;
0 1 1 1 0 0 0 1 1 1 0 0 0 1 0 1 0 0 0 0;
0 1 1 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 1 0 0 0 1 1 0 0 0 0 0 0;
0 0 0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 1 0 0 0 0 0 0 1 1 0 0 1 1 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 1 1 0;
0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0;
0 0 1 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 1 1 1 0 0 0 0 0 1 1 0;
0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 1 1 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;];
MAX = rot90(MAX0, 3); % 逆时针旋转270度
MAX_X = size(MAX, 2);
MAX_Y = size(MAX, 1);
figure
axis([1 MAX_X+1, 1 MAX_Y+1])
set(gca, 'xtick', 1:1:MAX_X+1, 'ytick', 1:1:MAX_Y+1, ...
'GridLineStyle', '-', 'xGrid', 'on', 'yGrid', 'on')
grid on;
hold on;
k = 1;
CLOSED = [];
for j = 1:MAX_X
for i = 1:MAX_Y
if (MAX(i, j) == 1)
fill([i, i+1, i+1, i], [j, j, j+1, j+1], 'k');
CLOSED(k, :) = [i, j];
k = k + 1;
end
end
end
Area_MAX = [MAX_X, MAX_Y];
Obs_Closed = CLOSED;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 设置起始点和目标点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = msgbox('请使用鼠标左键选择目标');
uiwait(h, 5);
if ishandle(h) == 1
delete(h);
end
but = 0;
while (but ~= 1)
[xval, yval, but] = ginput(1);
xval = floor(xval);
yval = floor(yval);
end
xTarget=xval;%X Coordinate of the Target %%% 目标的坐标
yTarget=yval;%Y Coordinate of the Target
plot(xval+.5, yval+.5, 'go');
text(xval+1, yval+1, 'Target');
h = msgbox('请使用鼠标左键选择车辆初始位置');
uiwait(h, 5);
if ishandle(h) == 1
delete(h);
end
but = 0;
while (but ~= 1)
[xval, yval, but] = ginput(1);
xval = floor(xval);
yval = floor(yval);
end
xStart=xval;%Starting Position
yStart=yval;%Starting Position
plot(xval+.5, yval+.5, 'b^');
text(xval+1, yval+1, 'Start');
Start=[xStart yStart];
Goal=[xTarget yTarget];
% time_Dijkstra = 0;
% time_AStar_ = 0;
% time_ = 0;
% time_BFS = 0;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% 算法路径全局规划
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% tic; % 开始计时
% [Line_path, distance_x, OPEN_num] = Astar(Obs_Closed, Start, Goal, MAX_X, MAX_Y);
% toc; % 结束计时
%
% % 绘制路径
% plot(Line_path(:, 1)+.5, Line_path(:, 2)+.5, 'b-', 'LineWidth', 1.5);
% plot(xval+.5,yval+.5,'b^');
% plot(Goal(1,1)+.5,Goal(1,2)+.5,'bo');
% % 计算路径长度
% S = 0;
% for i = 1:size(Line_path, 1)-1
% Dist = sqrt((Line_path(i, 1) - Line_path(i+1, 1))^2 + (Line_path(i, 2) - Line_path(i+1, 2))^2);
% S = S + Dist;
% end
% % text(1, MAX_Y, ['Path Length: ', num2str(S)], 'Color', 'black', 'FontSize', 10);
% % 计算转折点个数
% turn_count = 0;
% for i = 2:size(Line_path, 1)-1
% dx1 = Line_path(i, 1) - Line_path(i-1, 1);
% dy1 = Line_path(i, 2) - Line_path(i-1, 2);
% dx2 = Line_path(i+1, 1) - Line_path(i, 1);
% dy2 = Line_path(i+1, 2) - Line_path(i, 2);
% if (dx1 * dy2 - dy1 * dx2) ~= 0
% turn_count = turn_count + 1;
% end
% end
%
% % 输出结果
% % disp('路径长度:');
% % disp(S);
% % disp('转折点个数:');
% % disp(turn_count);
%
% % 获取运行时间
% elapsed_time = toc; % 假设之前已经用tic开始计时
% % 在命令窗口中显示结果
% fprintf('Algorithm: A*\n');
% fprintf('Time: %.6f seconds\n', toc);
% fprintf('Path Length: %.2f meters\n', S);
% fprintf('Turn Points: %d\n', turn_count);
%
% % % 或者弹出一个对话框显示结果
% % result_text = sprintf('Algorithm: A*\nTime: %.6f seconds\nPath Length: %.2f meters\nTurn Points: %d', ...
% % toc, S, turn_count);
% % msgbox(result_text, 'Path Planning Results');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 算法对比实验主程序修改部分
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义算法名称和参数
algorithms = {
{@Dijkstra, 'r--', 'Dijkstra'},{@Astar_traditional, 'b-', 'A*'}, {@Astar_, 'y-','未路径后处理A*算法'},{@Astar_G_du, 'g:', '改进A*'}};
% 创建新图窗并保持原有地图绘制
figure(1)
clf
axis([1 MAX_X+1, 1 MAX_Y+1])
grid on;
hold on;
% 重绘障碍物基础地图
for j = 1:MAX_X
for i = 1:MAX_Y
if (MAX(i, j) == 1)
fill([i, i+1, i+1, i], [j, j, j+1, j+1], 'k');
end
end
end
% 绘制起点终点
plot(xStart+.5, yStart+.5, 'b^', 'MarkerSize', 10, 'LineWidth', 2)
text(xval+1, yval+1, 'Start');
plot(xTarget+.5, yTarget+.5, 'go', 'MarkerSize', 10, 'LineWidth', 2)
text(xval+1, yval+1, 'Target');
% 预分配结果存储
results = struct('name', {}, 'time', {}, 'path_length', {}, 'turn_count', {});
% 循环执行不同算法
for i = 1:length(algorithms)
% 执行算法
tic;
[Line_path, ~, ~] = algorithms{i}{1}(Obs_Closed, Start, Goal, MAX_X, MAX_Y);
elapsed_time = toc;
% 计算结果指标
if ~isempty(Line_path)
% 计算路径长度
S = sum(sqrt(diff(Line_path(:,1)).^2 + diff(Line_path(:,2)).^2));
% 计算转折点
turn_count = 0;
for j = 2:size(Line_path,1)-1
dx1 = Line_path(j,1) - Line_path(j-1,1);
dy1 = Line_path(j,2) - Line_path(j-1,2);
dx2 = Line_path(j+1,1) - Line_path(j,1);
dy2 = Line_path(j+1,2) - Line_path(j,2);
if (dx1*dy2 - dy1*dx2) ~= 0
turn_count = turn_count + 1;
end
end
% 绘制路径
plot(Line_path(:,1)+.5, Line_path(:,2)+.5, algorithms{i}{2},...
'LineWidth', 1.5, 'DisplayName', algorithms{i}{3});
else
S = NaN;
turn_count = NaN;
end
% 存储结果
results(end+1) = struct(...
'name', algorithms{i}{3},...
'time', elapsed_time,...
'path_length', S,...
'turn_count', turn_count);
end
% 添加图例和标题
legend('Location', 'bestoutside','FontSize',12)
title('Path Planning Algorithm Comparison','FontSize',14)
hold off
% 输出结果表格
fprintf('\n%-10s | %-12s | %-12s | %-12s\n', 'Algorithm', 'Time(s)', 'Length(m)', 'Turns');
fprintf('-----------------------------------------------\n');
for i = 1:length(results)
fprintf('%-10s | %-12.6f | %-12.2f | %-12d\n',...
results(i).name,...
results(i).time,...
results(i).path_length,...
results(i).turn_count);
end上面的图例中只显示四种算法,该如何改?其他的不动