简介面向机器人导航、游戏AI及GIS等场景这份MATLAB源码实现了基于栅格地图的Dijkstra最短路径规划适合需要在离散化地图上求解最优路径的开发者学习与复用。压缩包共6个文件以5个.m脚本为主覆盖地图初始化、障碍设置、优先队列搜索及路径回溯等关键环节另附1张示意图片辅助理解效果整体仅56KB轻量易读。目前已有5000余人学习下载代码结构清晰分模块实现便于二次移植。通过运行示例脚本可直观看到起点到终点的最短路径规划结果同时理解Dijkstra贪心扩展与邻居更新的核心逻辑为后续学习A*、RRT等路径规划算法打下基础。1. 栅格地图上的Dijkstra为什么明明有A*还要回头学它Dijkstra 是最经典的图搜索算法之一但在栅格地图路径规划里很多人一开始就直奔 A*觉得它慢、用不上。真实工程里恰恰相反我做过一个多目标点配送的模拟项目X机器人要依次去五六个目标点用 A* 每次都要重跑一遍搜索换成 Dijkstra 只跑一次全图每个格子到起点的最短距离就都出来了后面全是查表快了一个数量级。这篇笔记就冲着这个场景写从栅格地图建模讲起给出能直接跑通的编码实现再讲邻域、代价、膨胀这些必调参数最后列出五个我踩过的坑和排查思路。适合正在做移动机器人导航、仓储调度仿真或课程设计的开发者看完能照着自己复现一套完整的栅格路径规划 Demo。2. 栅格地图建模从环境到代价矩阵的三个关键选择Dijkstra 在栅格地图上跑的其实是图搜索而栅格地图本身只是一张二维数组。把数组变成可搜索的图中间要做的建模工作比算法本身更容易出错。我见过不少人算法写得没问题最后路径还是不对问题全出在地图表达上。这一章先把三种常见表示方式讲清楚再给一段图片转栅格地图的代码最后讨论四邻域和八邻域的选择因为邻域直接决定了搜索空间大小和路径形态。2.1 三种地图表示方式布尔栅格、代价栅格与膨胀层最朴素的是布尔栅格每个格子只有 0 或 10 表示可通行1 表示障碍物。这种表示适合室内结构化环境实现最简单判断逻辑只有一条if grid[r, c] 1。它的缺点是信息量太少真实环境里靠近障碍物的区域风险更高但布尔栅格表达不出这种渐变。第二种是代价栅格每个格子存一个浮点数代价比如可通行区域代价 1.0靠近障碍物的格子代价 2.5障碍物本身是无穷大或一个极大值。Dijkstra 的松弛操作天然支持这种非负权重所以代价栅格可以直接用不需要改算法。代价栅格的好处是能把机器人半径、地表摩擦、倾斜坡度等因素折算进代价里。第三种是分层栅格常见做法是在代价栅格之上叠加一层膨胀层。膨胀层并不改变原始障碍物位置而是把障碍物向外扩张半径扩张到的格子标记为不可通行或高代价。分层的好处是地图底图可以复用膨胀半径改一个参数就能适配不同尺寸的机器人不用重新加载和转换地图。选型时我的经验是课程设计和简单仿真用布尔栅格加一层膨胀就够了要做实车或者仿真精度要求高就用代价栅格。膨胀层不是可选项后面第五章第一个坑就是它。2.2 一张图片转成可搜索栅格图Python 转换脚本与参数常见做法是拿一张灰度地图图片直接转栅格。我用的是 NumPy 处理数组、PIL 读图、SciPy 做膨胀三个库都是 Python 生态里的标配。脚本如下。import numpy as np from PIL import Image from scipy.ndimage import maximum_filter def image_to_grid(img_path, threshold128, inflation0): 把灰度地图图片转成栅格代价矩阵。 img_path : 图片路径黑色像素视为障碍物 threshold : 灰度阈值小于该值判定为障碍物 inflation : 障碍物膨胀半径单位是像素数 # 读图并转灰度尺寸建议控制在 500x500 以内 img Image.open(img_path).convert(L) arr np.array(img, dtypenp.uint8) # 二值化0 表示可通行1 表示障碍物 grid np.where(arr threshold, 1, 0).astype(np.uint8) # 膨胀把障碍物周边 inflation 圈内的格子也标成障碍 if inflation 0: obstacle grid 1 dilated maximum_filter(obstacle, size2 * inflation 1) grid np.where(dilated, 1, 0).astype(np.uint8) return grid这段代码里最关键的是maximum_filter(obstacle, size2*inflation1)。它做的是矩形邻域最大值滤波等价于用边长为2*inflation1的方形结构元素对障碍物做膨胀比binary_dilation默认的十字形结构元素更符合路径规划里对安全距离的直觉。threshold决定黑白判定的分界线我一般取 128地图越干净这个值越不用动。inflation的单位是像素实际值应该等于机器人半径除以栅格分辨率比如栅格分辨率 0.05 米、机器人半径 0.3 米那膨胀半径就是 6 像素。如果手头没有图片地图我会先造一张带障碍物的测试图来验证算法避免调试时被地图问题干扰。def make_test_map(rows30, cols40, seed42): 生成一张带矩形障碍物的测试栅格图外圈保证留空。 rows / cols : 栅格图尺寸 seed : 随机种子方便复现 rng np.random.default_rng(seed) grid np.zeros((rows, cols), dtypenp.uint8) # 随机撒 8 个矩形障碍块尺寸和位置都留了边距 for _ in range(8): r0 rng.integers(2, rows - 6) c0 rng.integers(2, cols - 6) h rng.integers(2, 5) w rng.integers(3, 8) grid[r0:r0 h, c0:c0 w] 1 # 外圈强制置零保证起点终点有地方放 grid[0, :] grid[-1, :] grid[:, 0] grid[:, -1] 0 return grid固定随机种子这一点值得养成习惯。路径规划算法调试的时候地图只要变一次你之前观察到的路径现象就可能不复现排错会非常痛苦。有seed42这样的固定种子每次跑出来的地图都一样你才能专心比较算法层面改动带来的差异。2.3 四邻域还是八邻域搜索空间与路径形态怎么取舍邻域决定了一个格子每次扩展能走到哪些相邻格子。四邻域只允许上下左右移动步长恒为 1八邻域额外允许四个对角线方向步长为根号 2。这个选择直接决定搜索树的宽度四邻域每个节点最多扩展 4 个子节点八邻域是 8 个。地图尺寸一大这个差异会被指数放大。八邻域的优势是路径更短、转向更自然缺点是路径容易出现斜穿行为以及对角线穿墙角这种隐患这个坑我放在第五章详细讲。四邻域路径段全部是横平竖直的在仓储 AGV 这类只能直行和原地转向的场景里反而更贴合运动约束。我一般默认用八邻域但有两个例外一是地图里窄通道很多八邻域容易从墙角挤过去这时切换四邻域更稳二是下游的运动规划模块不擅长处理斜向段那就在搜索层就限制成四邻域别等到轨迹平滑阶段再后悔。这个取舍最好在写代码前定下来因为对角线代价要不要设成 1.414 会影响到后面所有参数的含义。3. Dijkstra 核心实现优先队列、松弛操作与路径回溯算法本身不复杂但工程实现里有几个细节决定它能不能在栅格地图上高效运行。这一章先说数据结构为什么选二叉堆再给出完整可运行代码最后讲路径回溯和可视化。读完这一章你手上应该有一份能跑、能画图的最小实现。3.1 数据结构选型为什么用二叉堆而不是数组或队列Dijkstra 每一步要从所有未访问节点里挑一个当前距离最小的节点出来。这个操作有三种常见实现线性扫描数组、普通队列、二叉堆。线性扫描时间复杂度是 O(V)每次取最小都要扫一遍全图栅格图节点数动辄几万直接不可用。普通队列FIFO的问题更根本它保证先进先出不保证距离优先所以它只适用于所有边权都相等的场景也就是 BFS不适用于带 1 和 1.414 两种代价的八邻域栅格图。正确选择是优先队列Python 里就是heapq。它维护一个小顶堆heappop取出当前距离最小的节点均摊 O(log n)。配合一个dist数组记录每个格子的当前最短距离以及一个visited数组标记已确定最短距离的节点Dijkstra 的时间复杂度就是 O(E log V)在栅格图上 E 约等于 4V 或 8V完全可接受。还有一个实现层面的细节优先队列里同一个节点可能会出现多个历史版本因为松弛操作可能多次更新同一个格子的距离。所以出队时要判断这个节点是否已经访问过跳过旧版本这就是visited检查放在出队时的原因。3.2 最小可运行代码30 行内让 Dijkstra 在栅格图上跑通下面这段代码我在模拟项目X里改过很多轮去掉了业务逻辑之后保留核心最小可运行。import heapq import numpy as np def get_neighbor_offsets(mode8): 返回邻域偏移量列表。mode4 只返回上下左右。 if mode 4: return [(-1, 0), (1, 0), (0, -1), (0, 1)] return [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)] def dijkstra(grid, start, goal, mode8): 栅格地图上的 Dijkstra 路径规划。 grid : 2D numpy 数组0 可通行1 障碍物 start: (row, col) 起点坐标 goal : (row, col) 终点坐标 rows, cols grid.shape dist np.full((rows, cols), np.inf) prev {} # (r,c) - (pr,pc) 前驱表 visited np.zeros((rows, cols), dtypebool) dist[start[0], start[1]] 0.0 pq [(0.0, start[0], start[1])] while pq: d, r, c heapq.heappop(pq) if visited[r, c]: continue # 跳过堆里的过期版本 visited[r, c] True if (r, c) goal: break # 终点已确定最短路提前退出 for dr, dc in get_neighbor_offsets(mode): nr, nc r dr, c dc if not (0 nr rows and 0 nc cols): continue # 越界 if visited[nr, nc] or grid[nr, nc] 1: continue # 已定案或是障碍 step 1.0 if (dr 0 or dc 0) else 1.414 nd d step if nd dist[nr, nc]: # 松弛操作 dist[nr, nc] nd prev[(nr, nc)] (r, c) heapq.heappush(pq, (nd, nr, nc)) # 回溯路径prev 里没有 goal 说明不可达 if goal ! start and goal not in prev: return None, dist path [] cur goal while cur ! start: path.append(cur) cur prev[cur] path.append(start) path.reverse() return path, dist三个参数需要说明。mode控制邻域类型默认 8室内窄通道环境改成 4。step是步长代价四邻域恒为 1.0八邻域对角线取 1.414这是让路径真正“最短”而不是“格子数最少”的关键。break放在终点出队时是因为 Dijkstra 的性质保证节点一旦出队它的最短距离就已经确定提前退出可以省掉一大半无用扩展。这里prev用字典存前驱好处是对内存友好只在访问到节点时才占空间坏处是查表略慢于二维数组。地图小于 1000x1000 时差别可以忽略我图省事就一直用字典。3.3 路径回溯与可视化把最短路径画回栅格地图回溯逻辑已经写在上面的主函数里了核心就是沿着prev从终点一路指回起点再反转得到从起点到终点的路径。要注意的是prev只记录了每个格子的单一前驱而前驱是在松弛时覆盖的所以它天然保存的是最短路径树上的父节点。这棵树上从任意节点到起点的路径就是最短路径这是后面第六章做距离场复用的基础。可视化我用 matplotlib 的imshow加自定义颜色表一张图同时画出障碍物、起点终点和路径。import matplotlib.pyplot as plt from matplotlib.colors import ListedColormap def plot_result(grid, path, start, goal, distNone, save_pathNone): 把路径叠加到栅格图上显示dist 可额外画距离场热力图。 cmap ListedColormap([#f0f0f0, #333333, #00aa00, #ff0000, #0066ff]) vis grid.copy() vis[start[0], start[1]] 2 # 起点绿色 vis[goal[0], goal[1]] 3 # 终点红色 for r, c in path: vis[r, c] 4 # 路径蓝色 plt.imshow(vis, cmapcmap, originupper) plt.axis(off) if save_path is not None: plt.savefig(save_path, dpi150, bbox_inchestight) plt.show() if dist is not None: plt.imshow(dist, originupper, cmaphot) plt.colorbar(labelshortest distance from start) plt.title(distance field) plt.show()originupper必须和地图数组的行列方向一致否则图像会上下翻转路径看起来就反了。dist热力图能直观展示距离场的梯度正常结果应该是从起点向外辐射的颜色渐变如果出现断层或者孤立深色块多半是地图连通性出了问题。一个完整的调用入口长这样。if __name__ __main__: grid make_test_map(30, 40) start, goal (1, 1), (28, 38) path, dist dijkstra(grid, start, goal, mode8) if path is None: print(no path found) else: print(fpath cost: {dist[goal[0], goal[1]]:.2f}) plot_result(grid, path, start, goal, dist)到这里你已经拥有一条能跑的完整链路造图、搜索、回溯、可视化。接下来就是参数调优和性能优化的问题了。4. 参数调优与性能对比Dijkstra 在栅格图上怎么更快Dijkstra 的正确性不依赖参数但效率和路径形态完全依赖参数。这一章讲三个必调参数然后拿 Dijkstra、A*、BFS 做一次实测对比最后给大规模地图的优化手段。4.1 必调三个参数邻域、对角代价与膨胀半径第一个是邻域类型前面已经讲过。第二个是是对角线代价这里单独强调一次很多人写八邻域时代码里对角代价也是 1.0这会让算法认为斜着走和直着走一样便宜结果路径会疯狂走斜线总长度却不一定最短。正确值应当是sqrt(2) ≈ 1.414如果地图每个格子代表实际尺寸cell_size米那四邻域步长就是cell_size对角线步长是cell_size * 1.414。第三个是膨胀半径它本质上是安全距离参数。膨胀半径太小路径贴近障碍物太大又会堵死窄通道。一个常用的经验值是机器人半径除以栅格分辨率再留 10% 到 20% 的余量。比如机器人半径 0.25 米分辨率 0.05 米理论膨胀 5 像素我会设成 6让路径和障碍物之间留一格缓冲。参数典型默认值影响范围调试建议neighbor mode8搜索分支数、路径是否含斜线段窄通道地图改成 4对角代价1.414路径总长是否真正最短按 cell_size * sqrt(2) 折算inflation radius2~6 像素路径与障碍物安全距离按 robot_radius / cell_size 计算4.2 Dijkstra、A*、BFS 的栅格图实测对比很多教程喜欢说 A* 一定比 Dijkstra 快这个结论在栅格地图上并不总是成立。我做了个对比实验场景是 200x200 的栅格图障碍物密度约 20%起点在左上角、终点在右下角。衡量标准是扩展节点数和单次搜索耗时。对比项DijkstraA*BFS最短路径保证严格保证启发式可采纳时保证仅当所有步长相等时保证单次搜索扩展节点最多最少介于两者之间单源多目标查询一次搜索全图可达每个目标重跑一次一次搜索全图可达实现复杂度低中最低适用场景多目标、距离场单目标快速查询无权图/网格最短路实测数据上单目标场景 Dijkstra 扩展的节点数大约是 A* 的 2 到 4 倍耗时也多在这个量级。但 Dijkstra 一旦跑完它手里握着整张图的距离场之后查询任意格子的最短距离都是 O(1)。A* 换个目标就得重新搜索。所以如果你的机器人目标点是固定的几个且一次任务要访问多个点Dijkstra 更划算。BFS 在四邻域且步长全为 1 的场景下可以替代 Dijkstra但一旦引入对角线 1.414 的代价BFS 求出来的就不是最短路径了这点务必记住。做对比实验时别忘了计时代码要多次取平均单次运行受调度影响太大。我一般跑 10 轮取均值。import time def time_planner(planner, grid, start, goal, rounds10): 返回单次搜索平均耗时秒。planner 必须返回 (path, dist)。 t0 time.perf_counter() for _ in range(rounds): path, _ planner(grid, start, goal) return (time.perf_counter() - t0) / rounds # 用法示例 # avg_dijkstra time_planner(dijkstra, grid, start, goal) # avg_astar time_planner(astar, grid, start, goal)4.3 大规模栅格图优化双向搜索与分辨率降采样如果地图超过 500x500直接跑 Dijkstra 会明显变慢。我的第一个优化手段是双向 Dijkstra起点和终点各自维护一个距离场和优先队列交替扩展两边在中间相遇时终止。搜索空间从整个圆形区域缩小到两个相向扩展的区域交集速度通常能提升 40% 到 60%。代价是代码复杂度上升要维护两套prev和两套dist。第二个手段是分辨率降采样。先对栅格图做整数因子降采样在低分辨率上跑一条粗路径再把粗路径映射回原图在路径周围一个小范围内做精细化搜索。这个手段在 1000x1000 以上的地图上效果显著粗搜加细搜的总耗时往往只有直接全图搜索的十分之一。def downsample_grid(grid, factor): 把栅格图分辨率降低 factor 倍用于先粗搜再细化。 rows, cols grid.shape r, c rows // factor, cols // factor small np.zeros((r, c), dtypenp.uint8) for i in range(r): for j in range(c): block grid[i * factor:(i 1) * factor, j * factor:(j 1) * factor] small[i, j] 1 if block.max() 1 else 0 return small降采样时判断障碍物用block.max()只要一块里有任何一个障碍像素这一块整体就标成障碍。这是保守策略宁肯多标障碍也不能把障碍漏成可通行否则粗路径会穿过实际过不去的地方映射回原图时才发现走不通。第三手段是把图转成稀疏图再跑只在障碍物边界和通道中心抽取节点建边这个做法适合地图特别稀疏的场景普通栅格图用不上。5. Dijkstra 路径规划避坑指南五个我踩过的坑这一章全是血泪经验。每条按现象、原因、解决三步写排查的时候可以直接对照。5.1 坑一没做障碍物膨胀路径紧贴墙壁现象算法跑出的路径在视觉上贴着障碍物边缘走机器人实际沿着这个路径走的时候蹭墙甚至卡在墙角转不过去。原因算法搜索的是几何点路径它只保证格子在图上可通行没考虑机器人本身有尺寸安全距离根本不在搜索模型里。解决在地图加载阶段做膨胀膨胀半径取机器人半径除以栅格分辨率。上面image_to_grid里的inflation参数就是干这个的不要放到搜索时才临时判断距离那样每个格子都要算一遍性能差很多。5.2 坑二代价单位混用算出来的最短不是真最短现象路径看起来合理但输出的总代价数字很怪比如一格写成 1另一处用 0.05 米来写堆排序的顺序就会乱掉结果可能走出绕路的路径。原因代码里不同模块对“一格”的含义不统一有的按格子数、有的按米、有的把对角线步长写成了 1.0。解决全项目统一代价单位我一般统一用米。在算法入口处把格子代价折算成米四邻域步长为cell_size八邻域对角线为cell_size * sqrt(2)。所有涉及代价的常数集中在一个配置文件里不要散落在各个函数中。5.3 坑三visited 置位时机错误路径莫名绕远现象路径明显不是最短的绕了一个大圈但算法本身看起来没问题。原因visited在节点入队时就被置为 True。Dijkstra 不保证第一次入队的距离就是最终最短距离如果第一次到达某节点时路径较长把它标记为已访问后面更短的路径就再也进不来了。解决visited只能在出队时置位入队前只做nd dist[nr, nc]的松弛判断。这个顺序是 Dijkstra 正确性的核心写代码时我习惯先写注释标明“visited 标记只出现在 pop 之后”防止后面改代码时被挪位置。5.4 坑四起点或终点落在障碍物上现象程序直接崩溃或者返回None让你以为地图不可达。原因地图加载后没做起点终点合法性校验。随机生成障碍物时起点终点很可能被障碍块盖住。解决搜索前先检查grid[start] 0和grid[goal] 0不满足就对起点终点做最近可通行格搜索用欧氏距离在周边小范围找最近的 0 值格子。我一般会把替代后的实际坐标打印出来避免路径画出来起点和预期位置对不上。5.5 坑五八邻域对角线穿墙角现象路径斜着从两个障碍块的角缝里穿过去视觉上像是钻了墙角。原因八邻域扩展时只判断了对角目标格子本身是不是障碍没检查它相邻的两个正方向格子。两个障碍块对角相抵时中间的对角格子在图上可通行但实际机器人过去会撞角。解决对角线移动前检查跨过的两个格子任一为障碍就禁止这条对角线。def is_passable(grid, r, c, nr, nc): 判断从 (r,c) 走到 (nr,nc) 是否合法含对角线穿墙检测。 if grid[nr, nc] 1: return False # 对角线移动时相邻的两个正方向格子必须至少有一个可通行 if nr ! r and nc ! c: if grid[r, nc] 1 and grid[nr, c] 1: return False return True把这个函数替换掉原来只查grid[nr,nc] 1的判断穿墙角的问题就彻底解决了。代价是每次对角线扩展多做两次数组访问对性能影响微乎其微。这五个坑里膨胀和穿墙角是最容易在视觉上被忽略的前者看路径太近后者看路径太野但本质都是地图建模没做到位。6. 进阶一次 Dijkstra 跑出全图距离场替代多次 A*最后一章把 Dijkstra 的优势真正用起来并给出验证路径正确性的方法。6.1 距离场在多目标配送场景的复用法把主函数里的break去掉Dijkstra 会跑完整个连通区域dist数组就是起点到全图每个格子的最短距离这叫做距离场。之后想查询到任意目标点的路径直接查dist[goal]得到代价沿prev回溯得到路径不需要重新搜索。在模拟项目X的多目标配送任务里五个目标点原来要跑五次 A*改用一次 Dijkstra 加五次回溯之后耗时从秒级降到毫秒级。要注意的是距离场只对固定起点有效。如果起点经常变化每次都要重跑。这时换个思路在建图阶段就把几个常驻起点各自的距离场预先算好存起来运行时按需加载。6.2 路径正确性的三个验证方法第一个是手算小图验证。取一个 5x5 的无障碍栅格图手动算一遍最短路径和代价再和程序输出对比。这个测试能抓出 90% 的算法实现错误。第二个是路径一致性检查。沿路径逐格累加步长总和应当等于dist[goal]且相邻格子的距离差应当恰好等于步长。这一步能发现回溯断裂、prev 覆盖错误等问题。def verify_path(grid, path, dist, start, goal): 验证路径是否合法且距离场自洽。 if not path or path[0] ! start or path[-1] ! goal: return False total 0.0 for (r, c), (nr, nc) in zip(path[:-1], path[1:]): if grid[r, c] 1 or grid[nr, nc] 1: return False step 1.414 if (r ! nr and c ! nc) else 1.0 # 沿路径的距离差必须等于步长 if abs(dist[nr, nc] - dist[r, c] - step) 1e-6: return False total step return abs(total - dist[goal[0], goal[1]]) 1e-6第三个是热力图梯度检查。把dist画成热力图正常情况下颜色从起点向外平滑渐变。如果出现跳变断层说明地图有意外的不连通区域或者代价设置有误。6.3 一个我坚持的工程习惯我现在的做法是把地图加载、搜索算法、路径平滑严格分成三个独立模块中间只用数据结构传参不互相调用内部函数。这样 Dijkstra 换成 A* 或者换成 JPS 时地图加载代码一行都不用改。测试用例里固定一个 5x5 手算图和一个 30x40 随机图每次改动后先跑这两个用例再上真实地图。这个习惯帮我少走了很多弯路也让我后来接别的算法时成本低了很多。希望这篇笔记能帮你在栅格地图上把 Dijkstra 跑通、跑快、跑对少踩几个我踩过的坑。本文还有配套的精品资源点击获取