4. 全局路径规划：Dijkstra算法原理与实现
说到全局路径规划，Dijkstra算法是个绕不开的基础。我刚开始做机器人导航那会儿，第一个接触的规划算法就是它。说实话，这算法看着简单，但真正用好它，还是有不少门道的。
4.1 算法核心思想
Dijkstra算法要解决什么问题？说白了就是：在带权图中，找到从起点到所有其他节点的最短路径。它是个贪心算法，每次选当前代价最小的节点去扩展。
我记得第一次在项目里用它，是在一个仓储机器人上。地图有几十个节点，机器人要从A点去B点取货。当时我直接用Dijkstra跑了一遍，路径是出来了，但总觉得哪里不对劲。后来才发现——我忘了考虑路径的平滑性。嗯，这是后话了。
算法的核心逻辑其实就三步：
- 初始化：起点代价为0，其他节点代价为无穷大
- 选择：从未处理的节点中，选代价最小的那个
- 松弛：更新该节点所有邻居的代价
重复2和3，直到所有节点都被处理完。
关键点：Dijkstra要求图中所有边的权值必须为非负数。如果有负权边，算法会失效。我曾经踩过这个坑——地图里有个"下坡路段"设成了负权值，结果路径全乱了。
4.2 算法流程详解
我们用一个具体例子来走一遍流程。假设有这样一个图：
我们一步步来：
| 步骤 | 当前节点 | 已处理集合 | 距离记录 |
|---|---|---|---|
| 0 | A | {A} | A:0, B:∞, C:∞, D:∞, E:∞ |
| 1 | A→C | {A, C} | A:0, B:4, C:2, D:∞, E:∞ |
| 2 | C→B | {A, C, B} | A:0, B:3, C:2, D:3, E:∞ |
| 3 | B→D | {A, C, B, D} | A:0, B:3, C:2, D:3, E:5 |
| 4 | D→E | {A, C, B, D, E} | A:0, B:3, C:2, D:3, E:5 |
你看，最终从A到E的最短路径是A→C→D→E，总代价5。而不是A→B→D→E（代价7）。
4.3 代码实现
我习惯用Python来实现Dijkstra。为什么？因为项目里做原型验证快，而且代码可读性好。下面是我在项目中常用的实现：
import heapq
def dijkstra(graph, start):
"""
graph: 邻接表表示的图，格式 {node: [(neighbor, cost), ...]}
start: 起点
返回: (dist, prev) 距离字典和前驱节点字典
"""
# 初始化
dist = {node: float('inf') for node in graph}
dist[start] = 0
prev = {node: None for node in graph}
# 优先队列，存 (距离, 节点)
pq = [(0, start)]
visited = set()
while pq:
current_dist, current = heapq.heappop(pq)
# 如果当前节点已处理，跳过
if current in visited:
continue
visited.add(current)
# 遍历邻居
for neighbor, cost in graph[current]:
if neighbor in visited:
continue
new_dist = current_dist + cost
# 松弛操作
if new_dist < dist[neighbor]:
dist[neighbor] = new_dist
prev[neighbor] = current
heapq.heappush(pq, (new_dist, neighbor))
return dist, prev
def reconstruct_path(prev, start, goal):
"""重建从start到goal的路径"""
path = []
current = goal
while current is not None:
path.append(current)
current = prev[current]
path.reverse()
return path if path[0] == start else []
# 使用示例
graph = {
'A': [('B', 4), ('C', 2)],
'B': [('A', 4), ('C', 3), ('D', 5)],
'C': [('A', 2), ('B', 3), ('D', 1)],
'D': [('B', 5), ('C', 1), ('E', 2)],
'E': [('D', 2)]
}
dist, prev = dijkstra(graph, 'A')
path = reconstruct_path(prev, 'A', 'E')
print(f"最短路径: {' → '.join(path)}") # A → C → D → E
print(f"总代价: {dist['E']}") # 5我的经验：用heapq实现优先队列是关键。如果自己写排序，时间复杂度会从O((V+E)logV)退化到O(V²)。在节点数超过1000的地图上，差距非常明显。
4.4 算法复杂度分析
你可能会问：Dijkstra到底快不快？这得看实现方式。
| 实现方式 | 时间复杂度 | 适用场景 |
|---|---|---|
| 朴素数组 | O(V²) | 稠密图（边多） |
| 二叉堆（heapq） | O((V+E)logV) | 稀疏图（边少），最常用 |
| 斐波那契堆 | O(VlogV+E) | 理论最优，但实现复杂 |
我在实际项目中，90%的情况都用二叉堆实现。为什么？因为机器人地图通常是稀疏图——每个节点只连接周围几个邻居，边数远小于V²。二叉堆已经够用了。
4.5 避坑指南
我曾经踩过的坑：
- 负权边：有一次地图里有个"捷径"设成了负值，Dijkstra直接死循环了。记住，有负权边要用Bellman-Ford。
- 大图内存爆炸：10000个节点的全连接图，邻接矩阵要存1亿个元素。我后来改用邻接表，内存从800MB降到了20MB。
- 路径不唯一：当多条路径代价相同时，Dijkstra只返回其中一条。如果你需要所有最短路径，得改算法。
4.6 在机器人规划中的实际应用
Dijkstra在机器人领域最常见的用法是：在拓扑地图上做全局路径规划。比如：
- 仓储机器人：货架间的路径规划，节点是货架位置，边是通道距离
- 扫地机器人：房间到房间的导航，节点是房间中心，边是门之间的距离
- 自动驾驶：道路网络中的路径规划，节点是路口，边是路段长度
我个人习惯是：先用Dijkstra算出一条全局路径，再用局部规划器（比如DWA）做平滑和避障。这样既保证了全局最优，又兼顾了局部灵活性。
一个实用技巧：如果地图很大（比如几万个节点），可以用双向Dijkstra——从起点和终点同时搜索，相遇时停止。速度能快2-4倍。我在一个物流项目里用过，搜索时间从3秒降到了0.8秒。
4.7 小结
Dijkstra算法是全局路径规划的基石。它简单、可靠、保证最优。但你要记住它的限制：不能有负权边，在大图上要考虑性能优化。
嗯，掌握了Dijkstra，后面学A*算法就会轻松很多——A*其实就是Dijkstra加了个启发式函数。下节课我们会看到，加了"方向感"之后，搜索效率能提升多少。