4. Dijkstra算法实现：数据结构定义、核心逻辑代码、单步调试与可视化
好，咱们进入正题。Dijkstra算法，说白了就是“贪心+松弛”的经典组合。我在做自动驾驶路径规划时，经常用它来做全局路径的初筛。别看它简单，但坑不少。今天我就带你一步步把代码写出来，再亲手调试一遍。
4.1 数据结构定义
先定义几个核心结构。我个人习惯用Python的类来组织，清晰又好调试。
class Graph:
def __init__(self):
self.nodes = {} # 节点字典，key:节点名, value:节点对象
self.edges = [] # 边列表，每个元素是 (起点, 终点, 权重)
class Node:
def __init__(self, name):
self.name = name
self.neighbors = [] # 邻居列表，每个元素是 (邻居节点, 边权重)
self.distance = float('inf') # 从起点到当前节点的最短距离
self.previous = None # 前驱节点，用于回溯路径
self.visited = False # 是否已访问嗯，这里要注意：distance初始化为无穷大，这是Dijkstra算法的标准做法。我刚开始学的时候，总想着给个很大的数就行，结果有一次在复杂路网中跑出了负数距离——因为溢出。从那以后，我就老老实实用float('inf')了。
小技巧：如果你用C++实现，建议用
std::numeric_limits<double>::max()，别用INT_MAX，否则加法会溢出。
4.2 核心逻辑代码
核心逻辑其实就三步：选最小、更新邻居、标记已访问。我直接上代码，你感受一下。
def dijkstra(graph, start_name):
# 初始化起点
start_node = graph.nodes[start_name]
start_node.distance = 0
# 优先队列（最小堆）
import heapq
pq = [(0, start_name)]
while pq:
current_dist, current_name = heapq.heappop(pq)
current_node = graph.nodes[current_name]
# 如果当前节点已访问，跳过（防止重复处理）
if current_node.visited:
continue
current_node.visited = True
# 遍历所有邻居
for neighbor_node, weight in current_node.neighbors:
if not neighbor_node.visited:
new_dist = current_dist + weight
if new_dist < neighbor_node.distance:
neighbor_node.distance = new_dist
neighbor_node.previous = current_node
heapq.heappush(pq, (new_dist, neighbor_node.name))
# 回溯路径
def get_path(end_name):
path = []
node = graph.nodes[end_name]
while node:
path.append(node.name)
node = node.previous
return list(reversed(path))
return get_path你想想看，为什么这里要用优先队列？说白了，就是为了每次都能快速拿到当前距离最小的节点。如果不用堆，每次遍历所有节点找最小，复杂度就是O(n²)，在自动驾驶的实时场景下根本扛不住。
避坑指南：我曾经在项目中遇到一个bug——忘记检查
current_node.visited，结果同一个节点被多次弹出处理，导致距离被错误地更新。嗯，这个坑我踩过，你别再踩了。
4.3 单步调试与可视化
光看代码不过瘾，咱们来手动跑一遍。假设有这样一个图：
| 边 | 权重 |
|---|---|
| A → B | 4 |
| A → C | 2 |
| B → C | 1 |
| B → D | 5 |
| C → D | 8 |
| C → E | 10 |
| D → E | 2 |
起点是A。咱们一步步看：
- 初始化：A距离=0，其他都是∞。优先队列：[(0, A)]
- 弹出A：更新B(4)、C(2)。队列：[(2, C), (4, B)]
- 弹出C：更新D(2+8=10)、E(2+10=12)。队列：[(4, B), (10, D), (12, E)]
- 弹出B：更新C？C已访问，跳过。更新D(4+5=9)，比10小，更新。队列：[(9, D), (10, D), (12, E)]
- 弹出D(9)：更新E(9+2=11)，比12小，更新。队列：[(11, E), (12, E)]
- 弹出E(11)：结束。
最终路径：A→B→D→E，总距离11。
关键观察：注意第4步，D的距离从10更新到了9。这就是Dijkstra的“松弛”过程——不断用更短的路径替换当前路径。我在实际项目中，经常用这个特性来动态调整路径，比如遇到临时拥堵时重新计算。
可视化方面，我建议用matplotlib简单画一下。核心代码就几行：
import matplotlib.pyplot as plt
import networkx as nx
def visualize_graph(graph, path=None):
G = nx.Graph()
for node_name, node in graph.nodes.items():
G.add_node(node_name)
for neighbor, weight in node.neighbors:
G.add_edge(node_name, neighbor.name, weight=weight)
pos = nx.spring_layout(G)
nx.draw(G, pos, with_labels=True, node_color='lightblue')
labels = nx.get_edge_attributes(G, 'weight')
nx.draw_networkx_edge_labels(G, pos, edge_labels=labels)
if path:
edges_in_path = [(path[i], path[i+1]) for i in range(len(path)-1)]
nx.draw_networkx_edges(G, pos, edgelist=edges_in_path,
edge_color='red', width=2)
plt.show()嗯，这里有个小细节：spring_layout每次运行位置可能不一样，如果你要对比不同算法的路径，建议固定随机种子或者手动指定坐标。
调试建议：在while循环里加个
print(f"当前节点: {current_name}, 距离: {current_dist}")，你就能看到每一步的决策过程。我当年调试自动驾驶的路径规划模块时，就是靠这个打印语句找到了一个隐藏的bug——有个节点的邻居列表居然包含了自己，导致死循环。
好了，Dijkstra的实现就到这里。下一章咱们会对比A*算法，看看它怎么用启发式信息来加速搜索。说实话，A*在自动驾驶中更常用，但Dijkstra是基础，基础不牢，地动山摇。