企业官网建设流程全解析

简介：基于栅格地图的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邻域每次移动代价是1，8邻域直线移动代价也是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 matplotlib

heapq是Python标准库，不需要额外安装。下面先创建一张30乘40的测试栅格地图，手动设置几个长方形的障碍物区域，模拟墙体和桌子：

import numpy as np import heapq import matplotlib.pyplot as plt def create_test_grid(height=30, width=40): grid = np.zeros((height, width), dtype=np.uint8) # 墙体1：横在中间的一段障碍 grid[10:14, 8:24] = 1 # 墙体2：右侧竖着的障碍 grid[6:20, 28:32] = 1 # 桌子：地图下方的一个矩形障碍 grid[22:26, 18:26] = 1 return grid

这里grid=0代表可通行，grid=1代表障碍物。注意我只是为了验证算法逻辑，如果用真实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), dtype=bool) 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, path=None, start=None, goal=None): plt.imshow(grid, cmap='gray_r', origin='upper') if start: plt.plot(start[1], start[0], 'go', markersize=8, label='Start') if goal: plt.plot(goal[1], goal[0], 'ro', markersize=8, label='Goal') if path: rows = [p[0] for p in path] cols = [p[1] for p in path] plt.plot(cols, rows, 'b-', linewidth=2, label='Path') 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乘1000，Dijkstra可能会跑到秒级以上，这时候就要考虑第4节里介绍的优化方法了。

4. 实操中的现象、问题与排查技巧

算法代码能跑只是第一步。我实际调试中，真正麻烦的问题往往不是算法本身，而是各种很“现实”的情况：起点被障碍物占了、地图分辨率太高导致搜索太慢、路径看起来“不对劲”等等。

4.1 常见错误与排查对照表

我整理了一份常见问题速查表，这些问题我在不同项目里基本都遇到过：

提醒：如果你发现路径是正确的，但总感觉“拐弯太多”，不一定错，很可能是栅格分辨率太低导致路径只能走直角拐弯。想顺滑的话后续再做路径平滑，去掉多余转折点，或者改用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返回的路径里有很多连续共线的点，机器人跟踪这样的路径不仅会有大量冗余计算，运动控制时也容易一顿一顿的。我的做法是遍历路径，把处于同一直线上的中间点全部删掉，只保留拐点。效果立竿见影，路径从几十个点直接变成几个关键拐点，后面接轨迹跟踪算法也轻松很多。

本文还有配套的精品资源，点击获取