3. 全局路径规划之Dijkstra算法：算法原理、伪代码、Python实现、优缺点分析
3.1 算法原理：从点A到点B的最短路径怎么找？
Dijkstra算法，说白了就是一张地图上，从起点到所有其他点的最短路径怎么算。它由荷兰计算机科学家Edsger Dijkstra在1956年提出，到现在快70年了，依然是路径规划的基石。
我个人习惯把它理解成「水波扩散」的过程。你往平静的水面扔一颗石子，波纹会一圈一圈往外扩散。Dijkstra算法也是这样——从起点开始，一层一层往外探索，每次只走当前已知最短的那条路。
核心思想其实就一句话：每次从未访问的节点中，选一个距离起点最近的节点，然后更新它邻居的距离。嗯，就这么简单。
我当年在做一个仓储机器人项目时，第一次亲手实现这个算法。当时觉得不就是个BFS加权重嘛，结果调试了一整天——因为没处理好负权边的问题。这个坑后面我会细说。
3.2 算法伪代码：清晰到可以直接抄
先看伪代码，我习惯用这种形式来理清思路。你想想看，写代码前把逻辑捋顺了，后面能少踩多少坑。
function Dijkstra(Graph, source):
// 初始化
dist[source] = 0
for each vertex v in Graph:
if v != source:
dist[v] = INFINITY
prev[v] = UNDEFINED
add v to unvisited_set
// 主循环
while unvisited_set is not empty:
u = vertex with smallest dist in unvisited_set
remove u from unvisited_set
for each neighbor v of u:
alt = dist[u] + weight(u, v)
if alt < dist[v]:
dist[v] = alt
prev[v] = u
return dist[], prev[]这里有个细节要注意：prev[]数组是用来回溯路径的。从终点开始，沿着prev一直往前找，就能得到完整的路径。我在项目中经常用这个技巧来调试路径规划的结果。
3.3 Python实现：能跑起来的代码才是好代码
下面是我写的一个简洁版本。为了让你看得更清楚，我特意没用任何第三方库，纯Python实现。
import heapq
def dijkstra(graph, start):
"""
graph: 邻接表表示的图，格式 {node: [(neighbor, weight), ...]}
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, u = heapq.heappop(pq)
# 如果已经访问过，跳过
if u in visited:
continue
visited.add(u)
# 遍历邻居
for v, weight in graph[u]:
if v in visited:
continue
alt = current_dist + weight
if alt < dist[v]:
dist[v] = alt
prev[v] = u
heapq.heappush(pq, (alt, v))
return dist, prev
def reconstruct_path(prev, start, end):
"""回溯路径"""
path = []
current = end
while current is not None:
path.append(current)
current = prev[current]
path.reverse()
return path if path[0] == start else []
# 使用示例
if __name__ == "__main__":
# 一个简单的图
graph = {
'A': [('B', 4), ('C', 2)],
'B': [('A', 4), ('C', 1), ('D', 5)],
'C': [('A', 2), ('B', 1), ('D', 8), ('E', 10)],
'D': [('B', 5), ('C', 8), ('E', 2), ('F', 6)],
'E': [('C', 10), ('D', 2), ('F', 3)],
'F': [('D', 6), ('E', 3)]
}
dist, prev = dijkstra(graph, 'A')
path = reconstruct_path(prev, 'A', 'F')
print(f"最短路径: {path}")
print(f"最短距离: {dist['F']}")
我的小技巧：用
heapq实现优先队列，比每次手动找最小距离快得多。在节点数超过1000时，这个优化能让运行时间从秒级降到毫秒级。
3.4 优缺点分析：没有银弹
任何算法都有它的适用场景。Dijkstra算法虽然经典，但也不是万能的。我把它优缺点列出来，你心里有个数。
| 优点 | 缺点 |
|---|---|
| 保证找到最短路径（正权图） | 不能处理负权边 |
| 算法稳定，结果可预测 | 时间复杂度较高 O((V+E)logV) |
| 实现简单，容易调试 | 在大规模图上效率低 |
| 适用于静态环境 | 动态环境需要重新计算 |
| 可以同时得到到所有点的最短路径 | 如果只需要到某个点，仍然要遍历全图 |
避坑指南：我曾经在一个AGV调度项目里，直接用Dijkstra做全局路径规划。地图有5000多个节点，每次规划要跑200多毫秒。后来换成A*算法，同样的地图只需要20毫秒。所以，如果你只需要找两个点之间的最短路径，别用Dijkstra，用A*。
3.5 实际应用中的注意事项
说几个我在实战中踩过的坑，希望能帮你省点时间。
- 图太大怎么办？ 可以用双向Dijkstra——从起点和终点同时开始搜索，能减少一半的搜索空间。
- 地图是网格怎么办？ 把每个格子当成一个节点，上下左右相邻的格子之间有权重。我习惯用曼哈顿距离或者欧几里得距离作为权重。
- 路径不平滑怎么办？ Dijkstra出来的路径是折线，直接给机器人用会一顿一顿的。我一般会在后面加一个路径平滑处理，比如用B样条曲线。
- 内存不够怎么办？ 如果图特别大，可以考虑用邻接表而不是邻接矩阵。邻接矩阵是O(V²)的空间复杂度，邻接表是O(V+E)。
一句话总结：Dijkstra算法是路径规划的「基本功」，就像练武要扎马步一样。虽然实际项目中很少直接用它，但理解它的思想，对你理解A*、D*、RRT等更高级的算法非常有帮助。
好了，这一章就到这里。下一章我们会讲A*算法——说白了就是在Dijkstra的基础上加了一个启发式函数，让搜索更有方向感。到时候我会对比着讲，你就能看出两者的区别了。