做移动机器人开发这几年我数不清处理过多少条路径规划的需求。从差速底盘在仓库里的避障到AGV在车间里的调度最早让我觉得“终于有谱了”的算法就是A*。不管是想读机器人方向的研究生还是准备算法岗面试又或者只是想把课程作业做得像样一点A算法都是绕不开的基石。这篇文章我会把A的来龙去脉、栅格地图建模、fgh的每一个细节以及一份能在MATLAB里直接跑的完整代码从头到尾讲一遍。看完之后你可以立刻复现这段代码改改地图和起终点观察路径变化。1. 从Dijkstra到A*启发式搜索为什么更快1.1 Dijkstra的“地毯式搜索”到底慢在哪Dijkstra算法本身没有错它保证能找到最短路径问题是它完全不看终点的方向。每次迭代它只选当前累计代价g最小的节点向外扩展从起点发起的搜索波前会均匀地向四面八方扩散。假设起点在栅格地图左上角终点在右下角中间没有障碍物。Dijkstra依然会花大量时间去扩展左下方、右上方这些明显“跑偏”的节点等波前终于碰到终点时大半张地图已经被扫过一遍。空旷场景下这种无差别搜索浪费严重地图越大越明显。我经常用一个生活类比Dijkstra像是你在陌生城市找最近的奶茶店手里只有一张标了街道长度的地图没有方向提示于是你把以起点为圆心的所有街道都扫一遍走一步算一步直到奶茶店出现在视野里。一定能找到但绕的路太多。1.2 启发式h(n)给搜索装上了“方向感”A和Dijkstra唯一的区别就是在排序标准里加了一个启发项h(n)表示从当前节点n到终点还需要的大致代价。A按f(n)g(n)h(n)排序每次优先扩展f最小的节点。还是找奶茶店的例子。现在你手里多了一个指南针能直接看到奶茶店在东南方向你自然优先往东南方向找而不是把西北方向的街道也全部翻一遍。h(n)就是那个指南针它不需要知道路上有没有障碍只需要给出一个基于几何位置的估计值。在栅格地图上这个“估计值”通常用当前点和终点的直线距离、曼哈顿距离或切比雪夫距离来计算。关键是h(n)不能“高估”真实代价我们把这种性质叫可采纳性。只要h可采纳A*第一次从OpenList中取出终点时回溯得到的路径就一定是最优的。1.3 三种搜索算法只看代价、只看启发、两者都看算法排序依据是否保证最优搜索范围贪心最佳优先BFS类只按h排序不保证容易走进死胡同很小速度快Dijkstra只按g排序保证最优大向四周均匀扩散A*按gh排序h可采纳时保证最优比Dijkstra小得多有个很直观的结论当h(n)恒等于0时A就完全退化成Dijkstra当h(n)远大于实际代价时A又变得像贪心搜索速度上来了但可能丢掉最优解。所以启发函数的设计本质是在“效率”和“最优性”之间做权衡。面试里经常被问“A*一定能找到最短路径吗”标准答案必须加前提——启发函数可采纳。这个细节很容易忽略但工程上非常重要。2. 栅格地图上的fgh建模细节直接决定路径质量2.1 地图的矩阵表示和坐标约定栅格地图在MATLAB里最直观的表示就是二维矩阵0代表可通行1代表障碍。例如map [0 0 0 1 0; 0 1 0 1 0; 0 1 0 0 0];这里map(r,c)中行r对应平面上的y坐标列c对应x坐标和数学里“先x后y”的习惯正好相反。这个问题放在最后画图时会给你带来很大困扰我见过不少人在plot时把行列写反导致路径像照了镜子一样左右翻转却怎么也找不出原因。2.2 4邻域、8邻域与移动代价路径方向由邻域定义决定4邻域只能上下左右移动每步移动代价为1。8邻域允许对角移动水平和垂直方向代价为1对角线方向代价为sqrt(2)。我在工程里默认推荐8邻域因为路径更平滑转弯更自然。但8邻域有一个隐患就是机器人可以斜着穿过墙角缝。比如当前格左方是障碍、上方是障碍如果不对角移动做检查A*会规划出一条贴着墙角穿过去的路径真实机器人根本没法走。处理方式是在获取邻居时增加穿墙检查只有当前格与对角格之间的两个正交邻居都可行时才允许进入对角格。这个细节看起来小实际项目中非常重要。2.3 三种启发函数怎么选启发函数公式适用场景特点曼哈顿距离abs(dr)abs(dc)4邻域代价紧凑搜索较广欧几里得距离norm([dr,dc])8邻域路径平滑推荐切比雪夫距离max(abs(dr),abs(dc))8邻域计算最快易走斜线选择启发函数的核心逻辑很简单h(n)绝不能高估真实剩余代价否则会丢掉最优解。在8邻域下每次移动代价最多是sqrt(2)欧氏距离给出的估计通常不高估真实代价所以可以放心用。如果8邻域下用曼哈顿距离斜线实际只需要走1.414曼哈顿距离却估计成2明显高估了那么搜索出的路径反而可能不是全局最短的。2.4 h(n)的“大小”如何影响路径形状h(n)越接近真实代价搜索范围越小、路径越“聪明”h(n)如果远小于真实代价搜索范围变大路径虽然最优但效率变低h(n)如果高估代价搜索速度快但路径可能贴着障碍走甚至会牺牲最优性。我习惯的做法是先用欧氏距离作为基准观察搜索结果如果地图上障碍很少、路径很长可以把启发项乘上一个略大于1的权重例如fg1.1h这样搜索明显变快代价损失通常很小。这就是后面会提到的加权A思想实际工程里非常常用。3. 主循环五步走OpenList、邻居扩展与路径回溯3.1 OpenList和CloseList到底在维护什么OpenList存放“已经发现但还没最终确认”的候选节点它按f值从小到大排列。CloseList存放“已经处理完”的节点处理完之后就不再动它。可以把OpenList理解成一个候选名单每次从里面挑选一个看起来总代价最小的人去考察CloseList就是黑名单考察过就不再翻了。这两个名单是整个A*运行状态的核心。3.2 主循环的五个步骤A*主循环看起来复杂拆开其实就是这五步初始化把起点加入OpenList起点的g0h启发距离fgh。从OpenList中取出f值最小的节点n。判断n是否终点如果是跳转到路径回溯阶段。把n移入CloseList然后扩展n的所有邻居跳过障碍格、边界外以及已在CloseList中的邻居对每个邻居m计算临时代价tempG g(n)cost(n,m)如果tempG比m已有的g值更小更新m的g、f并把m的父节点设为n再把m加入OpenList如果之前不在。回到第2步。如果OpenList为空说明起点和终点不可达。对应的伪代码如下open.add(start) while open is not empty: n open.popSmallestF() if n goal: return reconstructPath(parent, goal) close.add(n) for m in neighbors(n): if m in close: continue tentative_g n.g cost(n, m) if tentative_g m.g or m not in open: m.parent n m.g tentative_g m.f m.g heuristic(m, goal) open.add(m) return failure3.3 路径回溯从终点反向找回起点当终点被OpenList弹出时说明终点已经确认是最优可达状态。这时从终点沿着parent指针一路走回起点再把序列反转就能得到完整路径。回溯代码的常见写法是while循环path []; node goal; while ~isequal(node, start) path [node; path]; node squeeze(nodes.parent(node(1), node(2), :)); end path [start; path];注意这里要把终点本身也加进路径开头很多初学者会漏掉这一步导致路径少一个点。3.4 为什么说Dijkstra是A*的特例把A的启发函数h(n)设成0f(n)就只剩g(n)排序规则和Dijkstra完全一致。所以从算法体系上看Dijkstra就是A在“完全放弃启发信息”时的退化形态。理解这一点后你再回头看Dijkstra为什么慢、A为什么快就会非常清晰A只是多了一个“方向估计”其余流程几乎完全相同。4. MATLAB代码实现数据结构、主函数与四个关键函数4.1 文件规划为了让代码清晰可复用我推荐拆成以下文件main_Astar.m构建地图、设置起终点、调用A*、可视化Astar_planning.mA*核心主函数getNeighbors.m获取当前节点的可行邻居heuristic.m启发函数方便切换。4.2 为什么我不用cell数组而用矩阵存节点信息初学A*时很多人喜欢用结构体数组或cell数组存每个节点的g、f、parent。但MATLAB的cell和对象数组访问效率不高代码写起来也啰嗦。我推荐用“同尺寸矩阵”来存所有节点属性一个字段一个矩阵这样既紧凑又直观。nodes.g inf(rows, cols); nodes.h zeros(rows, cols); nodes.f inf(rows, cols); nodes.parent zeros(rows, cols, 2); nodes.visited false(rows, cols);g矩阵记录从起点到该节点的已知最小代价f矩阵记录gh的估计总代价parent矩阵的最后一个是2列存父节点的行列坐标visited矩阵表示该节点是否已被扩展过相当于CloseList。这套结构的缺点是每次找最小f节点都需要扫一遍f(:)地图尺寸在几十万个栅格内都完全够用。真正跑到上百万栅格时再考虑二叉堆我后面会讲。4.3 A*主函数的完整代码function path Astar_planning(map, start, goal) % A*路径规划主函数 % 输入 map: 0-可行, 1-障碍 % start: 起点坐标 [row, col] % goal: 终点坐标 [row, col] % 输出 path: 路径点序列 [n, 2] [rows, cols] size(map); nodes.g inf(rows, cols); nodes.h zeros(rows, cols); nodes.f inf(rows, cols); nodes.parent zeros(rows, cols, 2); nodes.visited false(rows, cols); nodes.g(start(1), start(2)) 0; nodes.h(start(1), start(2)) heuristic(start, goal); nodes.f(start(1), start(2)) nodes.h(start(1), start(2)); while true % 取出OpenList中f最小的节点 [minF, idx] min(nodes.f(:)); if isinf(minF) error(OpenList为空路径不存在); end [curR, curC] ind2sub([rows, cols], idx); % 如果该节点已经在CloseList跳过并标记为inf if nodes.visited(curR, curC) nodes.f(curR, curC) inf; continue; end % 到达终点 if curR goal(1) curC goal(2) break; end % 当前节点移入CloseList nodes.visited(curR, curC) true; nodes.f(curR, curC) inf; % 扩展邻居 nbrs getNeighbors(map, curR, curC); for i 1:size(nbrs, 1) nr nbrs(i, 1); nc nbrs(i, 2); if nodes.visited(nr, nc) continue; end % 临时g值水平垂直为1对焦线为sqrt(2) moveCost norm([nr - curR, nc - curC]); tempG nodes.g(curR, curC) moveCost; if tempG nodes.g(nr, nc) nodes.g(nr, nc) tempG; nodes.h(nr, nc) heuristic([nr, nc], goal); nodes.f(nr, nc) tempG nodes.h(nr, nc); nodes.parent(nr, nc, :) [curR, curC]; end end end % 回溯路径 path []; node goal; while ~isequal(node, start) path [node; path]; node squeeze(nodes.parent(node(1), node(2), :)); end path [start; path]; end4.4 getNeighbors邻居获取和穿墙检查function nbrs getNeighbors(map, r, c) % 获取当前节点的8邻域可行邻居 [rows, cols] size(map); d [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; nbrs zeros(0, 2); for k 1:size(d, 1) nr r d(k, 1); nc c d(k, 2); % 越界或障碍跳过 if nr 1 || nr rows || nc 1 || nc cols continue; end if map(nr, nc) 1 continue; end % 对角穿墙检查 if abs(d(k,1)) abs(d(k,2)) 2 if map(r d(k,1), c) 1 || map(r, c d(k,2)) 1 continue; end end nbrs(end 1, :) [nr, nc]; %#okAGROW end end穿墙检查很好理解从当前格走向右上方对角格时如果正上方或正右方是障碍机器人会被墙卡住所以这个对角格不能直接进入。如果你用4邻域只需把d改成上下左右四行并把穿墙判断删掉。4.5 heuristic启发函数单独封装function h heuristic(node, goal) % 欧氏距离适合8邻域 h norm(node - goal); % 曼哈顿距离适合4邻域 % h abs(node(1) - goal(1)) abs(node(2) - goal(2)); end把启发函数单独放一个文件的理由是方便实验。你在研究不同启发函数对搜索范围的影响时只需要注释切换一行不需要改主函数逻辑。5. 运行验证与可视化如何判断路径是不是最优5.1 主脚本设计与测试地图我用一个20x30的地图来做演示加入三个不同方向的障碍墙让路径必须绕弯。clc; clear; close all; map zeros(20, 30); map(5:15, 10:12) 1; % 左侧竖直墙 map(3:5, 18:25) 1; % 上方横向墙 map(12:18, 20:22) 1; % 右下方障碍 start [2, 2]; goal [18, 28]; path Astar_planning(map, start, goal); figure; imagesc(map); colormap(gray); hold on; plot(start(2), start(1), go, MarkerSize, 10, MarkerFaceColor, g); plot(goal(2), goal(1), ro, MarkerSize, 10, MarkerFaceColor, r); if ~isempty(path) plot(path(:, 2), path(:, 1), b-, LineWidth, 2); end axis equal; grid on; title(A* 路径规划结果);这里最关键的坑就是画图坐标。imagesc(map)显示矩阵时第一行显示在顶部列坐标是x轴方向的像素位置。所以画路径时一定要用path(:,2)作为x坐标、path(:,1)作为y坐标。如果写成plot(path(:,1), path(:,2))路径就会和地图对不上。5.2 验证路径最优性的一组小命令路径跑出来之后不要只看图要用数据确认路径合法且最优。segLens sqrt(sum(diff(path).^2, 2)); totalCost sum(segLens);diff(path)计算每段路径的差sqrt(sum(...))求每段欧氏长度求和得到总代价。你可以把启发函数从欧氏距离换成曼哈顿距离再跑一次。如果两次总代价一致说明路径仍是最优如果变长了说明新的启发函数在8邻域下高估了实际代价。5.3 观察搜索范围把visited节点也画出来A*最有意思的可视化是看它扩展了多少节点。修改Astar_planning.m让函数多返回一个nodes结构然后在主脚本里画出来[visitedR, visitedC] find(nodes.visited); plot(visitedC, visitedR, c., MarkerSize, 4);你会看到A的探索范围像一个指向终点的扇形而不是Dijkstra那样的大圆盘。这也是为什么A在大地图上比Dijkstra快得多的直观原因。我建议你在研究算法时把这套可视化做成固定工具它会让你对“启发函数效率”的理解远超只看公式。6. 工程里最容易翻车的三个细节坐标写反、穿墙、性能6.1 坐标写反最隐蔽的“镜像路径”这个问题在MATLAB里极其普遍。矩阵用(row, col)索引画图却用(x, y)两者天然错位。一种解决方式是画图前统一确定规则矩阵第一维永远对应y第二维永远对应x。然后所有plot都写成plot(path(:,2), path(:,1))。不要开axis xy虽然它能把y轴翻过来让图和数学坐标一致但翻来翻去更容易把人绕晕。我自己的习惯是固定不翻转y轴因为imagesc默认的坐标关系在叠加栅格数据时最不容易出错。6.2 对角线穿墙仿真里没问题真机就出问题纯数学栅格中斜穿墙角是合法的真实机器人有体积墙角会卡住它。如果你做的是仿真演示可能根本发现不了这个坑。但一旦把路径下发到真实底盘机器人就会在墙脚原地打转。最简单的处理就是我上面写的穿墙检查。更严格的做法是给机器人做膨胀层把障碍物周围额外标一圈不可通行区域然后再用A*规划这样不管走对角线还是正交方向都能保证路径离障碍物足够远。6.3 大地图性能从线性扫描到二叉堆我在第4章用了min(nodes.f(:))本质是在全图上线性扫描找最小f。当地图是几百乘以几百时这个操作问题不大但到1000x1000甚至更大时每次循环扫一遍f(:)会非常慢因为主循环可能要扩展几万个节点最终复杂度接近O(N^2)。工程上的标准解法是用最小堆二叉堆维护OpenList。MATLAB没有现成的可更新优先队列类需要自己写堆或者直接用更通用的语言实现。我的经验是如果只是验证算法保留线性扫描完全没问题代码简单不容易出bug如果要做大规模栅格规划建议用Python的heapq或在C里用std::priority_queue。这也是为什么很多实际项目只在MATLAB里验证A*逻辑最终部署版本却换成了C的原因。6.4 从静态A*到动态场景的扩展思路A适合静态地图地图一变化就要重规划。真实机器人工作场景中障碍物往往是动态的于是有了DLite、D等增量式重规划算法如果只是想让A更快可以加大启发权重得到加权A*如果地图是均匀栅格且障碍结构明显JPS跳点搜索也能把扩展节点数量压缩到一个非常小的集合。这些算法听起来复杂但核心逻辑都是从A的主循环演化出来的。把A理解透再看它们的改进点就轻松很多。最后说一点我自己实际操作的体会。第一次写A时我最喜欢的事情就是把启发函数、邻域类型、穿墙检查全都做成可配置的然后对着同一张地图来回切换观察搜索范围的变化。这个过程比背十遍伪代码都管用。你拿到这篇文章的代码后建议先跑通默认地图再随便改两个障碍物然后把启发函数从欧氏距离换成曼哈顿距离看看路径和扩展节点差多少。只有亲手被“锯齿路径”或者“斜穿墙角”坑过一次才算真正把A理解透了。
移动机器人A*路径规划：栅格地图建模与MATLAB实现
做移动机器人开发这几年我数不清处理过多少条路径规划的需求。从差速底盘在仓库里的避障到AGV在车间里的调度最早让我觉得“终于有谱了”的算法就是A*。不管是想读机器人方向的研究生还是准备算法岗面试又或者只是想把课程作业做得像样一点A算法都是绕不开的基石。这篇文章我会把A的来龙去脉、栅格地图建模、fgh的每一个细节以及一份能在MATLAB里直接跑的完整代码从头到尾讲一遍。看完之后你可以立刻复现这段代码改改地图和起终点观察路径变化。1. 从Dijkstra到A*启发式搜索为什么更快1.1 Dijkstra的“地毯式搜索”到底慢在哪Dijkstra算法本身没有错它保证能找到最短路径问题是它完全不看终点的方向。每次迭代它只选当前累计代价g最小的节点向外扩展从起点发起的搜索波前会均匀地向四面八方扩散。假设起点在栅格地图左上角终点在右下角中间没有障碍物。Dijkstra依然会花大量时间去扩展左下方、右上方这些明显“跑偏”的节点等波前终于碰到终点时大半张地图已经被扫过一遍。空旷场景下这种无差别搜索浪费严重地图越大越明显。我经常用一个生活类比Dijkstra像是你在陌生城市找最近的奶茶店手里只有一张标了街道长度的地图没有方向提示于是你把以起点为圆心的所有街道都扫一遍走一步算一步直到奶茶店出现在视野里。一定能找到但绕的路太多。1.2 启发式h(n)给搜索装上了“方向感”A和Dijkstra唯一的区别就是在排序标准里加了一个启发项h(n)表示从当前节点n到终点还需要的大致代价。A按f(n)g(n)h(n)排序每次优先扩展f最小的节点。还是找奶茶店的例子。现在你手里多了一个指南针能直接看到奶茶店在东南方向你自然优先往东南方向找而不是把西北方向的街道也全部翻一遍。h(n)就是那个指南针它不需要知道路上有没有障碍只需要给出一个基于几何位置的估计值。在栅格地图上这个“估计值”通常用当前点和终点的直线距离、曼哈顿距离或切比雪夫距离来计算。关键是h(n)不能“高估”真实代价我们把这种性质叫可采纳性。只要h可采纳A*第一次从OpenList中取出终点时回溯得到的路径就一定是最优的。1.3 三种搜索算法只看代价、只看启发、两者都看算法排序依据是否保证最优搜索范围贪心最佳优先BFS类只按h排序不保证容易走进死胡同很小速度快Dijkstra只按g排序保证最优大向四周均匀扩散A*按gh排序h可采纳时保证最优比Dijkstra小得多有个很直观的结论当h(n)恒等于0时A就完全退化成Dijkstra当h(n)远大于实际代价时A又变得像贪心搜索速度上来了但可能丢掉最优解。所以启发函数的设计本质是在“效率”和“最优性”之间做权衡。面试里经常被问“A*一定能找到最短路径吗”标准答案必须加前提——启发函数可采纳。这个细节很容易忽略但工程上非常重要。2. 栅格地图上的fgh建模细节直接决定路径质量2.1 地图的矩阵表示和坐标约定栅格地图在MATLAB里最直观的表示就是二维矩阵0代表可通行1代表障碍。例如map [0 0 0 1 0; 0 1 0 1 0; 0 1 0 0 0];这里map(r,c)中行r对应平面上的y坐标列c对应x坐标和数学里“先x后y”的习惯正好相反。这个问题放在最后画图时会给你带来很大困扰我见过不少人在plot时把行列写反导致路径像照了镜子一样左右翻转却怎么也找不出原因。2.2 4邻域、8邻域与移动代价路径方向由邻域定义决定4邻域只能上下左右移动每步移动代价为1。8邻域允许对角移动水平和垂直方向代价为1对角线方向代价为sqrt(2)。我在工程里默认推荐8邻域因为路径更平滑转弯更自然。但8邻域有一个隐患就是机器人可以斜着穿过墙角缝。比如当前格左方是障碍、上方是障碍如果不对角移动做检查A*会规划出一条贴着墙角穿过去的路径真实机器人根本没法走。处理方式是在获取邻居时增加穿墙检查只有当前格与对角格之间的两个正交邻居都可行时才允许进入对角格。这个细节看起来小实际项目中非常重要。2.3 三种启发函数怎么选启发函数公式适用场景特点曼哈顿距离abs(dr)abs(dc)4邻域代价紧凑搜索较广欧几里得距离norm([dr,dc])8邻域路径平滑推荐切比雪夫距离max(abs(dr),abs(dc))8邻域计算最快易走斜线选择启发函数的核心逻辑很简单h(n)绝不能高估真实剩余代价否则会丢掉最优解。在8邻域下每次移动代价最多是sqrt(2)欧氏距离给出的估计通常不高估真实代价所以可以放心用。如果8邻域下用曼哈顿距离斜线实际只需要走1.414曼哈顿距离却估计成2明显高估了那么搜索出的路径反而可能不是全局最短的。2.4 h(n)的“大小”如何影响路径形状h(n)越接近真实代价搜索范围越小、路径越“聪明”h(n)如果远小于真实代价搜索范围变大路径虽然最优但效率变低h(n)如果高估代价搜索速度快但路径可能贴着障碍走甚至会牺牲最优性。我习惯的做法是先用欧氏距离作为基准观察搜索结果如果地图上障碍很少、路径很长可以把启发项乘上一个略大于1的权重例如fg1.1h这样搜索明显变快代价损失通常很小。这就是后面会提到的加权A思想实际工程里非常常用。3. 主循环五步走OpenList、邻居扩展与路径回溯3.1 OpenList和CloseList到底在维护什么OpenList存放“已经发现但还没最终确认”的候选节点它按f值从小到大排列。CloseList存放“已经处理完”的节点处理完之后就不再动它。可以把OpenList理解成一个候选名单每次从里面挑选一个看起来总代价最小的人去考察CloseList就是黑名单考察过就不再翻了。这两个名单是整个A*运行状态的核心。3.2 主循环的五个步骤A*主循环看起来复杂拆开其实就是这五步初始化把起点加入OpenList起点的g0h启发距离fgh。从OpenList中取出f值最小的节点n。判断n是否终点如果是跳转到路径回溯阶段。把n移入CloseList然后扩展n的所有邻居跳过障碍格、边界外以及已在CloseList中的邻居对每个邻居m计算临时代价tempG g(n)cost(n,m)如果tempG比m已有的g值更小更新m的g、f并把m的父节点设为n再把m加入OpenList如果之前不在。回到第2步。如果OpenList为空说明起点和终点不可达。对应的伪代码如下open.add(start) while open is not empty: n open.popSmallestF() if n goal: return reconstructPath(parent, goal) close.add(n) for m in neighbors(n): if m in close: continue tentative_g n.g cost(n, m) if tentative_g m.g or m not in open: m.parent n m.g tentative_g m.f m.g heuristic(m, goal) open.add(m) return failure3.3 路径回溯从终点反向找回起点当终点被OpenList弹出时说明终点已经确认是最优可达状态。这时从终点沿着parent指针一路走回起点再把序列反转就能得到完整路径。回溯代码的常见写法是while循环path []; node goal; while ~isequal(node, start) path [node; path]; node squeeze(nodes.parent(node(1), node(2), :)); end path [start; path];注意这里要把终点本身也加进路径开头很多初学者会漏掉这一步导致路径少一个点。3.4 为什么说Dijkstra是A*的特例把A的启发函数h(n)设成0f(n)就只剩g(n)排序规则和Dijkstra完全一致。所以从算法体系上看Dijkstra就是A在“完全放弃启发信息”时的退化形态。理解这一点后你再回头看Dijkstra为什么慢、A为什么快就会非常清晰A只是多了一个“方向估计”其余流程几乎完全相同。4. MATLAB代码实现数据结构、主函数与四个关键函数4.1 文件规划为了让代码清晰可复用我推荐拆成以下文件main_Astar.m构建地图、设置起终点、调用A*、可视化Astar_planning.mA*核心主函数getNeighbors.m获取当前节点的可行邻居heuristic.m启发函数方便切换。4.2 为什么我不用cell数组而用矩阵存节点信息初学A*时很多人喜欢用结构体数组或cell数组存每个节点的g、f、parent。但MATLAB的cell和对象数组访问效率不高代码写起来也啰嗦。我推荐用“同尺寸矩阵”来存所有节点属性一个字段一个矩阵这样既紧凑又直观。nodes.g inf(rows, cols); nodes.h zeros(rows, cols); nodes.f inf(rows, cols); nodes.parent zeros(rows, cols, 2); nodes.visited false(rows, cols);g矩阵记录从起点到该节点的已知最小代价f矩阵记录gh的估计总代价parent矩阵的最后一个是2列存父节点的行列坐标visited矩阵表示该节点是否已被扩展过相当于CloseList。这套结构的缺点是每次找最小f节点都需要扫一遍f(:)地图尺寸在几十万个栅格内都完全够用。真正跑到上百万栅格时再考虑二叉堆我后面会讲。4.3 A*主函数的完整代码function path Astar_planning(map, start, goal) % A*路径规划主函数 % 输入 map: 0-可行, 1-障碍 % start: 起点坐标 [row, col] % goal: 终点坐标 [row, col] % 输出 path: 路径点序列 [n, 2] [rows, cols] size(map); nodes.g inf(rows, cols); nodes.h zeros(rows, cols); nodes.f inf(rows, cols); nodes.parent zeros(rows, cols, 2); nodes.visited false(rows, cols); nodes.g(start(1), start(2)) 0; nodes.h(start(1), start(2)) heuristic(start, goal); nodes.f(start(1), start(2)) nodes.h(start(1), start(2)); while true % 取出OpenList中f最小的节点 [minF, idx] min(nodes.f(:)); if isinf(minF) error(OpenList为空路径不存在); end [curR, curC] ind2sub([rows, cols], idx); % 如果该节点已经在CloseList跳过并标记为inf if nodes.visited(curR, curC) nodes.f(curR, curC) inf; continue; end % 到达终点 if curR goal(1) curC goal(2) break; end % 当前节点移入CloseList nodes.visited(curR, curC) true; nodes.f(curR, curC) inf; % 扩展邻居 nbrs getNeighbors(map, curR, curC); for i 1:size(nbrs, 1) nr nbrs(i, 1); nc nbrs(i, 2); if nodes.visited(nr, nc) continue; end % 临时g值水平垂直为1对焦线为sqrt(2) moveCost norm([nr - curR, nc - curC]); tempG nodes.g(curR, curC) moveCost; if tempG nodes.g(nr, nc) nodes.g(nr, nc) tempG; nodes.h(nr, nc) heuristic([nr, nc], goal); nodes.f(nr, nc) tempG nodes.h(nr, nc); nodes.parent(nr, nc, :) [curR, curC]; end end end % 回溯路径 path []; node goal; while ~isequal(node, start) path [node; path]; node squeeze(nodes.parent(node(1), node(2), :)); end path [start; path]; end4.4 getNeighbors邻居获取和穿墙检查function nbrs getNeighbors(map, r, c) % 获取当前节点的8邻域可行邻居 [rows, cols] size(map); d [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; nbrs zeros(0, 2); for k 1:size(d, 1) nr r d(k, 1); nc c d(k, 2); % 越界或障碍跳过 if nr 1 || nr rows || nc 1 || nc cols continue; end if map(nr, nc) 1 continue; end % 对角穿墙检查 if abs(d(k,1)) abs(d(k,2)) 2 if map(r d(k,1), c) 1 || map(r, c d(k,2)) 1 continue; end end nbrs(end 1, :) [nr, nc]; %#okAGROW end end穿墙检查很好理解从当前格走向右上方对角格时如果正上方或正右方是障碍机器人会被墙卡住所以这个对角格不能直接进入。如果你用4邻域只需把d改成上下左右四行并把穿墙判断删掉。4.5 heuristic启发函数单独封装function h heuristic(node, goal) % 欧氏距离适合8邻域 h norm(node - goal); % 曼哈顿距离适合4邻域 % h abs(node(1) - goal(1)) abs(node(2) - goal(2)); end把启发函数单独放一个文件的理由是方便实验。你在研究不同启发函数对搜索范围的影响时只需要注释切换一行不需要改主函数逻辑。5. 运行验证与可视化如何判断路径是不是最优5.1 主脚本设计与测试地图我用一个20x30的地图来做演示加入三个不同方向的障碍墙让路径必须绕弯。clc; clear; close all; map zeros(20, 30); map(5:15, 10:12) 1; % 左侧竖直墙 map(3:5, 18:25) 1; % 上方横向墙 map(12:18, 20:22) 1; % 右下方障碍 start [2, 2]; goal [18, 28]; path Astar_planning(map, start, goal); figure; imagesc(map); colormap(gray); hold on; plot(start(2), start(1), go, MarkerSize, 10, MarkerFaceColor, g); plot(goal(2), goal(1), ro, MarkerSize, 10, MarkerFaceColor, r); if ~isempty(path) plot(path(:, 2), path(:, 1), b-, LineWidth, 2); end axis equal; grid on; title(A* 路径规划结果);这里最关键的坑就是画图坐标。imagesc(map)显示矩阵时第一行显示在顶部列坐标是x轴方向的像素位置。所以画路径时一定要用path(:,2)作为x坐标、path(:,1)作为y坐标。如果写成plot(path(:,1), path(:,2))路径就会和地图对不上。5.2 验证路径最优性的一组小命令路径跑出来之后不要只看图要用数据确认路径合法且最优。segLens sqrt(sum(diff(path).^2, 2)); totalCost sum(segLens);diff(path)计算每段路径的差sqrt(sum(...))求每段欧氏长度求和得到总代价。你可以把启发函数从欧氏距离换成曼哈顿距离再跑一次。如果两次总代价一致说明路径仍是最优如果变长了说明新的启发函数在8邻域下高估了实际代价。5.3 观察搜索范围把visited节点也画出来A*最有意思的可视化是看它扩展了多少节点。修改Astar_planning.m让函数多返回一个nodes结构然后在主脚本里画出来[visitedR, visitedC] find(nodes.visited); plot(visitedC, visitedR, c., MarkerSize, 4);你会看到A的探索范围像一个指向终点的扇形而不是Dijkstra那样的大圆盘。这也是为什么A在大地图上比Dijkstra快得多的直观原因。我建议你在研究算法时把这套可视化做成固定工具它会让你对“启发函数效率”的理解远超只看公式。6. 工程里最容易翻车的三个细节坐标写反、穿墙、性能6.1 坐标写反最隐蔽的“镜像路径”这个问题在MATLAB里极其普遍。矩阵用(row, col)索引画图却用(x, y)两者天然错位。一种解决方式是画图前统一确定规则矩阵第一维永远对应y第二维永远对应x。然后所有plot都写成plot(path(:,2), path(:,1))。不要开axis xy虽然它能把y轴翻过来让图和数学坐标一致但翻来翻去更容易把人绕晕。我自己的习惯是固定不翻转y轴因为imagesc默认的坐标关系在叠加栅格数据时最不容易出错。6.2 对角线穿墙仿真里没问题真机就出问题纯数学栅格中斜穿墙角是合法的真实机器人有体积墙角会卡住它。如果你做的是仿真演示可能根本发现不了这个坑。但一旦把路径下发到真实底盘机器人就会在墙脚原地打转。最简单的处理就是我上面写的穿墙检查。更严格的做法是给机器人做膨胀层把障碍物周围额外标一圈不可通行区域然后再用A*规划这样不管走对角线还是正交方向都能保证路径离障碍物足够远。6.3 大地图性能从线性扫描到二叉堆我在第4章用了min(nodes.f(:))本质是在全图上线性扫描找最小f。当地图是几百乘以几百时这个操作问题不大但到1000x1000甚至更大时每次循环扫一遍f(:)会非常慢因为主循环可能要扩展几万个节点最终复杂度接近O(N^2)。工程上的标准解法是用最小堆二叉堆维护OpenList。MATLAB没有现成的可更新优先队列类需要自己写堆或者直接用更通用的语言实现。我的经验是如果只是验证算法保留线性扫描完全没问题代码简单不容易出bug如果要做大规模栅格规划建议用Python的heapq或在C里用std::priority_queue。这也是为什么很多实际项目只在MATLAB里验证A*逻辑最终部署版本却换成了C的原因。6.4 从静态A*到动态场景的扩展思路A适合静态地图地图一变化就要重规划。真实机器人工作场景中障碍物往往是动态的于是有了DLite、D等增量式重规划算法如果只是想让A更快可以加大启发权重得到加权A*如果地图是均匀栅格且障碍结构明显JPS跳点搜索也能把扩展节点数量压缩到一个非常小的集合。这些算法听起来复杂但核心逻辑都是从A的主循环演化出来的。把A理解透再看它们的改进点就轻松很多。最后说一点我自己实际操作的体会。第一次写A时我最喜欢的事情就是把启发函数、邻域类型、穿墙检查全都做成可配置的然后对着同一张地图来回切换观察搜索范围的变化。这个过程比背十遍伪代码都管用。你拿到这篇文章的代码后建议先跑通默认地图再随便改两个障碍物然后把启发函数从欧氏距离换成曼哈顿距离看看路径和扩展节点差多少。只有亲手被“锯齿路径”或者“斜穿墙角”坑过一次才算真正把A理解透了。