简介这套MATLAB代码围绕Dijkstra算法在格栅地图上的最短路径搜索展开适合机器人路径规划、智能车导航或算法课程设计场景。压缩包共4个文件包含3个.m脚本和1个.mat数据文件脚本涵盖主程序、核心算法实现与辅助函数数据文件提供可通行的格栅地图矩阵整体仅2KB轻量且便于二次开发。已有1780人学习下载。代码采用邻接矩阵建模通过维护距离向量与访问状态并借助优先队列优化节点选取最终利用前驱矩阵回溯最短路径。使用者可替换地图数据直观验证Dijkstra算法在静态格栅环境中的寻路效果也可对照代码理解初始化、迭代更新、邻居松弛与路径重构的完整流程是入门路径规划与算法仿真的实用参考。1. 为什么 Dijkstra 仍是格栅地图路径规划的首选定调算法在格栅地图上做路径规划很多人的第一反应是 A*毕竟有启发式函数可以加速收敛。但当你需要对障碍物和非均匀代价做频繁调整时A* 的启发式设计会带来额外的心智负担Dijkstra 算法不依赖启发信息只按累计代价一层层往外扩散代价最小者先被确定因此第一次带出终点时就是全局最优。这个特点让它在 MATLAB 里实现起来异常稳尤其适合课程作业、算法答辩以及给动态避障小车路径规划、无人机路径规划算法当基准线。当你需要实现 Dijkstra 算法求解格栅地图路径、交付可运行的 matlab 代码时核心就是这张地图和那个扩展循环把它讲透后面再学 A*、JPS 都会顺很多。这篇文章不搬运某个压缩包里的具体实现而是把这类工程最常见的建模方式、函数写法、参数调法和验证技巧完整过一遍。2. 格栅地图建模与 Dijkstra 算法的 MATLAB 表示2.1 把栅格地图变成带权图的两种常见做法栅格地图本身是二维数组0 代表障碍1 代表可通行。Dijkstra 要在这个数组上找一条从起点到终点的最小代价路径第一步是把地图转换成图。常见做法有两种第一种是稀疏图节点是每个格子边是相邻格子的连接关系MATLAB 里用graph()函数构造第二种是隐式图不把边存下来计算时动态枚举邻居。对格栅地图我更推荐第二种因为边和节点都排布规则动态枚举省内存也方便加各种检查条件比如对角线穿墙判断。隐式图的关键在于邻域集合。四邻域只有上下左右每一步代价相同八邻域还要处理对角方向代价要按欧几里得距离乘上sqrt(2)。下面这段代码定义了两种邻域% 四邻域偏移量是 [行偏移, 列偏移] neighbor4 [-1 0; 1 0; 0 -1; 0 1]; % 八邻域前 4 行还是上下左右后 4 行是对角 neighbor8 [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1];逻辑说明偏移量按“行、列”排列不是按“x、y”。这样可直接用在矩阵下标上。neighbor8里的行序不要求固定只要保持后 4 行为对角即可。参数选择上四邻域路径更长更多直角适合只能前后左右移动的 AGV八邻域路径更自然但必须加斜穿检查这点会在下文专门说明。2.2 用矩阵存代价值避开 MATLAB 稀疏图接口Dijkstra 的循环里要反复查询“当前未确定节点中谁的累计代价值最小”。如果用graph对象做每次更新权重还要调用rmedge和addedge语法冗长且容易被误解。我一般的做法是把代价值直接存成二维矩阵dist尺寸与地图一致初始值为Inf。这样既方便后续热力图显示又能在计算时用矩阵下标直接定位节点。dist Inf(rows, cols); dist(start(1), start(2)) 0; visited false(rows, cols); parent zeros(rows, cols, 2); parent(start(1), start(2), :) [start(1), start(2)];这段代码定义了 Dijkstra 需要的三个核心数据结构dist保存起点到每个格子的当前最短代价值visited标记该格子的最短路是否已经确定parent记录上一跳坐标用于最后回溯整条路径。这里选择用三维矩阵而不是 cell 数组保存 parent是因为三维矩阵在 MATLAB 的for循环里读写速度更快回溯代码也更紧凑。在地图大于 1000x1000 时可以拆成parentRow和parentCol两个矩阵避免第三维访问稍微变慢但对一般格栅地图三维写法最容易读。2.3 边权设计四邻域、八邻域与非均匀代价表边权是 Dijkstra 的全部输入。如果地图就是 0/1 二值图四邻域下每条边代价为 1算出来的就是最短步数八邻域下对角边要设成sqrt(2)否则会低估斜移代价导致路径偏爱斜走。实际地图通常不是均匀的草地、沙地、减速带都有不同通行成本。下面这张表是我处理地图时代价矩阵的常规设计地形类型地图值单步代价说明障碍0不可通行扩展时直接跳过普通路面11基准代价草地21.8模拟速度下降沼泽33.5只适合低速通过读取这类代价矩阵时只需要把costmap(nr,nc)当作边的权重对角线方向乘上sqrt(2)后再乘该格值。代价矩阵里不能出现 0 但又不是障碍的值否则“走不走都无所谓”会让算法产生很多等价路径。障碍值 0 要跟 Dijkstra 的初始化值Inf区分开代码里判断不可通行时看的是地图值而不是dist值这个细节在后面第 4 章会继续展开。3. 用 MATLAB 在格栅地图上跑通 Dijkstra 的最小代码3.1 函数骨架输入与输出我提供的实现叫dijkstra_grid输入是代价地图、起点、终点和邻域类型。完整函数代码如下你应该可以直接复制到本地运行。function [path, totalCost] dijkstra_grid(costmap, start, goal, neighborType) % DIJKSTRA_GRID 在格栅地图上用 Dijkstra 求最短路径 % costmap: 二维矩阵0 表示障碍正数表示通行代价 % start/goal: [row, col] 格式 % neighborType: 4 或 8 [rows, cols] size(costmap); if costmap(start(1), start(2)) 0 || costmap(goal(1), goal(2)) 0 error(起点或终点必须在可通行区域); end if nargin 4 || isempty(neighborType) neighborType 4; end dist Inf(rows, cols); dist(start(1), start(2)) 0; visited false(rows, cols); parent zeros(rows, cols, 2); parent(start(1), start(2), :) [start(1), start(2)]; if neighborType 4 offsets [-1 0; 1 0; 0 -1; 0 1]; diagMode false; else offsets [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1]; diagMode true; end while true minVal Inf; minIdx []; for i 1:rows for j 1:cols if ~visited(i,j) dist(i,j) minVal minVal dist(i,j); minIdx [i,j]; end end end if isempty(minIdx) path []; totalCost Inf; return; end if minIdx(1) goal(1) minIdx(2) goal(2) break; end visited(minIdx(1), minIdx(2)) true; for k 1:size(offsets, 1) nr minIdx(1) offsets(k, 1); nc minIdx(2) offsets(k, 2); if nr 1 || nr rows || nc 1 || nc cols continue; end if costmap(nr, nc) 0 || visited(nr, nc) continue; end if diagMode k 4 dr offsets(k, 1); dc offsets(k, 2); if costmap(minIdx(1)dr, minIdx(2)) 0 || ... costmap(minIdx(1), minIdx(2)dc) 0 continue; end stepCost costmap(nr, nc) * sqrt(2); else stepCost costmap(nr, nc); end newDist dist(minIdx(1), minIdx(2)) stepCost; if newDist dist(nr, nc) dist(nr, nc) newDist; parent(nr, nc, :) minIdx; end end end path []; cur goal; while any(cur ~ start) path [cur; path]; cur [parent(cur(1), cur(2), 1), parent(cur(1), cur(2), 2)]; end path [start; path]; totalCost dist(goal(1), goal(2)); end函数输入参数说明如下参数类型说明costmapdouble 矩阵每个格子的通行代价0 为障碍start1x2 向量起点[行, 列]goal1x2 向量终点[行, 列]neighborTypechar4或8主循环里我现在没有在visited之前判断终点。依赖的是 Dijkstra 的性质当终点从未扩展节点里以最小值被取出时它的 dist 已被收敛。如果你把判断放在“弹出后更新邻居前”也一样正确但写法会让初学者误以为需要多扩展一轮。代码中每次寻找最小节点都要扫描全图复杂度 O(V^2)对 200x200 地图完全够用超过 400x400 建议使用堆优化。3.2 主循环与终点判断为什么可以先 break主循环中有一行特别容易被误删visited(minIdx(1), minIdx(2)) true;如果漏掉算法会反复扩展起点因为起点的 dist 总是最小最终导致死循环。所有由栅格地图派生的 Dijkstra 代码都必须保证每个节点最多被扩展一次。当终点被挑出时我们直接break如果地图不存在通路最后一个可达节点扩展完后minIdx为空函数返回空 path 和Inf总代价。这里有一个常见的判断误区有人在minIdx为空时用error直接报错其实路径规划里“无路可达”是业务结果而不是异常返回空数组更合适调用脚本可以根据isempty(path)给出提示。另一个要注意的是newDist dist(nr,nc)更新条件。如果地图里有负边权Dijkstra 会失效但格栅地图的通行代价全为正数所以可以放心使用。对于非均匀代价地图stepCost使用的是目标格子的代价而不是当前格子的代价这意味着“进入草地”才计费。如果你希望把“停在当前格子”也产生代价需要在建图阶段先把当前格代价均匀到周围边的权重里。这个细节对泊车路径规划这类关心原地转弯代价的场景特别重要能用 Dijkstra 跑通不代表模型正确代价矩阵的语义要先定稿。3.3 回溯路径与绘制脚本回溯路径的代码在函数末尾。由于parent里存的是坐标沿 parent 从终点一路向前即可。注意起点不会被更新所以要手动设置parent(start) start否则回溯到起点后会取到[0,0]越界报错。绘制路径的脚本如下% visualize_dijkstra.m rng(7); costmap ones(30,30); for k 1:round(30*30*0.2) costmap(randi(30), randi(30)) 0; end costmap(1,1) 1; costmap(30,30) 1; [path, totalCost] dijkstra_grid(costmap, [1,1], [30,30], 8); imagesc(costmap); colormap(gray); hold on; if ~isempty(path) plot(path(:,2), path(:,1), r.-, LineWidth, 2, MarkerSize, 12); title(sprintf(Dijkstra path, total cost %.2f, totalCost)); else title(No path found); end axis equal; axis tight;这段脚本用rng(7)固定随机种子得到稳定可复现的地图。plot(path(:,2), path(:,1))因为图像显示时横坐标是列、纵坐标是行所以交换前两列否则路径会相对障碍物旋转 90 度。这里如果你的 MATLAB 版本比较旧记得先按 MATLAB 安装教程确认 Image Processing Toolbox 已勾选imagesc和colormap虽然不需要工具箱但后面要用bwlabel时没有工具箱会直接报错。4. 格栅地图 Dijkstra 的 4 个必调参数与常见坑4.1 邻域数量与斜穿障碍角检查参数neighborType决定路径形态。四邻域生成的路径只含直线段几乎不会出现机器人原地转不过弯的情况八邻域路径更短更自然但必须处理对角线穿墙。处理方法是从(i,j)走向(idi,jdj)时要同时检查(idi,j)和(i,jdj)是否可通行。两者只要有一个是障碍就禁止斜穿。不检查的代码长这样路径直接从墙角“咬过去”视觉上看好像比绕行短但真实机器人会撞墙。检查代码在dijkstra_grid里面已经写好了但你需要记住它与bwlabel的连通规则要统一四邻域 Dijkstra 对应四连通八邻域 Dijkstra 对应八连通。4.2 地图分辨率、膨胀半径与代价归一化栅格地图的每个格都有一个物理尺寸比如 0.05 米。Dijkstra 算出来的路径是栅格中心的连线直接下发到底盘前必须做膨胀。膨胀方法可以很简单% 障碍物向外扩张 robotRadius/cellSize 圈 radius ceil(robotRadius / cellSize); mapDilated imdilate(~costmap, strel(square, 2*radius1)); costmapDilated double(~mapDilated);注意这里先取反因为imdilate对 1 表示障碍的区域扩张得到的结果再取反还原成代价图。radius的单位是栅格数计算时向上取整宁大勿小。同一份代价图分辨率越低、膨胀圈数越少路径越贴近障碍物分辨率越高、膨胀圈数越多路径越保守但计算时间变长。地图代价的归一化也容易被忽视如果普通路面是 1草地是 1.8而相邻划分非常粗糙Dijkstra 可能宁可绕过很大一片草地也不走直线。先把代价归一化到[1, 10]区间再让 Dijkstra 运行得到的路径在高代价区域切换时不会突变。参数推荐值影响neighborType8 带斜穿检查路径平滑度与安全性折中膨胀半径ceil(robotRadius/cellSize)防止路径贴墙代价归一化1~10 整型避免高代价区被绕过bwlabel 连通性与 Dijkstra 邻域一致防止误判可达性4.3 用 bwlabel 判断起点终点连通性无解地图让 Dijkstra 把整个连通区域都耗尽才返回性能上很亏。用bwlabel先做连通域标记可以在算法开始前就结束cc bwlabel(costmap 0, 4); % 4 或 8 与 Dijkstra 邻域一致 if cc(start(1), start(2)) ~ cc(goal(1), goal(2)) path []; totalCost Inf; return; end这段预处理的时间复杂度是 O(rows*cols)虽然不会让整体变快多少但能提供明确的“不可达”反馈。使用 4 邻域时bwlabel的第二参数必须传 4否则会误判为可达。实际很多代码默认传 8四邻域 Dijkstra 用户看到bwlabel没加参数很容易出现结果为空还找不到原因的问题。4.4 边权为 0 和 Inf 混用时的行为差异最后一个常见坑在代码里没有直接显示但挺隐蔽dist初始值用Inf地图障碍值用 0这两个值混在一起使用时千万别把障碍物写到dist里。正确做法是代价矩阵costmap里的 0 只负责判断跳过。如果你用isinf(dist)去判断是否访问过那起点dist0反而被认为是未访问逻辑会错。另外有的老代码会用NaN替代Inf表示不可达但在比较时NaN永远为 false导致最小节点选择失效。因此统一用Inf并在函数入口处把所有NaN转成1或Infcostmap(isnan(costmap)) 0;。这样就算地图由图像处理流程生成也不会因为存在NaN让 Dijkstra 静默失败。5. 从算法到交付模块化打包与最短路径验证技巧以“Dijkstra算法求解格栅地图路径matlab代码.rar”为交付物时我会把文件拆成main.m、dijkstra_grid.m、visualize_dijkstra.m和README.txt。解压后直接运行main.m就能复现。为什么强调直接运行MATLAB 的当前路径经常是破碎的多级目录会让用户看到一堆未定义函数。把函数与脚本放同一目录并在main.m开头用mfilename获取当前文件路径再用cd(fileparts(mfilename(fullpath)))切过去能省掉很多“为什么报错”的提问。5.1 验证 Dijkstra 结果的三种方法验证最优性靠肉眼不够。我常用的三种方法依次是小地图手算、BFS 对照、距离变换交叉验证。小地图直接看 dist 矩阵0/1 代价地图上让 Dijkstra 与bwdist比较最后画代价热力图。下面这段脚本用bwdist来验证总代价% verify_path_cost.m clear; clc; costmap ones(32,32); costmap(10:14, 16:18) 0; % 障碍块 start [2,2]; goal [30,30]; [path, cost] dijkstra_grid(costmap, start, goal, 4); % 用图像距离变换做参考 D bwdist(costmap 0, quasi-euclidean); refCost D(goal(1), goal(2)) - D(start(1), start(2)); if abs(cost - refCost) 1e-9 disp(cost check passed); else error(cost mismatch: %.4f vs %.4f, cost, refCost); endbwdist计算每个格子到最近障碍的距离距离场的差可以作为最短路径代价的参考。这个验证不依赖路径本身只核对总代价能快速发现边权设置错误。quasi-euclidean是折中的距离度量与 Dijkstra 在四邻域下的结果近似但不完全等于四邻域步数。因此容差要放宽松到 1e-3。5.2 用代价热力图快速定位错误格子把dist矩阵画成imagesc(dist)对障碍物旁边的高代价区域做目检。最常见的异常是热力图里有几个格子颜色跟周围明显断层那通常是parent更新时用了上一格的代价而不是目标格代价。这种情况单看路径很容易漏掉热力图一眼就能看出来。另外可以把path画在热力图上方同时显示grid on方便逐格检查路径是否穿过障碍角。若你后面要迁移到 C同样可以把这个 dist 矩阵导出成 CSV在 MATLAB 里复用这段可视化逻辑。这套验证流程跑顺了再往无人机路径规划、动态避障小车路径规划这些高级场景扩展时你至少有底气说底层最短路径是正确的。本文还有配套的精品资源点击获取