机器人路径规划算法之A*算法详解＋MATLAB代码实现

目录一、A*算法核心原理1. 算法概述2. 核心公式3. 启发函数 h(n) 的性质4. 常用启发函数二、A*算法步骤详解1. 算法流程2. 与Dijkstra的对比三、MATLAB实现A*算法一、A*算法核心原理1.算法概述A*算法是一种启发式搜索算法结合了Dijkstra的最优性保证和贪心搜索的高效性。它的核心思想是通过启发函数*引导搜索方向优先探索最有可能到达目标的节点。2.核心公式A*使用评价函数 f(n)*​ 来决定节点的优先级f(n) g(n) h(n)其中g(n)从起点到节点n的实际代价h(n)从节点n到终点的估计代价启发函数f(n)通过节点n的路径的总估计代价3.启发函数 h(n) 的性质可采纳性(Admissibility)h(n) ≤ 实际最短距离保证找到最优解一致性(Consistency)h(n) ≤ c(n, n) h(n)其中c是移动代价保证高效4.常用启发函数曼哈顿距离适用于4方向移动h |x1 - x2| |y1 - y2|欧几里得距离适用于8方向移动h sqrt((x1 - x2)² (y1 - y2)²)切比雪夫距离适用于国王移动8方向h max(|x1 - x2|, |y1 - y2|)二、A*算法步骤详解1.算法流程1. 初始化 - 将起点加入开放列表(open list) - g(起点)0, f(起点)h(起点) - 关闭列表(closed list)为空 2. 循环直到开放列表为空 a. 从开放列表中选择f值最小的节点作为当前节点 b. 将当前节点移到关闭列表 c. 如果当前节点是目标重建路径并返回 d. 对当前节点的每个邻居 - 如果邻居在关闭列表或不可通过跳过 - 计算临时g值 g(当前) 移动代价 - 如果邻居不在开放列表或临时g值 g(邻居) * 更新g(邻居) 临时g值 * 计算h(邻居) 启发函数(邻居, 目标) * 计算f(邻居) g(邻居) h(邻居) * 设置父节点为当前节点 * 如果邻居不在开放列表加入开放列表 3. 如果开放列表为空但未找到路径返回失败2.与Dijkstra的对比特性DijkstraA*搜索策略​均匀扩展所有方向向目标方向优先扩展启发函数​无(h0)有(h启发估计)效率​较低探索节点多较高启发式引导最优性​保证最优保证最优(启发函数可采纳)内存使用​高较低三、MATLAB实现A*算法%% 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); % 将起点加入开放列表 [f_score, g_score, row, col] 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]); % 子图14方向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); % 子图28方向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); % 子图44方向代价地图 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); % 子图58方向代价地图 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输出内容命令行输出 A*算法路径规划演示 1. 创建20x20网格地图... 起点: (2, 2) 终点: (18, 18) 2. 运行A*算法... 启发函数: 曼哈顿距离 移动方式: 4方向 规划完成耗时: 0.0079秒 3. 运行8方向A*算法对比... 启发函数: 对角线距离 规划完成耗时: 0.0085秒 4. 可视化结果... 可视化完成 结果对比 性能指标对比: 算法 4方向A* 8方向A* 扩展节点数: 198 99 规划时间(s): 0.0079 0.0085 路径长度: 34 22 效率提升: 0.0 % 50.0 % 程序执行完成 A*算法原理总结: 1. 评价函数: f(n) g(n) h(n) 2. g(n): 从起点到n的实际代价 3. h(n): 从n到终点的启发估计曼哈顿/对角线距离 4. 开放列表: 存储待探索节点按f值排序 5. 关闭列表: 存储已探索节点 6. 保证最优性: 启发函数h(n) ≤ 实际最短距离可采纳性图形窗口6个子图4方向A*路径结果8方向A*路径结果两种路径对比4方向实际代价地图8方向实际代价地图性能对比柱状图A算法的优势*高效性通过启发函数引导搜索减少探索节点最优性在可采纳启发函数下保证找到最短路径灵活性可通过不同启发函数适应不同场景完备性如果存在路径一定能够找到4方向 vs 8方向4方向只允许上下左右移动路径可能更长但更符合某些机器人运动约束8方向允许对角线移动路径更短更自然但需要考虑切角问题机器人应用% 实际机器人路径规划考虑因素 % 1. 机器人尺寸将机器人膨胀到障碍物 % 2. 运动学约束考虑转弯半径 % 3. 动态障碍物结合D*或终身规划A*(LPA*) % 4. 多层规划全局A* 局部DWA避障