简介基于栅格地图的Dijkstra算法路径规划是一份MATLAB实现的算法源码包面向机器人导航、游戏AI、GIS等场景中的最短路径求解。资源将地图抽象为栅格以0与非0区分可通行区域和障碍完整实现了从起点到终点的Dijkstra搜索代码覆盖地图建模、优先队列维护、邻居扩展、路径回溯与可视化标注等关键环节。压缩包共6个文件以5个m脚本为主另含1张结果示意图整体仅56KB轻量紧凑。已有5026人学习下载。通过阅读源码可掌握栅格地图的数据结构设计、Dijkstra算法在MATLAB中的实现技巧以及如何借助imagesc等函数直观展示规划结果多个可运行脚本支持自定义地图与起终点便于验证不同场景下的路径效果。对理解图搜索原理和路径规划工程实现均有直接帮助适合算法初学者、MATLAB使用者和机器人相关专业学生作为课程设计或入门参考。 做移动机器人导航时间长了你大概率会有这种感觉不管后面用多少“高级”的规划算法Dijkstra永远是那个最保底、最不容易出错的家伙。尤其是在栅格地图上做路径规划Dijkstra算法虽然不像A*那样带“脑子”但它把“最短路径”这件事掰开揉碎讲清楚了是很多机器人导航工程里绕不开的基础模块。这篇文章我想把一件事讲透栅格地图上怎么用Dijkstra算法做路径规划从地图建模的原理、算法核心逻辑到一份可以直接跑的Python实现再到我实际调试中踩过的坑。这篇文章适合正在入门机器人导航、ROS开发、移动机器人竞赛或者纯写路径规划算法的朋友。我不写那种收藏吃灰的“理论科普”而是尽量按我实际项目里怎么做的来写。1. 栅格地图建模先把物理空间变成数字棋盘路径规划之前得先把环境变成算法能处理的输入。栅格地图Grid Map是目前最直观也最常用的一种表达方式简单说就是把连续空间切成一格一格的像素每个格子要么是空地、要么是障碍物机器人就在这张“棋盘”上从起点挪到终点。但真正开始建模时问题马上来了格子切多大障碍物边界怎么留机器人能不能“切着角”走这些参数直接决定路径的安全性和规划速度不是随手设一个就能用的。1.1 分辨率定多少才合适先看机器人和地图规模栅格分辨率就是每个格子对应的物理尺寸比如0.05米/格意思是现实里5厘米一个格子。分辨率越高地图刻画得越精细但格子数量也会爆炸式增长搜索时间、内存占用都会跟着涨。按我平时的经验选择分辨率时有三个参照机器人底盘尺寸一般保证机器人内切圆半径至少占2到3个格子否则路径生成后机器人很容易蹭到障碍物。传感器精度激光雷达建图精度在厘米级把分辨率设到比传感器精度还高其实没有意义反而放大噪声。环境面积同一个校园地图0.05米/格可能产生几百万个格子Dijkstra算法在里面跑一遍会非常慢。大场景通常先降到0.1米/格或更粗做全局规划够用了。我做过一个食堂送餐小车的项目车宽约50厘米地图是70米乘20米的室内大平层最后选了0.1米/格。这样机器人两侧各有几个格子的余量路径点数量也控制在了十几万个级别Dijkstra跑一次大概几百毫秒整体还能接受。如果你做的是仿真验证、课程设计分辨率设成1像素对应0.5米也没问题关键是心里要清楚“一格到底代表多大”。1.2 障碍物膨胀宁可让路径多绕几步也别贴着墙走栅格地图上很多“障碍物”只有几个格子的边界但机器人是有体积的。如果直接把机器人的中心点当作路径点、贴着障碍物边缘规划结果就是车过不去、轮子卡在墙角的尴尬场面。所以建图之后几乎都要做一步操作障碍物膨胀。膨胀的基本思路是把每个障碍物格子向外扩张一圈圈内全部标记为不可通行。膨胀半径一般取机器人外接圆半径加上一点安全余量。比如小车半径25厘米我会往外再放10厘米的呼吸空间所以膨胀半径就是35厘米。在栅格地图上实现也很简单一种办法是用图像形态学里的膨胀运算另一种是对每个非障碍物格子计算到最近障碍物的距离距离小于膨胀半径就当作不可通行。后者更灵活后面做代价地图的时候也用得上。注意膨胀后起点和终点如果落在膨胀范围内Dijkstra会直接找不到路径。因此在算法运行前要检查起点和终点对应的格子是否是可通行状态必要时把落点吸附到最近的可通行格子。膨胀的代价是路径可能比真实最短路径要长一点因为“绕”过了膨胀圈。但对于实际机器人来说一条贴着墙但能安全走完的路径远比一条数学上最短但根本走不过去的路径有价值。1.3 4邻域和8邻域省时间还是走斜线的选择栅格地图上机器人的移动方向需要提前定义常见的有4邻域和8邻域两种。4邻域只允许上下左右移动8邻域在4邻域基础上加了四个斜对角方向。很多刚接触栅格规划的人会下意识选8邻域觉得这样路径更自然、不会出现“走直角拐弯”的问题。但8邻域要注意一个陷阱假设左上格和左下格都是障碍物机器人从当前格斜穿到右上格实际上是贴着障碍物的角穿过去的这在物理上很容易碰撞。处理办法是当横向和纵向相邻格中存在障碍物时禁止斜向移动否则会得到一个“穿墙”的路径。代价定义上4邻域每次移动代价是18邻域直线移动代价也是1斜线移动代价建议设置成约1.414也就是根号2这样算法才会真正比较“走直线”和“走斜线”哪种总代价更小规划出来的路径才符合视觉直觉。2. Dijkstra算法原理从起点“摊大饼”一样找最短路径Dijkstra算法是1956年提出的经典最短路径算法思路非常朴素每次都从“当前已访问节点”里找一个离起点总代价最小的点向外扩展一圈一直扩展到终点为止。这个“代价最小优先”的策略是它能保证全局最短路径的关键。我常跟人打比方Dijkstra像在地上倒一摊水水从起点同时向四周均匀扩散水最先漫到的那个点一定就是从起点到这个点的最短路径。只不过在带权图里“水面”扩散的速度要按代价换算代价大传播得就慢代价小传播得就快。2.1 为什么“代价最小优先”就一定得到最短路径这个结论我第一次学时也觉得想不通后来画了几张图就理解了。算法维护两个集合已经确定最短路径的节点集合和还没确定最短路径的候选节点集合常用优先队列实现。每次从候选集合里弹出总代价最小的节点把这个节点放进已确定集合然后检查它的邻居节点如果通过当前节点到达邻居的代价比原来记录的小就更新邻居的代价和父节点。关键是“当前弹出的节点代价已经是全局最小”这一点。因为所有边的代价都是非负的Dijkstra要求不能有负权边否则结论不成立所以哪怕后续绕路也不可能比已经弹出的这个代价更小。栅格地图里移动代价不是1就是1.414天然满足非负条件所以直接用没问题。在代码上我一般用堆heap来维护候选节点。Python里用heapq包每次压入(累计代价, 当前节点坐标)弹出时自动是代价最小的节点。这里的细节是坐标要换成一个可比较元组像我习惯用(row, col)或者用(x, y)。2.2 Dijkstra、BFS和A*差别到底在哪很多人会把Dijkstra和广度优先搜索BFS搞混因为它们都像水波扩散。区别在于BFS只适合所有边的代价都相同的情况它按层数扩展天然保证每层扩展的步数最少Dijkstra则适用于边权不同的图它按累计代价决定扩展顺序。栅格地图如果只用4邻域、每步代价都是1那BFS和Dijkstra的结果是等价的一旦引入8邻域斜向1.414的代价BFS就不行了Dijkstra才能正确算出“走斜线其实更快”。A则是在Dijkstra的基础上加了一个启发式估计比如当前点到目标点的欧氏距离或曼哈顿距离代价变成“已走真实代价 预估剩余代价”这样扩展方向会更“偏向”终点搜索节点数比Dijkstra少很多。Dijkstra的主要问题是太“老实”它会向起点周围所有方向均匀探索在大地图上会很慢。但Dijkstra也有A替代不了的优势任何修改后的地图上它都能保证严格最优不依赖启发式质量且实现简单、调试友好。2.3 格子代价不是只有“1”和“0”栅格地图里常见的存储格式是0表示可通行、1表示障碍物但Dijkstra算法里的“代价”跟地图数值不是一回事。地图数值只是告诉你这个格子能不能走而算法里的代价是“走过这个格子需要付出的成本”。可通行格子移动代价一般是1或1.414障碍物格子则直接被排除。实际项目里这个移动代价不一定非要固定。比如走廊中间可以设低代价、靠近障碍物区域设高代价这样规划出来的路径虽然长度可能略微变长但会更靠近安全区域、减少碰撞概率。这就是代价地图Costmap的思路Dijkstra照样适用只要把“代价”字段从1替换成对应的代价权重即可。只不过代价权重太大会导致路径宁可绕大圈也不靠近障碍物边缘这个权重需要现场调一般取1到3之间比较合理。3. 完整实现一份可直接跑的Dijkstra路径规划Demo下面我写一个不算复杂但足够完整的Python实现不用ROS也能跑。代码主要分成三块栅格地图生成、Dijkstra核心搜索、路径回溯与可视化。地图我随意画了一张带几个障碍物的测试图实际项目里替换成真实的栅格地图数据即可。3.1 环境准备和数据准备需要安装的依赖很少pip install numpy matplotlibheapq是Python标准库不需要额外安装。下面先创建一张30乘40的测试栅格地图手动设置几个长方形的障碍物区域模拟墙体和桌子import numpy as np import heapq import matplotlib.pyplot as plt def create_test_grid(height30, width40): grid np.zeros((height, width), dtypenp.uint8) # 墙体1横在中间的一段障碍 grid[10:14, 8:24] 1 # 墙体2右侧竖着的障碍 grid[6:20, 28:32] 1 # 桌子地图下方的一个矩形障碍 grid[22:26, 18:26] 1 return grid这里grid0代表可通行grid1代表障碍物。注意我只是为了验证算法逻辑如果用真实ROS导航的栅格地图通常每个格子的值表示占据概率需要先做阈值处理高于某个概率值比如0.65视为障碍物低于这个值的视为可通行。3.2 核心实现Dijkstra搜索与路径回溯接下来是核心的Dijkstra函数。我直接用8邻域并且做了“禁止贴角穿行”的处理避免路径斜穿障碍物顶角def dijkstra(grid, start, goal): h, w grid.shape # 检查起点和终点是否在地图范围内、是否可通行 if not (0 start[0] h and 0 start[1] w): return None if not (0 goal[0] h and 0 goal[1] w): return None if grid[start[0], start[1]] 1 or grid[goal[0], goal[1]] 1: return None # 8邻域行偏移、列偏移、对应代价 neighbors [ (-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0), (-1, -1, 1.414), (-1, 1, 1.414), (1, -1, 1.414), (1, 1, 1.414) ] distances np.full((h, w), np.inf) parent {} distances[start[0], start[1]] 0.0 heap [(0.0, start[0], start[1])] visited np.zeros((h, w), dtypebool) while heap: cost, r, c heapq.heappop(heap) if visited[r, c]: continue visited[r, c] True if (r, c) goal: break for dr, dc, move_cost in neighbors: nr, nc r dr, c dc if not (0 nr h and 0 nc w): continue if grid[nr, nc] 1: continue # 禁止斜穿顶角如果水平或垂直方向被障碍物挡住不允许斜着走 if dr ! 0 and dc ! 0: if grid[r, nc] 1 or grid[nr, c] 1: continue new_cost cost move_cost if new_cost distances[nr, nc]: distances[nr, nc] new_cost parent[(nr, nc)] (r, c) heapq.heappush(heap, (new_cost, nr, nc)) # 回溯得到路径 if (goal[0], goal[1]) not in parent and (goal[0], goal[1]) ! start: return None path [] node goal while node ! start: path.append(node) node parent[node] path.append(start) path.reverse() return path, distances[goal[0], goal[1]]这段代码里我觉得比较值得说的是两个细节。一个是visited数组因为同一个节点可能被多次压入堆中但一旦弹出并确认是最小代价后就不再需要重复扩展所以用visited标记来跳过冗余分支。另一个是斜穿判断走斜对角之前要检查两个相邻的横向/纵向格子是否有一个是障碍物如果是就不能走否则路径会沿着障碍物边缘“擦边”过去。3.3 从代码运行到可视化验证写好搜索函数后再写一个可视化函数把栅格地图、起点终点和规划出来的路径画出来。这一步在调试算法时特别重要很多时候只看坐标根本看不出问题一画图立刻暴露了。def visualize(grid, pathNone, startNone, goalNone): plt.imshow(grid, cmapgray_r, originupper) if start: plt.plot(start[1], start[0], go, markersize8, labelStart) if goal: plt.plot(goal[1], goal[0], ro, markersize8, labelGoal) if path: rows [p[0] for p in path] cols [p[1] for p in path] plt.plot(cols, rows, b-, linewidth2, labelPath) plt.legend() plt.show() if __name__ __main__: grid create_test_grid() start (2, 2) goal (26, 35) result dijkstra(grid, start, goal) if result is None: print(没有找到可行路径请检查起点、终点或障碍物设置) else: path, total_cost result print(f路径节点数{len(path)}总代价{total_cost:.3f}) visualize(grid, path, start, goal)我实际跑过很多次这个脚本一般输出路径节点数在40到70之间总代价和障碍物布局直接相关。你需要重点看的是路径是否出现“贴墙”走、有没有不自然的拐角、是否出现了斜穿障碍物顶角的情况。如果发现斜穿顶角多半是邻域判断那段代码没生效。这个实现的性能在30乘40的地图上毫秒级完成。但如果你把地图放大到1000乘1000Dijkstra可能会跑到秒级以上这时候就要考虑第4节里介绍的优化方法了。4. 实操中的现象、问题与排查技巧算法代码能跑只是第一步。我实际调试中真正麻烦的问题往往不是算法本身而是各种很“现实”的情况起点被障碍物占了、地图分辨率太高导致搜索太慢、路径看起来“不对劲”等等。4.1 常见错误与排查对照表我整理了一份常见问题速查表这些问题我在不同项目里基本都遇到过现象可能原因处理方法程序死循环卡住不返回起点无法到达终点且没有终止判断或者堆中节点重复弹出太多给单次搜索设置最大扩展节点数超时直接放弃路径斜着穿过障碍物角斜向邻居判断时没有检查相邻格子是否被占用补上“禁止斜穿顶角”的判断逻辑路径贴着障碍物边界走肉眼看着很悬栅格分辨率过高机器人实际体积大于格子尺寸先做障碍物膨胀再跑搜索大尺寸地图搜索非常慢栅格数量大Dijkstra均匀扩展了太多无用节点改用A*或者先用低分辨率地图做粗规划终点明明比起点高走的路径却很绕手动设置的障碍物把终点围成了“孤岛”检查终点附近的可通行区域是否被膨胀圈覆盖结果不是全局最短移动代价设置不一致比如对角线用了1而不是1.414统一代价改成根号2提醒如果你发现路径是正确的但总感觉“拐弯太多”不一定错很可能是栅格分辨率太低导致路径只能走直角拐弯。想顺滑的话后续再做路径平滑去掉多余转折点或者改用B样条拟合。4.2 性能优化从“能跑”到“跑得快”Dijkstra有个很明显的性能瓶颈它是无差别向四周扩展的。在一个空旷大场地里哪怕终点就在起点正前方Dijkstra也会把起点周围一圈一圈的节点全部扩展一遍这对全局规划来说有点浪费。我在项目里常用的三个优化手段按性价比排序双向Dijkstra从起点和终点同时向中间搜索两边在中间相遇就结束。在障碍物不太复杂的地图上搜索节点数大概能减少一半左右实现也不难。换成A*A*和Dijkstra代码差异其实非常小只在代价中加入了启发式h(n)。如果用的是8邻域启发式可以直接用欧氏距离如果是4邻域用曼哈顿距离。改进后搜索节点数经常能减少一个量级。分层规划先在一个低分辨率地图上跑粗路径再沿着粗路径建立一条窄带只在窄带的高分辨率地图上精细化搜索。这样可以兼顾大尺寸地图和路径质量代价是代码复杂度上升。我个人的经验是如果栅格地图边长在200格以内Dijkstra基本够用不用太担心性能如果地图边长超过500格又不方便降分辨率就认真考虑换A*。4.3 多场景扩展从静态全局规划到动态避障标题里看到热搜词里有个“动态避障小车路径规划”这块值得多说几句。全局路径规划里的Dijkstra是静态算法它基于的地图是固定的。一旦环境中出现动态障碍物比如行人、突然开过来的其他机器人静态规划出的路径很可能马上失效。常见做法是把全局规划和局部规划分开全局层用Dijkstra或A*在栅格地图上算出宏观路径局部层用DWA、TEB之类的方法做实时避障只在遇到动态障碍物时绕开局部一小段然后再回到全局路径上。我在实际项目里就是这样搭配用的Dijkstra负责“大方向不迷路”局部规划负责“细节不撞人”配合起来很稳。如果环境变化太频繁比如室内大量人员走动Dijkstra的实时重规划就会成为瓶颈因为每次重规划都要全图搜一遍。这时候需要D* Lite这类增量算法或者适当缩小重规划区域。不过初学者先别急着上增量算法把Dijkstra和A跑明白后面切DLite会轻松很多。5. 这块内容还能怎么延伸Dijkstra在栅格地图上的实现是一个“地基”性质的模块学到手之后后面可以往好几个方向扩展并且扩展路径都特别清晰。延伸方向一从Dijkstra改A*。代码改动量很小但规划速度提升肉眼可见。以后面试或者做比赛能在5分钟内说出这两个算法的差异和改写思路会显得基本功很扎实。延伸方向二从静态地图切换到代价地图。在栅格地图基础上增加代价层让机器人远离障碍物、远离未知区域规划出的路径会更“人性化”。延伸方向三对接ROS Navigation栈。把这篇里的核心逻辑封装成ROS节点或者nav_core插件输入地图数据输出路径消息就能直接接到move_base框架里距离真正能跑的机器人导航系统只差几步。另外栅格地图Dijkstra用C实现也很常见原理和Python版本完全一致主要区别是手动实现优先队列或者用std::priority_queue以及注意坐标索引从0开始。如果你未来想走机器人算法岗建议把这套逻辑再手撸一遍C版本对理解内存访问和算法性能会很有帮助。最后再说一个实际工程里的小经验输出路径之后记得做一步“路径点压缩”。Dijkstra返回的路径里有很多连续共线的点机器人跟踪这样的路径不仅会有大量冗余计算运动控制时也容易一顿一顿的。我的做法是遍历路径把处于同一直线上的中间点全部删掉只保留拐点。效果立竿见影路径从几十个点直接变成几个关键拐点后面接轨迹跟踪算法也轻松很多。本文还有配套的精品资源点击获取