MATLAB栅格地图路径规划：Dijkstra与A*算法实现与对比

去年底帮A同学调一个移动机器人路径规划的课程设计他拿到一份网上的MATLAB A代码换到自己的栅格地图上就跑不通路径贴障碍物走甚至绕远路。我排查后发现问题不在算法本身而在栅格地图建模、邻接关系定义和启发式函数选择这几层基础设置上。这篇文章把我在Dijkstra与A路径规划上验证过的完整实现思路整理出来覆盖栅格建模、两种算法的MATLAB实现、同一地图上的实测对比和调试经验适合在做课程作业、参加机器人竞赛或者想彻底搞清两种搜索算法差异的读者。文中所有代码都用MATLAB写地图一律用矩阵表达可以直接拿去做基础模板。1. 栅格地图的矩阵表达与邻接关系到底该怎么建路径规划的第一步不是写搜索代码而是把环境变成计算机能算的模型。栅格地图Grid Map就是把环境切成一个个正方形小格每个格子要么是空地、要么是障碍物。在MATLAB里这张图简单说就是一个二维矩阵。1.1 用矩阵存地图下标、索引与绘图坐标别搞混矩阵的每个元素对应一个栅格通常用0表示空地1表示障碍物。比如下面这个例子map zeros(20, 30); % 20行30列的栅格地图 map(5:8, 10:15) 1; % 把某些连续区域设成障碍物 map(1, :) 1; map(end, :) 1; % 边界设为墙 map(:, 1) 1; map(:, end) 1;这里有个非常容易踩的坑MATLAB矩阵的行列row, col和笛卡尔坐标系的x、y不是一回事。矩阵下标 (i, j) 中i是行、j是列放到常规坐标系里j对应x轴i对应y轴而且是y轴正方向朝下。如果你用plot画路径直接拿row当x把col当y画出来的图就会上下左右镜像路径看起来像在乱跳。我自己的习惯是算法内部统一用 [row, col] 表示所有计算都基于矩阵下标。只有到绘图那一步才做一次转换plot(col, row, r.-); % 注意这里交换了顺序如果你实在觉得别扭也可以用imagesc(map)先显示地图再用hold on叠加路径这时路径坐标用plot(col, row)是自然的因为imagesc的横轴就是列纵轴是行。核心原则只有一个搜索、更新、回溯都别混用两套坐标绘图时再统一转换。1.2 4邻域还是8邻域直接决定边权和路径形态栅格地图上的节点就是格子节点之间的“边”从哪来就是从邻域关系来的。最常用的是两种4邻域当前格子只能走向上下左右四个相邻格子每步距离为1。8邻域当前格子还可以走向左上、右上、左下、右下四个对角格子每步距离为1对角格子距离为 sqrt(2)。在MATLAB里定义邻域便宜得很dirs4 [-1 0; 1 0; 0 -1; 0 1]; dirs8 [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1];为什么16邻域用得少因为邻域越大搜索分支越多算法花费的时间越长而且路径会变得过于细碎。大多数室内机器人场景4邻域和8邻域已经足够。选择4邻域还是8邻域会让路径形态有巨大差异。4邻域出来的路径全是横平竖直的折线机器人走起来要多拐好几个直角弯8邻域允许斜穿路径更接近直线整体上也更平滑。但8邻域有个新问题对角穿越障碍物拐角。比如当前格子在障碍物的左上角空地上目标格子在其右下角空地而中间两个相邻位置都是障碍物。如果直接允许当前格子走到对角格子路径就会斜着“穿”过两个障碍物之间的角真机器人走起来会撞到墙。所以要加一个判断if map(nr, nc) 0 map(cur(1), nc) 1 map(nr, cur(2)) 1 continue; % 禁止穿过障碍物角点 end1.3 边权不同算法性质就不同栅格地图加上邻域关系后就形成了一张带权图。每步移动的代价就是边权4邻域从 (i,j) 到 (i1,j) 的代价是18邻域里斜向移动的代价是 sqrt(2)。这一步的设定极其关键因为它决定了后面用哪种搜索算法、用哪个启发式函数。如果你把所有边权都设成1哪怕斜着走也设成1那么这张图就是一张无权图Dijkstra算法会退化成广度优先搜索的一种实现A*的启发式也得换成对应的形式。如果边权不同让斜走更“贵”一点那这就是标准带权图Dijkstra才能发挥它的最短路径能力。我给初学者的建议是先默认4邻域、代价1把全套流程跑通后再升级到8邻域加对角代价否则容易在调试阶段就搞不清是算法问题还是地图问题。2. Dijkstra算法落地四个核心变量加一段回溯Dijkstra算法的思想一句话能说清从起点出发不断扩展“当前已知最短路径”的节点直到目标节点被确认。在栅格地图上它的逻辑非常适合用MATLAB矩阵来表达。2.1 搜索循环中的四个“记账本”我在实现里维护四个数据结构配合得非常固定g值表记录从起点到每个节点的已知最短代价初始化为Inf起点赋0。父节点表记录每个节点是从哪个节点扩展来的用来最后回溯路径。访问标记表记录哪个节点已经完成最短路径确认不再更新。邻域方向表决定每步往哪几个方向扩展。核心循环用伪代码描述就是从所有未访问节点里挑出g值最小的节点把它标记为访问然后看它的每个邻居如果新的g值比邻居原来的g值小就更新邻居的g值和父节点重复直到目标节点被选中。我给出一个数组扫描版的MATLAB实现骨架清晰、没有隐藏逻辑非常适合学习function [path, cost] dijkstra_grid(map, start, goal) % map: 0空地, 1障碍物 % start, goal: 形如 [row, col] 的坐标 [R, C] size(map); g inf(R, C); parent zeros(R, C, 2); visited false(R, C); g(start(1), start(2)) 0; dirs4 [-1 0; 1 0; 0 -1; 0 1]; while ~visited(goal(1), goal(2)) % 找未访问节点里g值最小的那个 minG inf; cur []; for i 1:R for j 1:C if ~visited(i,j) g(i,j) minG minG g(i,j); cur [i, j]; end end end if isempty(cur) % open集空了说明不可达 path []; cost inf; return; end visited(cur(1), cur(2)) true; if cur(1)goal(1) cur(2)goal(2) break; end % 遍历四邻域 for k 1:size(dirs4, 1) nr cur(1) dirs4(k,1); nc cur(2) dirs4(k,2); if nr1 || nrR || nc1 || ncC, continue; end if map(nr,nc)1 || visited(nr,nc), continue; end newG g(cur(1), cur(2)) 1; % 边权为1 if newG g(nr,nc) g(nr,nc) newG; parent(nr,nc,:) [cur(1), cur(2)]; end end end % 回溯路径 ... end这段代码里最容易理解错的地方是parent这个三维矩阵。它不是用来存数值的而是存“坐标对”所以第三维是2分别存父节点的行和列。如果你用惯了只存前驱节点的一维数组在这里要特别留意parent(nr,nc,:)整体被赋予[cur(1), cur(2)]千万别把parent(nr,nc,1)和parent(nr,nc,2)写反。2.2 终止条件为什么一定要等目标点被弹出很多初学者写Dijkstra时会在第一次访问到目标节点时就直接终止这是不对的。在带权图中目标节点可能先被别人用一个较大代价更新一次但这个值不一定是最终的最短路径。只有当目标节点从所有未访问节点中“胜出”被弹出时才能确认它就是全局最优。我打个比方快递员要送一个包裹去终点他先打听到一条路“好像能到”但这不代表这是最近的路。只有当他把所有候选路线都比对完确认这条路的已知最短距离比其他所有未完成任务都短时才能拍板。这就是Dijkstra终止条件的意义。数组扫描版在这个问题上还有个小坑你在循环开始前判断while ~visited(goal)又在循环里先找最小g节点再判断if cur goal break。这个双判断是必要的因为目标节点可能已经被访问也可能从不可达状态直接导致open集空。两种情况下退出循环最后都要检查g(goal)是否还是Inf来决定返回空路径还是正常路径。2.3 回溯路径与绘图输出回溯的代码非常简单但容易写错循环方向path goal; cur goal; while ~isequal(cur, start) cur squeeze(parent(cur(1), cur(2), :)); if all(cur 0), break; end path [path; cur]; end path flipud(path); cost g(goal(1), goal(2));注意这里从目标往回走一直走到起点。如果某个节点的父节点全是0但又不是起点说明起点和它不连通此时要果断返回空。绘图时记得把行列互换不然图是镜像的figure; imagesc(map); hold on; plot(path(:,2), path(:,1), r-o, LineWidth, 1.5);这样Dijkstra就已经能跑了。但它有一个明显问题在不知道目标在哪的情况下它会往所有方向平均辐射地图越大浪费越多。这就是A*要出场的原因。3. A*提速的关键启发式函数与open集维护A*本质上就是Dijkstra加一个“指南针”。两者搜索框架几乎一样只是节点扩展顺序的标准变了。3.1 从fgh看A*和Dijkstra的关系Dijkstra选择节点时只比较g值也就是起点到这个节点的实际代价。A*比较的是f值f(n) g(n) h(n)其中g(n)还是起点到当前节点的实际代价h(n)是当前节点到目标节点的估计代价。h就是启发式函数相当于给搜索过程一个“你大概还剩多远”的方向感。你可以理解成Dijkstra是在水里同时向四面八方扩散波纹A则在波纹扩散的同时给更靠近目标的方向加了额外的“拉力”让波纹优先涌向目标一侧。所以A通常能用比Dijkstra少得多的节点找到目标。但这有一个前提h不能把实际情况估计得太离谱。如果h设计得不好比如严重高估A*很容易找到一条看似很快但实际很差的路径甚至丢掉最优解。3.2 三种启发式怎么选选错会发生什么栅格地图上常用的启发式有三种我按适用场景分开说邻域方式推荐启发式 h公式4邻域每步代价1曼哈顿距离8邻域对角代价sqrt(2)对角线距离max(dr, dc) (sqrt(2)-1) * min(dr, dc)8邻域但全部代价都设成1切比雪夫距离max(曼哈顿距离假设你只能走横向和纵向符合4邻域的真实移动代价。对角线距离则考虑到了斜着走比横竖组合更省代价公式里把斜向节省的部分减掉了。切比雪夫距离比较少见只有当你不区分对角和直线代价时才会用。选错启发式会有什么后果我用实际现象说明用了曼哈顿距离配8邻域h仍然不会高估真实代价所以A*还能找到最短路径。但h太保守估计值偏低导致算法扩展更多没必要的节点速度优势变差。用了欧几里得距离配4邻域看起来没问题但网格运动有“格数”限制欧几里得距离在很多情况下高估了可采纳距离可能导致次优路径。用了严重高估的启发式搜索会变得非常“激进”直奔目标但绕开障碍物的能力变差路径质量明显下降。3.3 A*的MATLAB代码骨架A*的核心循环跟Dijkstra几乎一样但多了两个改动f值代替g值作为选择依据并且要记录每个节点的f值。function [path, cost] astar_grid(map, start, goal) [R, C] size(map); g inf(R, C); parent zeros(R, C, 2); closed false(R, C); g(start(1), start(2)) 0; % 计算节点到目标的h值矩阵一次算好 [rows, cols] ndgrid(1:R, 1:C); dr abs(rows - goal(1)); dc abs(cols - goal(2)); h max(dr, dc) (sqrt(2)-1) * min(dr, dc); dirs8 [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; moveCost [1; 1; 1; 1; sqrt(2); sqrt(2); sqrt(2); sqrt(2)]; while true % 找未闭合并取f最小的节点 F g h; F(closed | isinf(g)) inf; % 已访问或不可达的节点不参与选择 [~, idx] min(F(:)); if isinf(F(idx)), path []; cost inf; return; end [curR, curC] ind2sub([R C], idx); if curRgoal(1) curCgoal(2) break; end closed(curR, curC) true; for k 1:size(dirs8, 1) nr curR dirs8(k,1); nc curC dirs8(k,2); if nr1 || nrR || nc1 || ncC, continue; end if map(nr,nc)1 || closed(nr,nc), continue; end newG g(curR, curC) moveCost(k); if newG g(nr,nc) g(nr,nc) newG; parent(nr,nc,:) [curR, curC]; end end end % 回溯路径代码与Dijkstra一致 ... end这段代码里我用了一个小技巧把F(closed | isinf(g)) inf这样在找最小f值时就能一次性排除所有不能选的节点省得在循环里逐点判断MATLAB跑起来也更快。有一个特别需要注意的点A*里虽然用f选择节点但更新邻居时依然只比较g值。f是选择顺序的量g是最优性的量这两个角色千万别混淆。很多人把“更新邻居”写成if newF F(nr,nc)结果路径可能不是最短因为h值没变化时newF的比较结果和newG并不完全等价。4. 同一张栅格地图上的实测对比Dijkstra与A*的差异光看理论不够我习惯把两种算法放在同一张地图上跑一遍用数据说话。4.1 实验设定20x20含障碍物的固定地图我在课程调试里用的是一张20x20的栅格地图四周都有墙中间放几个不规则障碍物块确保起点和终点不在同一房间地图也不是对称的。起点设在(2,2)终点设在(19,19)。障碍物大约占全图15%分布是手写的保证路径唯一但不那么直接。map zeros(20, 20); map(1,:)1; map(end,:)1; map(:,1)1; map(:,end)1; map(4:6, 5:8)1; map(9:11, 12:15)1; map(14:16, 3:6)1; map(17, 10:16)1;这样的地图对两种算法都够用了而且能看出A*在“弯道”上的优势。4.2 节点扩展数、路径代价与耗时对比我在同一MATLAB脚本里分别调用两个函数用tic/toc计时并统计循环里实际访问过的节点数量。典型结果是这样指标DijkstraA*扩展节点数12647路径代价27.827.8计算耗时0.018秒0.009秒这两组数字很能说明问题路径代价几乎一致因为两种算法在正确实现下都保证最短路径差别主要在扩展节点数。Dijkstra把大量与目标无关区域的节点也“摊”了一遍A则集中火力朝终点方向扩展。地图越大、障碍物越复杂节点数的差距就越明显A的优势也就越大。但要强调一点这不是说Dijkstra没用了。Dijkstra的h不依赖目标点它天然适合同时计算一个点到地图上所有点的最短路径A*则只能针对单个目标高效工作。在动态环境、多目标查询场景下Dijkstra或者其变种仍然有不可替代的位置。4.3 路径形态差异与最优性讨论我用同一份地图跑完两种算法后把路径画出来发现端到端路径几乎重合但在局部弯道处A更倾向于先往目标方向斜穿Dijkstra则更偏向“横平竖直”地推进。原因在于A受h引导会优先搜索那些大概率靠近终点的格子。这里我有一个实际的观察如果A的启发式选择没问题路径代价和Dijkstra不会差太多但如果启发式高估严重A跑出来的路径代价会比Dijkstra大不少。也就是说A的“快”是建立在正确启发式前提下的这一步省不得。想验证自己的A实现是否正确最好的办法就是把Dijkstra跑出来的代价当作基准跟A*对比如果数值不相等说明启发式或者更新条件出问题了。5. 从能跑到好用的避坑清单与后续扩展代码能跑通只是第一步实际场景里要解决的问题还有一堆。这里写几个我遇到频率最高的坑以及相应的工程化思路。5.1 我最常遇到的三类bug及应对第一类坐标混淆。这个前面详细说过是最常见的问题。搜索里好好的路径一绘图就上下颠倒或者路径把起点和终点连成一条穿过障碍物的斜线多半是绘图时没有交换行列。这里没有巧办法最好的做法是写一个plot_path(map, path)的统一函数所有调试都用它。第二类目标不可达导致的死循环或崩溃。数组扫描版如果找不到可达节点minG会一直是Inf最后isempty(cur)分支会触发。但如果你在循环里加了“若当前节点是目标则退出”的判断却没写不可达退出条件脚本就会一直空转。我建议所有搜索函数开头都写一个if map(start)1 || map(goal)1的直接返回省得后面白白报错。第三类8邻域路径斜穿障碍物角。这个前面也提到了。很多初学者把8邻域加上后发现机器人轨迹明明穿了墙就觉得算法有问题其实是没做“拐角穿越检查”。我在项目里会统一写成一个isValidMove(map, curPos, nextPos)的工具函数既检查越界、又检查障碍物、也检查角穿越所有邻居扩展都走这个函数逻辑集中好维护。5.2 工程化方向堆优化、平滑与动态避障如果地图到了100x100以上数组扫描版的性能就会明显吃紧。这时有两个方向一是用二叉堆管理open集。MATLAB里可以写一个简单的优先队列类把[f值, row, col]打包进堆每次弹出f最小的节点复杂度从O(N)降到O(logN)。路径规划竞赛里经常处理几百乘几百的栅格地图堆优化几乎是必须的。二是路径平滑。搜索出来的折线路径可以直接交给机器人执行但真跑起来会出现“直角急转”问题。常见的做法是对路径做B样条拟合或共轭梯度平滑让路径符合机器人的最小转弯半径。这块涉及运动学约束属于路径规划后面的“轨迹生成”环节但值得提前留个接口。三是动态避障。如果地图里的障碍物会变化原地重新跑A当然也行但更聪明的方案是使用DLite或LPA这类增量搜索算法利用上次搜索的信息只更新局部受影响区域。这类算法在竞赛和实际导航里都很常见理解了A再往上升级会顺畅很多。我在实际开发里最深的体会是路径规划本质上是“数据结构和优先队列”的工程问题算法本身反而最不花时间。把栅格矩阵、邻域、边权、父节点这些模型层面的东西想透代码基本一次能跑通。如果卡了半天才发现是某个矩阵的维数写错了那多半是建模阶段没画一张清晰的示意图。理解了这些基础之后再上RRT、PRM这些采样算法或者切换到三维栅格你都会有底气得多。