简介这是一份基于MATLAB编写的Dijkstra算法格栅地图路径规划源码包面向需要学习经典最短路径算法或开展移动机器人、二维栅格环境路径搜索实验的本科生与研究人员。实现包含邻接矩阵构建、优先队列优化、节点距离与访问状态维护、前驱记录及路径回溯等核心环节相关变量与函数命名清晰便于对照算法流程理解。源码演示了如何用0/非0矩阵表达格栅地图并在迭代选择中采用优先队列提升效率最终通过前驱节点回溯得到完整路径输出形式便于结合绘图函数观察路径走向。压缩包共4个文件其中3个.m脚本用于主程序、站点定义与算法调用另含1个.mat数据文件保存地图或障碍物矩阵整体仅2KB轻量易运行。已有1780人学习下载适合在MATLAB中快速验证Dijkstra算法并为后续扩展A*等启发式搜索提供可修改的基础框架。1. 为什么 Dijkstra 算法在格栅地图上仍然值得手写一遍做 AGV 或移动机器人路径规划时很多人拿到地图第一反应是直接调 A*但 Dijkstra 算法依然是格栅地图路径里最不该跳过的起步点。格栅地图本质是一个节点多、边少的有权图Dijkstra 不靠启发函数也能保证全局最优而且在多目标查询、动态障碍物重规划、代价地图带坡度这些场景里它的结果可预期排错也容易。对刚接触路径规划的工程师手写一遍 Dijkstra 相当于把 open 集合、前驱表、路径回溯这些基本功全部过一遍对已经写过 A* 的人来说这个版本反而是理解「为什么启发式能提速」的最好对照物。2. 格栅地图先变成图再谈搜索Dijkstra 算法的建模与代价计算2.1 格栅地图两种常见表示与邻居集合格栅地图也叫栅格地图在 MATLAB 里最常见的表示是一个二维矩阵0 表示可通行区域1 表示障碍物。索引方式要提前统一否则后面写迭代时会反复出错。直接使用map(row, col)在可视化时直观但做最短路搜索时我们更常把整个矩阵拉成一维数组用线性索引访问dist、prev、visited这些数组这样每个栅格只有唯一编号回溯路径时不需要来回换算行列坐标。function [nbr_idx, nbr_cost] get_neighbors(idx, map, use_diag) % 返回当前格子的可通行邻居索引和步进代价 [ROWS, COLS] size(map); [r, c] ind2sub([ROWS, COLS], idx); % 四邻域上下左右步进代价均为 1 moves [-1 0; 1 0; 0 -1; 0 1]; move_costs [1 1 1 1]; if use_diag % 八邻域追加四个对角方向步进代价取 sqrt(2) moves [moves; -1 -1; -1 1; 1 -1; 1 1]; move_costs [move_costs; ... sqrt(2); sqrt(2); sqrt(2); sqrt(2)]; end nbr_idx zeros(size(moves, 1), 1); nbr_cost zeros(size(moves, 1), 1); k 0; for i 1:size(moves, 1) nr r moves(i, 1); nc c moves(i, 2); % 越界检查 if nr 1 || nc 1 || nr ROWS || nc COLS continue; end % 障碍物检查 if map(nr, nc) ~ 0 continue; end % 对角移动时禁止从障碍物夹缝穿过 if moves(i, 1) ~ 0 moves(i, 2) ~ 0 if map(r, nc) ~ 0 || map(nr, c) ~ 0 continue; end end k k 1; nbr_idx(k) sub2ind([ROWS, COLS], nr, nc); nbr_cost(k) move_costs(i); end nbr_idx nbr_idx(1:k); nbr_cost nbr_cost(1:k); end这段代码的关键在最后那个对角检查当从(r, c)走向(r1, c1)时如果(r, c1)或(r1, c)是障碍物路径实际上在穿过一个只有对角接触的缝隙。对于轮式机器人来说这种路径不可行必须在建图阶段就过滤掉。moves和move_costs分别存放位移向量与步进代价use_diag为 false 时只走四邻域路径长度偏长但不需要处理斜穿判断为 true 时路径更自然代价也从单位 1 变成 1 和 1.414 混合。2.2 四邻域与八邻域的选型边界搜索方式邻居代价最优性典型应用BFS全部为 1栅格数最少无权图、硬四邻域Dijkstra1 或 sqrt(2)移动距离最短八邻域、加权地图A*1 或 sqrt(2) 启发函数移动距离最短单目标快速查询BFS 在四邻域图上等价于所有边权为 1 的 Dijkstra但它不关心对角线带来的真实距离差异。Dijkstra 的价值在八邻域图上才开始体现一条直走加一条斜走的路径格数是 2实际距离是 1 sqrt(2) 约等于 2.414而两条直走是 2Dijkstra 会正确选择后者BFS 则可能认为两者一样。2.3 什么时候用 Dijkstra 而不是 A*A* 的单次查询性能通常优于 Dijkstra因为它用启发函数把搜索方向推向终点。但启发函数需要满足一致性条件而且每张地图的启发代价设计都要单独验证否则容易出现“看起来快了、结果却次优”的问题。Dijkstra 没有启发式扩展方式固定在地图规模不大、或者要同时查询多个目标点时更省心。动态避障小车路径规划里如果障碍物频繁变化每次重规划都重新运行一次 Dijkstra在 100x100 以下的格栅地图上耗时通常只有几十毫秒这个代价完全可以接受。3. 用 MATLAB 实现 Dijkstra 路径规划的核心循环3.1 先准备一张可测试的格栅地图手写算法之前先把地图和起终点固定下来。随机地图虽然能测鲁棒性但排查代码问题时不可复现所以第一版建议用人工画的障碍物地图设置随机种子保证后续实验可重复。rng(2024); ROWS 25; COLS 25; map zeros(ROWS, COLS); % 三个矩形障碍物区域 map(6:8, 10:14) 1; map(12:18, 5:7) 1; map(15:20, 16:20) 1; start_idx sub2ind([ROWS, COLS], 2, 2); goal_idx sub2ind([ROWS, COLS], 24, 24);start_idx和goal_idx必须落在可通行栅格上否则算法一开始就无法启动或永远找不到终点。地图尺寸这里取 25x25便于读者在命令行直接imagesc(map)观察障碍物排布实际工程里 300x300 的格栅地图使用接下来这版代码仍然可以流畅运行。3.2 dijkstra_grid 主函数核心循环可以拆成三件事从候选集中取出代价最小的节点、判断是否到达终点、松弛该节点的所有邻居。候选集这里直接用普通数组open保存节点索引每轮调用min(dist(open))找最小值。这是最直观的写法适合理解算法本身。function [dist, prev] dijkstra_grid(map, start_idx, goal_idx, use_diag) % 基于线性索引的 Dijkstra 搜索 [ROWS, COLS] size(map); n_nodes numel(map); dist inf(n_nodes, 1); % 起点到每个节点的当前最短距离 prev zeros(n_nodes, 1); % 记录每个节点的前驱 visited false(n_nodes, 1); dist(start_idx) 0; open start_idx; while ~isempty(open) % 取出 open 中距离最小的节点 [~, ii] min(dist(open)); cur open(ii); open(ii) []; % 弹出终点时其最短距离已经确定可以提前结束 if cur goal_idx break; end visited(cur) true; % 遍历所有可通行邻居 [nbr_idx, nbr_cost] get_neighbors(cur, map, use_diag); for i 1:numel(nbr_idx) n nbr_idx(i); if visited(n) continue; end nd dist(cur) nbr_cost(i); if nd dist(n) dist(n) nd; prev(n) cur; % 已在 open 中的节点无需重复加入dist 数组会自动取到更新后的值 if isempty(find(open n, 1)) open(end 1) n; end end end end enddist数组的语义是“当前已知最短代价”初始化为 inf 表示尚未发现。prev保存前驱节点路径回溯时依赖它一步步走回起点。visited标记已经弹出的节点被弹出的节点不会再被更新这是 Dijkstra 正确性的关键当节点从open中被选出时它的dist一定已经是全局最小值。这里有两个容易写错的地方。第一if cur goal_idx的检查必须在松弛之前因为终点一旦弹出就可以直接终止但如果你改成在松弛邻居时发现终点就立即返回则可能拿到次优解第二open中可能包含同一个节点多次防止重复加入可以让数组保持精简即使不加这个判断算法也正确只是每轮min会做更多无用扫描。3.3 路径回溯与可视化输出dijkstra_grid返回后用prev从终点往前回溯再把路径转换为行列坐标绘制在地图上。path []; u goal_idx; while u ~ 0 path(end 1) u; if u start_idx break; end u prev(u); end path fliplr(path); if dist(goal_idx) inf error(终点不可达请检查障碍物或起点终点设置); end [pr, pc] ind2sub([ROWS, COLS], path); figure; imagesc(map); colormap(gray); axis xy; hold on; plot(pc, pr, r-, LineWidth, 2);while u ~ 0依赖prev中未访问节点默认值为 0 这个设定一旦终点不可达回溯会走到 dist 仍为 inf 的节点此时它的prev为 0循环会提前终止。建议在回溯前用dist(goal_idx) inf主动报错避免画出一条断掉的路径。axis xy的作用是把 y 轴翻转使图像的显示方向与矩阵行列方向一致否则路径会上下颠倒这是第一次用imagesc画规划结果时最常见的困惑。4. 参数怎么设、坑在哪格栅大小、膨胀与终止条件4.1 必调参数与推荐值参数推荐值影响注意事项ROWS / COLS与地图物理尺寸匹配搜索空间规模网格越细路径越平滑但内存和耗时按平方增长use_diagtrue路径长度与形态必须配套对角穿越检查move_cost1 / sqrt(2)路径在直走与斜走间取舍不要换成 1.4浮点误差会干扰最优性判断膨胀半径车体半径对应栅格数路径离障碍物距离膨胀后再搜索可以避免贴墙最大迭代次数节点总数的 5 倍防止死循环主要在 bug 排查阶段有用正常地图不会触发use_diag是路径规划需求里最容易被忽视的开关。四邻域路径往往会给出许多不必要的直角转弯八邻域路径更短更自然但在运动学约束较强的场景比如差速小车原地转向成本高时可能反而希望路径由直线段和固定角度转弯组成这时候四邻域加路径平滑是更合理的选择。4.2 对角穿越的规则变体前面get_neighbors中的对角检查是一种保守规则只要斜向经过的两个相邻栅格中有一个是障碍物就禁止通行。这个规则适配大多数室内机器人地图但有些场景会放宽为“允许穿越但代价更高”。无人机路径规划算法里经常使用这种策略因为无人机有高度自由度对角穿越在物理上可行。% 如果想要允许斜穿但附加惩罚代价可替换 get_neighbors 中的相关逻辑 if moves(i, 1) ~ 0 moves(i, 2) ~ 0 if map(r, nc) ~ 0 || map(nr, c) ~ 0 nbr_cost(k) sqrt(2) 1.5; % 穿越缝隙的额外通行代价 end end额外代价 1.5 表示比正常斜向移动多付出 1.5 个单位的轨迹风险算法会优先绕行实在没有绕行路径时才选择穿越。具体数值需要根据障碍物边界的安全距离标定地图分辨率越高这个惩罚值通常越大。4.3 障碍物膨胀与“贴墙路径”处理格栅地图一般是根据传感器数据生成的障碍物边界直接对应栅格边缘。如果直接用原始地图搜索得到的路径通常会贴着障碍物栅格走稍有定位误差就会碰撞。常见做法是先用形态学膨胀或距离变换把障碍物向外扩一圈再在膨胀后的地图上搜索。% bwdist 计算每个自由格到最近障碍物的距离图像处理工具箱自带 D bwdist(map 0); inflated map | (D 1.5); % 距离障碍物 1.5 格以内的区域不可走这里1.5表示机器人中心与障碍物边缘之间至少保留 1.5 个栅格的安全距离取值与机器人半径一致。膨胀代价是用空间换安全但膨胀半径过大会在窄通道处直接生成一条不可行的“断路”需要结合实际通道宽度反复调整。4.4 大网格下的收敛加速手段暴力循环找最小节点在大约 500x500 的格栅地图上开始变得明显2000x2000 时会直接卡住。有两个改造方向比较实用一是把普通数组open换成二叉堆每次从堆顶取最小值松弛邻居时更新堆二是利用格栅地图边权只有 1 和 sqrt(2) 的特点用 Dial 算法的桶结构把取最小时的扫描摊到每一个距离桶上。MATLAB 环境里如果不想自己维护堆结构也可以把地图转成graph对象再调用内置的shortestpath内部实现就是经过充分优化的 Dijkstra 变体但自己维护一版堆实现的好处是可以在松弛逻辑里自由添加动态避障、时间窗、路径平滑等自定义规则这也是工程改造时最常见的出发点。5. 一次算完多目标查询验证 Dijkstra 在格栅地图上的搜索行为5.1 多目标距离表从起点扩展到所有栅格大多数教材里的 Dijkstra 找到终点就提前退出但在调度场景里比如机器人需要按顺序访问多个充电桩或工位把goal_idx参数去掉让算法完整跑完整个格栅地图就能得到从起点到所有格子的最短距离表dist。后续任意一个目标点的查询都只是查一次数组代价几乎为零。[dist_full, ~] dijkstra_grid(map, start_idx, [], true); dist_map reshape(dist_full, ROWS, COLS); figure; imagesc(dist_map); colorbar; title(每个格栅到起点的最短距离);运行后可以看到距离等值线从起点像波纹一样向外扩展遇到障碍物时等值线被压缩绕行区域颜色明显加深。这正是 Dijkstra 搜索顺序的可视化验证四邻域下等值线呈菱形八邻域下呈八边形如果等值线在某处出现不连续突变说明邻居生成或代价设置有错误。5.2 与 A* 的对比验证方法验证 Dijkstra 结果的正确性最好的办法是和 A* 交叉比对。给 A* 用相同的邻居生成函数启发函数取曼哈顿距离或欧氏距离跑完比对两条路径长度是否一致。理论上只要 A* 的启发函数满足一致性二者最短路代价必然相同差异只在扩展节点数和耗时。对比指标DijkstraA*扩展节点数50x50 随机地图全部可达节点约 30%-50% 的节点单次查询耗时中等较快多目标重复查询一次扩展全图后续零成本每个目标都要重跑验证时不要只比一条路径建议随机生成 30 张地图每张设置多组起终点统计两个算法返回的路径代价差值的最大值。如果差值超过 1e-9大概率是 A* 的启发函数不满足一致性或者 Dijkstra 在斜向穿越规则上处理不一致。把这两个算法放在一个框架里测试既验证了正确性也让你清楚知道何时该用哪一个单目标快速响应交给 A*需要一次算完多目标距离表或者重规划频繁时Dijkstra 的全图扩展反而是更好的底层算子。本文还有配套的精品资源点击获取