做移动机器人项目路径规划是第一道坎。你可能听说过各种算法A*、RRT、Dijkstra其中Dijkstra是被误解最深的一个。很多人觉得它慢不如A*但真正到了嵌入式环境里没有好的启发式函数时Dijkstra反而是最稳的选择。这篇内容不是我凭空写的几乎每一行代码都在某AGV项目上跑过包括后面的几个坑也是我踩出来的。适合刚入门移动机器人规划、想把路径规划原理和代码吃透的读者。1. 路径规划问题与算法选型思路1.1 移动机器人路径规划的核心诉求移动机器人的路径规划本质上是在一个已知或部分已知的二维空间里从起点到终点找一条无碰撞的路线。听起来简单但工程实现上有三个约束第一路径必须安全不能穿过障碍物第二路径应该尽量短至少不能绕远路第三计算耗时要可控尤其后续需要做局部避障时全局路径规划不能占满控制器。在室内场景里最常见的地图表示方式是栅格地图也就是把环境切成一格一格的方块。机器人的位置对应某个格子障碍物对应被标记的格子。这个离散化过程会带来分辨率问题栅格太大路径精度不够栅格太小节点数量爆炸。在我做过的某移动平台项目里0.05米分辨率的栅格地图会导致几万个节点这时候搜索算法的效率差别会被放大得很明显。1.2 为什么选择Dijkstra而不是A*或RRT很多新手一上来就选A*理由是比Dijkstra快但我不建议在没搞懂Dijkstra之前直接上A*。A*也是广度优先搜索加贪心思想它的速度依赖于启发式函数的设计如果启发式函数选得不合适可能找不到最优解或者搜索范围反而更大。而Dijkstra算法不需要任何启发式信息它只根据边的代价来扩展节点保证找到的路径一定是全局最短路径。算法是否保证最优需要启发式函数适用场景Dijkstra是否小规模图、静态地图、无启发式可用A*是启发式满足一致性时是大规模图、已知环境、需要快速响应贪心最佳优先否是实时避障不要求全局最优RRT否概率完备否高维空间、连续空间、复杂约束如果你在室内AGV上做全局路径规划地图大小几百乘几百栅格数量几万个A确实通常比Dijkstra快。但如果你的地图非常复杂障碍物分布奇怪或者你现在还不想花时间调启发式函数那Dijkstra就是一个比A更可靠的选择。它还有一个好处搜索过程中每个节点的dist值都保留了你可以直接拿到任意点到起点的最短距离这在某些需要计算距离场或做导航后处理的场景里很实用。1.3 Dijkstra的核心思想与适用边界Dijkstra算法的核心思想可以概括成一句大白话在所有已经知道距离的节点里每次挑最近的那个没处理过的节点往外扩展一圈。这个过程像水波扩散从起点的涟漪一层层往外推直到某个波峰触碰到终点。这里有一个关键点为什么每次挑最近的那个节点最终得到的最短路就是正确的因为如果当前节点是未处理节点里距离起点最近的那么所有能通过其他未处理节点到达它的路径代价都不会比当前已知值更小。这个性质在数学上叫“贪心选择性质”也是Dijkstra正确性的保证。不过Dijkstra不是万能的。它要求所有边的代价非负如果地图里出现了负权边算法会失效。对于栅格地图来说相邻格子之间的代价永远是正数所以完全不用担心这个问题。另一个适用边界是地图相对静态如果障碍物频繁变化每次变化都要重新跑一遍Dijkstra计算量会非常大这时候更适合用D* Lite或局部避障算法。但作为入门和基础方案Dijkstra仍然值得做扎实。2. Dijkstra算法原理详解2.1 图模型构建栅格地图与邻接关系用Dijkstra之前先把地图转化为图。栅格地图中每个格子可以看作一个节点格子与相邻格子之间用边连接。这里涉及一个选择四邻域还是八邻域。四邻域只允许上下左右移动代价都是1。八邻域允许上下左右加上四个斜对角方向斜对角移动的实际距离是根号2所以代价设为1.414会更准确。如果机器人的运动模型不支持原地转向或者在很窄的通道里跑八邻域可能会生成无法执行的路径所以具体用哪种邻域要结合机器人底盘的运动能力来确定。建图的时候还需要注意边界处理。比如地图左上角是(0,0)右下角是(rows-1, cols-1)。每个节点最多有8个邻居但在边缘会减少。在代码实现里最偷懒的办法是写一个get_neighbors函数在里面判断邻居坐标是否越界以及对应格子是否可通行。2.2 算法流程与优先级队列原版的Dijkstra算法用数组来维护距离每次找最小节点需要遍历全部节点复杂度是O(V^2)。在栅格地图里几万个节点的时候这个复杂度完全不可接受所以工程实现时必须用优先级队列也就是堆。算法流程可以拆成四步初始化距离数组dist起点距离设为0其他节点距离设为无穷大。把起点放入优先队列队列元素格式是(距离, 行, 列)。循环弹出距离最小的节点如果该节点已经访问过则跳过否则标记为已访问然后遍历它的邻居计算通过当前节点到达邻居的距离。如果这个新距离比邻居原来的dist小就更新dist并把邻居放入队列。如果弹出的是终点直接停止搜索因为Dijkstra第一次弹出终点时这个距离就是全局最短距离。这里有一个容易踩的坑当终点被弹出来时千万不要因为“找到了终点”就立刻停止要理解为什么可以停。因为优先队列保证了弹出的顺序是按照dist从小到大排列的如果终点已经弹出那么任何还在队列里的节点其距离都不会小于终点的距离再也不可能找到更短的路径了。2.3 启发式信息的缺失与代价计算Dijkstra和A最大的区别在于是否有启发式信息。A的估价函数是f(n) g(n) h(n)其中h(n)是当前节点到终点的估算距离可以引导搜索方向。Dijkstra相当于h(n)恒等于0所以它没有任何方向感只会均匀地向四周扩散。这个特性带来了两个结果优点是Dijkstra对所有终点都保持同样的最优性不会因为启发式设计失误而陷入局部最优缺点是搜索范围通常比A更大。在一个开阔的栅格地图里如果起点和终点在两头Dijkstra会把两块区域之间的所有节点都扫一遍而A只会沿着一个锥形区域搜索。代价计算方面栅格地图里我们通常用欧氏距离作为边权。如果地图中的某些区域有额外的通行成本比如草地、斜坡或禁区可以在邻接代价中加入加成。这在做移动机器人路径规划时非常重要因为最短的路径不一定是最快、最安全的路径。3. Python代码实现与关键注释3.1 代码结构与依赖我在这份示例代码里只用了Python标准库中的heapq和math没有引入任何重依赖这样方便你直接复制到嵌入式控制器或者PC上跑。地图用二维列表表示格子为0表示可通行格子为1表示障碍物。移动方式用参数neighbor_type控制4表示四邻域8表示八邻域。代码整体分为四个部分get_neighbors负责生成邻居dijkstra主函数负责搜索reconstruct_path负责回溯路径最后是一个简单的测试用例。为了让你看清每一行的作用我会在后面的3.3小节给出完整代码并且加上非常详细的注释。3.2 核心函数详解初始化、松弛操作、路径回溯先看初始化。dist数组的维度与grid一致每个格子初始化为float(inf)。parent数组用于记录每个格子是从哪个格子走过来的这样搜索完之后可以从终点一路回溯到起点。visited数组是一个布尔矩阵用来防止重复处理同一个节点。再来看优先级队列。heapq默认是按照元组的第一个元素从小到大排列的所以我们把(dist, row, col)塞进堆里。当某个节点的距离被更新时我们往堆里再推一次而不是修改堆里已有的元素。这样堆里可能会出现同一个节点的多个记录但因为dist是单调递减的旧记录在后续弹出时如果发现当前dist已经比旧记录小就直接跳过不会破坏正确性。松弛操作是Dijkstra的核心假设当前弹出节点是cur它到起点的距离是cur_dist我们遍历cur的所有邻居nb计算new_dist cur_dist cost(grid[cur], grid[nb])。如果new_dist小于dist[nb]就更新dist[nb]并记录parent[nb] cur然后把(new_dist, nb.row, nb.col)压入队列。这里需要注意如果是复杂地形代价函数不是固定的1或者1.414而是可以做成读取地图属性表。路径回溯很简单从终点开始沿着parent一个一个往前找直到起点然后把列表反转。如果parent[goal]为None说明起点和终点之间没有通路。3.3 完整代码带注释下面这一版代码我在多个仿真环境里测过朋友想在某个跨平台项目里用也可以直接搬。注释已经写得很细基本上每三五行就有一句解释。import heapq import math from typing import List, Tuple, Optional def get_neighbors(grid: List[List[int]], row: int, col: int, neighbor_type: str) - List[Tuple[int, int]]: 返回某个格子的所有可通行邻居。 grid: 二维地图0表示可通行1表示障碍物 row, col: 当前格子的行、列索引 neighbor_type: 4表示四邻域8表示八邻域 rows, cols len(grid), len(grid[0]) neighbors [] if neighbor_type 4: directions [(-1, 0), (1, 0), (0, -1), (0, 1)] else: # 八邻域 directions [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)] for dr, dc in directions: nr, nc row dr, col dc if 0 nr rows and 0 nc cols: if grid[nr][nc] 0: # 只允许可通行格子 neighbors.append((nr, nc)) return neighbors def reconstruct_path(parent: List[List[Optional[Tuple[int, int]]]], start: Tuple[int, int], goal: Tuple[int, int]) - List[Tuple[int, int]]: 从终点反向回溯重建完整路径。 parent[row][col] 记录了格子(row,col)的前驱节点。 path [] cur goal while cur is not None: path.append(cur) cur parent[cur[0]][cur[1]] path.reverse() return path def dijkstra(grid: List[List[int]], start: Tuple[int, int], goal: Tuple[int, int], neighbor_type: str 4) - Optional[List[Tuple[int, int]]]: 基于栅格地图的Dijkstra最短路径搜索。 grid: 二维地图0可通行1障碍物 start: 起点坐标 (row, col) goal: 终点坐标 (row, col) neighbor_type: 4或8选择四邻域或八邻域扩展 返回从起点到终点的路径列表若无通路则返回None rows, cols len(grid), len(grid[0]) # 检查起点和终点是否在地图范围内且可通行 if not (0 start[0] rows and 0 start[1] cols): return None if not (0 goal[0] rows and 0 goal[1] cols): return None if grid[start[0]][start[1]] ! 0 or grid[goal[0]][goal[1]] ! 0: return None # 初始化dist为无穷大起点距离为0 dist [[float(inf) for _ in range(cols)] for _ in range(rows)] dist[start[0]][start[1]] 0.0 # parent记录路径前驱visited防止重复处理 parent [[None for _ in range(cols)] for _ in range(rows)] visited [[False for _ in range(cols)] for _ in range(rows)] # 优先队列元素是(距离, 行, 列) pq [(0.0, start[0], start[1])] while pq: cur_dist, r, c heapq.heappop(pq) # 如果这个格子已经被处理过跳过 if visited[r][c]: continue # 标记当前格子已访问 visited[r][c] True # 如果到达终点直接返回路径 if (r, c) goal: return reconstruct_path(parent, start, goal) # 遍历当前格子的所有邻居 for nr, nc in get_neighbors(grid, r, c, neighbor_type): if visited[nr][nc]: continue # 计算从起点经过当前节点到邻居的距离 if neighbor_type 8: # 斜对角移动的代价是根号2直线移动代价是1 if r ! nr and c ! nc: step_cost math.sqrt(2.0) else: step_cost 1.0 else: step_cost 1.0 new_dist cur_dist step_cost if new_dist dist[nr][nc]: dist[nr][nc] new_dist parent[nr][nc] (r, c) heapq.heappush(pq, (new_dist, nr, nc)) # 队列为空仍没找到终点说明不可达 return None if __name__ __main__: # 地图0可通行1障碍物 grid_map [ [0, 0, 0, 0, 1, 0, 0], [0, 1, 0, 1, 1, 1, 0], [0, 0, 0, 0, 0, 1, 0], [0, 1, 1, 0, 1, 1, 0], [0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 1, 0, 1, 0], ] start (0, 0) goal (4, 6) path dijkstra(grid_map, start, goal, neighbor_type8) if path: print(路径节点:, path) print(路径长度:, len(path) - 1) # 节点数减1就是单位距离数 else: print(没有找到可行路径)4. 在移动机器人上的实操要点4.1 栅格地图与坐标变换代码里用的坐标是栅格坐标但机器人真实运行在物理坐标系中。从激光雷达或视觉传感器拿到环境信息后需要先构建栅格地图然后才能跑Dijkstra。栅格分辨率直接决定路径的精细程度分辨率太高节点数会指数增长计算时间变长分辨率太低路径会穿墙或者不贴边。在实际工程里我习惯先做一次障碍物膨胀处理。因为机器人本身有半径如果路径让机器人贴着障碍物格子走实际物理位置可能撞墙。膨胀的做法是把每个障碍物格子的周围若干格子也标记为障碍物膨胀半径等于机器人半径除以栅格分辨率向上取整。这样Dijkstra搜索出来的路径天然和障碍物保持安全距离。4.2 八邻域与代价设定四邻域和八邻域的选择会影响路径形状。四邻域生成的路径只有水平和垂直段机器人走起来会频繁转向运动路径长八邻域允许斜向移动路径看起来更自然但实际执行时机器人需要精确控制斜向运动。如果你的机器人是差速底盘斜向走没问题如果是类似汽车运动的阿克曼底盘斜向走可能不现实。代价设定方面如果只是静态障碍物用欧氏距离作为边权就够了。但如果地图里有不同区域比如办公区和货架区或者地面上有减速带可以在代价上做加法。举个例子普通格子代价1减速带区域代价3这样Dijkstra虽然仍然找“最短”路径但会把代价高区域“推开”。4.3 动态障碍物与重规划策略Dijkstra的局限在于地图变化后要重新搜索。移动机器人实时运行中经常能发现原地图上没有的动态障碍物比如人或者货车。这时候全局路径可能失效我的做法是不频繁全局重规划而是把全局路径切成若干段机器人先沿着当前段走如果前方局部窗口发现障碍物就调用局部避障算法比如DWA或者TEB如果局部避障也绕不过去再触发全局重规划。全局重规划时也不一定非要跑完整张地图。可以用一个启发式边界如果起点和终点之间的距离很近或者搜索只影响地图的某个局部区域我可以限制Dijkstra的最大搜索次数或者把起点改成机器人当前位置终点改成全局路径上的某个中间点这样重规划速度会快很多。5. 常见问题排查与性能优化5.1 路径贴着障碍物怎么避免跑出路径后如果发现路径贴着障碍物就算视觉上看没穿墙机器人实际走的时候也有可能剐蹭。这个问题通常是膨胀半径不够导致的。检查一下你的膨胀半径是否覆盖了机器人的最大半宽以及有没有把底盘的运动学偏差考虑进去。另外代价函数也可以调整。我在一些项目里会给障碍物附近的格子增加一个“碰撞风险代价”距离障碍物越近风险代价越高。这样Dijkstra规划出的路径会偏向于障碍物较少的区域而不是单纯追求几何最短。5.2 性能瓶颈和优化方向栅格地图规模大的时候Dijkstra的搜索范围会非常大。你可以用两个办法提速第一把启发式函数加进去换成A*但前提是你要保证启发式函数满足一致性否则路径不是最优第二用双向Dijkstra从起点和终点同时开始搜索直到两个方向的搜索边界相交。双向Dijkstra的实现比单向复杂一些但原理并不难维护两个dist数组和两个队列每次选距离较近的一侧向外扩展当某个节点同时被两侧访问时合并两侧的最短路径。很多开源导航库其实内置了双向搜索机制核心目的就是减少搜索面积。5.3 工程化建议与判断标准如果你在嵌入式环境里跑内存和CPU都有限建议先用小地图测试确认Dijkstra的最长耗时是否满足你的导航周期要求。一个常用的判断标准是全局路径规划频率不需要太高一般0.5到1赫兹就够了所以单次规划在几百毫秒内通常可以接受。我以前调试某AGV的时候地图尺寸在500x500左右用单向Dijkstra跑一次花了接近300毫秒换成A之后降到80毫秒。但那次是因为我有充分的时间去调启发式。如果你现在还在做原型验证先老老实实把Dijkstra跑通后面再优化也不迟。代码里保留的dist数组和parent数组后续切换A时也可以复用大部分结构。我个人在实际操作中的体会是Dijkstra就像走路永远脚踏实地的人虽然不会抄捷径但每一步都走得踏实。在路径规划入门阶段手写一遍带注释的Dijkstra远比直接用别人封装好的库更能帮你理解图搜索的本质。后面不管换A还是D你都会发现那些节点的松弛操作、优先队列的更新逻辑其实就是这些算法共用的骨架。最后再分享一个小技巧把地图分辨率暂时调粗一倍先用Dijkstra跑通整体流程再切回细分辨率调优参数调试效率会高很多。