%% ==================== A*算法路径规划 MATLAB 实现 ====================
clear; clc; close all;
fprintf('========== A*算法路径规划演示 ==========\n');
%% 1. 创建网格地图
fprintf('1. 创建 20x20 网格地图...\n');
map_size = 20;
grid_map = zeros(map_size, map_size); % 添加障碍物
grid_map(8, 3:18) = 1; % 水平墙
grid_map(5:15, 10) = 1; % 垂直墙
grid_map(12:16, 15:18) = 1; % 方块障碍
% 添加随机障碍物
rng(42);
num_obstacles = 30;
for i = 1:num_obstacles
r = randi([1, map_size]);
c = randi([1, map_size]);
grid_map(r, c) = 1;
end
% 设置起点和终点
start_pos = [2, 2];
goal_pos = [18, 18];
grid_map(start_pos(1), start_pos(2)) = 0;
grid_map(goal_pos(1), goal_pos(2)) = 0;
fprintf(' 起点：%d, %d\n', start_pos(1), start_pos(2));
fprintf(' 终点：%d, %d\n', goal_pos(1), goal_pos(2));
%% 2. 运行 A*算法
fprintf('\n2. 运行 A*算法...\n');
fprintf(' 启发函数：曼哈顿距离\n');
fprintf(' 移动方式：4 方向\n');
tic;
[path, nodes_expanded, g_scores] = a_star_4dir(grid_map, start_pos, goal_pos);
plan_time = toc;
fprintf(' 规划完成！耗时：%.4f 秒\n', plan_time);
%% 3. 运行 8 方向 A*算法对比
fprintf('\n3. 运行 8 方向 A*算法对比...\n');
fprintf(' 启发函数：对角线距离\n');
tic;
[path_8dir, nodes_expanded_8dir, g_scores_8dir] = a_star_8dir(grid_map, start_pos, goal_pos);
plan_time_8dir = toc;
fprintf(' 规划完成！耗时：%.4f 秒\n', plan_time_8dir);
%% 4. 可视化结果
fprintf('\n4. 可视化结果...\n');
visualize_a_star_results(grid_map, start_pos, goal_pos, path, path_8dir, ...
nodes_expanded, nodes_expanded_8dir, ...
g_scores, g_scores_8dir, plan_time, plan_time_8dir);
%% ==================== 4 方向 A*算法实现 ====================
function [path, nodes_expanded, g_scores] = a_star_4dir(grid_map, start_pos, goal_pos)
% A*算法实现（4 方向移动，曼哈顿距离）
% 输入：grid_map - 网格地图（0=空闲，1=障碍物）
% start_pos - 起点 [行，列]
% goal_pos - 终点 [行，列]
% 输出：path - 路径坐标
% nodes_expanded - 扩展的节点数
% g_scores - 实际代价矩阵
[rows, cols] = size(grid_map);
% 初始化数据结构
open_list = []; % 待探索节点列表
closed_list = false(rows, cols); % 已探索节点标记
g_score = inf(rows, cols); % 实际代价
f_score = inf(rows, cols); % 估计总代价
parent = zeros(rows, cols, 2); % 父节点坐标
% 设置起点
g_score(start_pos(1), start_pos(2)) = 0;
f_score(start_pos(1), start_pos(2)) = heuristic_manhattan(start_pos, goal_pos);
% 将起点加入开放列表
open_list = [open_list; f_score(start_pos(1), start_pos(2)), ...
g_score(start_pos(1), start_pos(2)), ...
start_pos(1), start_pos(2)];
% 4 个移动方向
directions = [-1, 0; 1, 0; 0, -1; 0, 1];
nodes_expanded = 0;
path = [];
while ~isempty(open_list)
% 从开放列表中找到 f 值最小的节点
[~, min_idx] = min(open_list(:, 1));
current_node = open_list(min_idx, 3:4);
current_g = open_list(min_idx, 2);
% 从开放列表移除
open_list(min_idx, :) = [];
% 如果已探索过，跳过
if closed_list(current_node(1), current_node(2))
continue;
end
% 标记为已探索
closed_list(current_node(1), current_node(2)) = true;
nodes_expanded = nodes_expanded + 1;
% 如果到达目标，重建路径
if isequal(current_node, goal_pos)
path = reconstruct_path(parent, start_pos, goal_pos);
break;
end
% 探索 4 个邻居方向
for d = 1:4
neighbor_row = current_node(1) + directions(d, 1);
neighbor_col = current_node(2) + directions(d, 2);
% 检查边界
if neighbor_row < 1 || neighbor_row > rows || ...
neighbor_col < 1 || neighbor_col > cols
continue;
end
% 检查障碍物
if grid_map(neighbor_row, neighbor_col) == 1
continue;
end
% 计算临时 g 值
tentative_g = current_g + 1; % 移动代价为 1
% 如果找到更好路径
if tentative_g < g_score(neighbor_row, neighbor_col)
% 更新父节点
parent(neighbor_row, neighbor_col, :) = current_node;
% 更新 g 值和 f 值
g_score(neighbor_row, neighbor_col) = tentative_g;
h = heuristic_manhattan([neighbor_row, neighbor_col], goal_pos);
f = tentative_g + h;
f_score(neighbor_row, neighbor_col) = f;
% 添加到开放列表
open_list = [open_list; f, tentative_g, neighbor_row, neighbor_col];
end
end
end
g_scores = g_score;
g_scores(g_scores == inf) = NaN;
end
%% ==================== 8 方向 A*算法实现 ====================
function [path, nodes_expanded, g_scores] = a_star_8dir(grid_map, start_pos, goal_pos)
% A*算法实现（8 方向移动，对角线距离）
% 输入：grid_map - 网格地图
% start_pos - 起点
% goal_pos - 终点
% 输出：路径、扩展节点数、代价矩阵
[rows, cols] = size(grid_map);
% 初始化
open_list = [];
closed_list = false(rows, cols);
g_score = inf(rows, cols);
f_score = inf(rows, cols);
parent = zeros(rows, cols, 2);
g_score(start_pos(1), start_pos(2)) = 0;
f_score(start_pos(1), start_pos(2)) = heuristic_diagonal(start_pos, goal_pos);
open_list = [open_list; f_score(start_pos(1), start_pos(2)), ...
g_score(start_pos(1), start_pos(2)), ...
start_pos(1), start_pos(2)];
% 8 个移动方向及其代价
% 前 4 个：上下左右（代价 1）
% 后 4 个：对角线（代价√2≈1.414）
directions = [-1, 0, 1.0; % 上
1, 0, 1.0; % 下
0, -1, 1.0; % 左
0, 1, 1.0; % 右
-1, -1, 1.414; % 左上
-1, 1, 1.414; % 右上
1, -1, 1.414; % 左下
1, 1, 1.414]; % 右下
nodes_expanded = 0;
path = [];
while ~isempty(open_list)
% 找到 f 值最小的节点
[~, min_idx] = min(open_list(:, 1));
current_node = open_list(min_idx, 3:4);
current_g = open_list(min_idx, 2);
open_list(min_idx, :) = [];
if closed_list(current_node(1), current_node(2))
continue;
end
closed_list(current_node(1), current_node(2)) = true;
nodes_expanded = nodes_expanded + 1;
if isequal(current_node, goal_pos)
path = reconstruct_path(parent, start_pos, goal_pos);
break;
end
% 探索 8 个邻居方向
for d = 1:size(directions, 1)
neighbor_row = current_node(1) + directions(d, 1);
neighbor_col = current_node(2) + directions(d, 2);
move_cost = directions(d, 3);
% 检查边界
if neighbor_row < 1 || neighbor_row > rows || ...
neighbor_col < 1 || neighbor_col > cols
continue;
end
% 检查障碍物
if grid_map(neighbor_row, neighbor_col) == 1
continue;
end
% 对于对角线移动，检查是否'切角'
if d > 4
% 检查两个相邻单元格是否都是障碍物
if grid_map(current_node(1), neighbor_col) == 1 && ...
grid_map(neighbor_row, current_node(2)) == 1
continue;
end
end
% 计算临时 g 值
tentative_g = current_g + move_cost;
if tentative_g < g_score(neighbor_row, neighbor_col)
parent(neighbor_row, neighbor_col, :) = current_node;
g_score(neighbor_row, neighbor_col) = tentative_g;
h = heuristic_diagonal([neighbor_row, neighbor_col], goal_pos);
f = tentative_g + h;
f_score(neighbor_row, neighbor_col) = f;
open_list = [open_list; f, tentative_g, neighbor_row, neighbor_col];
end
end
end
g_scores = g_score;
g_scores(g_scores == inf) = NaN;
end
%% ==================== 启发函数 ====================
function h = heuristic_manhattan(pos1, pos2)
% 曼哈顿距离（4 方向移动适用）
h = abs(pos1(1) - pos2(1)) + abs(pos1(2) - pos2(2));
end
function h = heuristic_diagonal(pos1, pos2)
% 对角线距离（8 方向移动适用）
dx = abs(pos1(1) - pos2(1));
dy = abs(pos1(2) - pos2(2));
h = 1.0 * (dx + dy) + (1.414 - 2 * 1.0) * min(dx, dy);
end
function h = heuristic_euclidean(pos1, pos2)
% 欧几里得距离
h = sqrt((pos1(1) - pos2(1))^2 + (pos1(2) - pos2(2))^2);
end
%% ==================== 工具函数 ====================
function path = reconstruct_path(parent, start_pos, goal_pos)
% 重建路径
path = [];
current = goal_pos;
while ~isequal(current, [0, 0])
path = [current; path];
prev = parent(current(1), current(2), :);
current = [prev(1), prev(2)];
end
% 验证路径有效性
if isempty(path) || ~isequal(path(1, :), start_pos)
path = [];
end
end
%% ==================== 可视化函数 ====================
function visualize_a_star_results(grid_map, start_pos, goal_pos, path_4dir, path_8dir, ...
nodes_exp_4, nodes_exp_8, g_4, g_8, time_4, time_8)
% 创建可视化窗口
figure('Position', [50, 50, 1400, 900]);
% 子图 1：4 方向 A*结果
subplot(2, 3, 1);
imagesc(grid_map);
colormap(gca, [1 1 1; 0.3 0.3 0.3]);
hold on;
plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
if ~isempty(path_4dir)
plot(path_4dir(:,2), path_4dir(:,1), 'b-', 'LineWidth', 2);
plot(path_4dir(:,2), path_4dir(:,1), 'bo', 'MarkerSize', 5, 'MarkerFaceColor', 'b');
end
axis equal tight;
grid on;
set(gca, 'YDir', 'reverse');
title(sprintf('4 方向 A*路径\n步数：%d, 扩展节点：%d', ...
size(path_4dir,1)-1, nodes_exp_4), 'FontSize', 11, 'FontWeight', 'bold');
xlabel('X'); ylabel('Y');
% 子图 2：8 方向 A*结果
subplot(2, 3, 2);
imagesc(grid_map);
colormap(gca, [1 1 1; 0.3 0.3 0.3]);
hold on;
plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
if ~isempty(path_8dir)
plot(path_8dir(:,2), path_8dir(:,1), 'm-', 'LineWidth', 2);
plot(path_8dir(:,2), path_8dir(:,1), 'mo', 'MarkerSize', 5, 'MarkerFaceColor', 'm');
end
axis equal tight;
grid on;
set(gca, 'YDir', 'reverse');
title(sprintf('8 方向 A*路径\n步数：%d, 扩展节点：%d', ...
size(path_8dir,1)-1, nodes_exp_8), 'FontSize', 11, 'FontWeight', 'bold');
xlabel('X'); ylabel('Y');
% 子图 3：路径对比
subplot(2, 3, 3);
imagesc(grid_map);
colormap(gca, [1 1 1; 0.3 0.3 0.3]);
hold on;
plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
if ~isempty(path_4dir)
plot(path_4dir(:,2), path_4dir(:,1), 'b-', 'LineWidth', 2, 'DisplayName', '4 方向');
end
if ~isempty(path_8dir)
plot(path_8dir(:,2), path_8dir(:,1), 'm-', 'LineWidth', 2, 'DisplayName', '8 方向');
end
axis equal tight;
grid on;
set(gca, 'YDir', 'reverse');
title('路径对比', 'FontSize', 11, 'FontWeight', 'bold');
xlabel('X'); ylabel('Y');
legend('Location', 'best');
% 子图 4：4 方向代价地图
subplot(2, 3, 4);
% 处理 NaN 值
g_4_plot = g_4;
g_4_plot(isnan(g_4_plot)) = max(g_4_plot(~isnan(g_4_plot))) + 1;
imagesc(g_4_plot);
colormap(gca, 'hot');
colorbar;
hold on;
plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
if ~isempty(path_4dir)
plot(path_4dir(:,2), path_4dir(:,1), 'c-', 'LineWidth', 1.5);
end
axis equal tight;
set(gca, 'YDir', 'reverse');
title('4 方向实际代价 (g 值)', 'FontSize', 11, 'FontWeight', 'bold');
xlabel('X'); ylabel('Y');
% 子图 5：8 方向代价地图
subplot(2, 3, 5);
g_8_plot = g_8;
g_8_plot(isnan(g_8_plot)) = max(g_8_plot(~isnan(g_8_plot))) + 1;
imagesc(g_8_plot);
colormap(gca, 'hot');
colorbar;
hold on;
plot(start_pos(2), start_pos(1), 'gs', 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'LineWidth', 2);
plot(goal_pos(2), goal_pos(1), 'rs', 'MarkerSize', 12, 'MarkerFaceColor', 'r', 'LineWidth', 2);
if ~isempty(path_8dir)
plot(path_8dir(:,2), path_8dir(:,1), 'c-', 'LineWidth', 1.5);
end
axis equal tight;
set(gca, 'YDir', 'reverse');
title('8 方向实际代价 (g 值)', 'FontSize', 11, 'FontWeight', 'bold');
xlabel('X'); ylabel('Y');
% 子图 6：性能比较
subplot(2, 3, 6);
categories = {'4 方向 A*', '8 方向 A*'};
nodes_data = [nodes_exp_4, nodes_exp_8];
time_data = [time_4 * 1000, time_8 * 1000]; % 转换为毫秒
if ~isempty(path_4dir) && ~isempty(path_8dir)
path_len_data = [size(path_4dir,1)-1, size(path_8dir,1)-1];
yyaxis left;
bar(1:2, nodes_data, 0.6, 'FaceColor', [0.2 0.6 0.8]);
ylabel('扩展节点数', 'FontSize', 10);
hold on;
yyaxis right;
plot(1:2, path_len_data, 'r-o', 'LineWidth', 2, 'MarkerSize', 8, 'MarkerFaceColor', 'r');
ylabel('路径长度（步）', 'FontSize', 10);
set(gca, 'XTick', 1:2, 'XTickLabel', categories);
grid on;
title('性能对比', 'FontSize', 11, 'FontWeight', 'bold');
% 添加数值标签
yyaxis left;
for i = 1:2
text(i, nodes_data(i), sprintf('%d', nodes_data(i)), ...
'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom', ...
'FontSize', 9, 'FontWeight', 'bold');
end
yyaxis right;
for i = 1:2
text(i, path_len_data(i), sprintf('%d', path_len_data(i)), ...
'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom', ...
'FontSize', 9, 'FontWeight', 'bold', 'Color', 'r');
end
legend({'扩展节点数', '路径长度'}, 'Location', 'best');
else
text(0.5, 0.5, '路径不存在，无法比较', ...
'HorizontalAlignment', 'center', 'FontSize', 11, 'FontWeight', 'bold');
axis off;
end
% 添加整体标题
sgtitle('A*算法路径规划结果对比', 'FontSize', 14, 'FontWeight', 'bold');
fprintf(' 可视化完成！\n');
fprintf('\n========== 结果对比 ==========\n\n');
fprintf('性能指标对比:\n');
fprintf('%-15s %-15s %-15s\n', '算法', '4 方向 A*', '8 方向 A*');
fprintf('%-15s %-15d %-15d\n', '扩展节点数:', nodes_exp_4, nodes_exp_8);
fprintf('%-15s %-15.4f %-15.4f\n', '规划时间 (s):', time_4, time_8);
if ~isempty(path_4dir) && ~isempty(path_8dir)
fprintf('%-15s %-15d %-15d\n', '路径长度:', size(path_4dir,1)-1, size(path_8dir,1)-1);
fprintf('%-15s %-15.1f%% %-15.1f%%\n', '效率提升:', 0, ...
100*(nodes_exp_4 - nodes_exp_8)/nodes_exp_4);
end
fprintf('\n========== 程序执行完成 ==========\n\n');
% 显示算法原理总结
fprintf('A*算法原理总结:\n');
fprintf('1. 评价函数：f(n) = g(n) + h(n)\n');
fprintf('2. g(n): 从起点到 n 的实际代价\n');
fprintf('3. h(n): 从 n 到终点的启发估计（曼哈顿/对角线距离）\n');
fprintf('4. 开放列表：存储待探索节点，按 f 值排序\n');
fprintf('5. 关闭列表：存储已探索节点\n');
fprintf('6. 保证最优性：启发函数 h(n) ≤ 实际最短距离（可采纳性）\n');
end
