第4章全局路径规划算法（上）

4、全局路径规划算法（上）：Dijkstra算法原理、Python实现与可视化

各位同学，欢迎来到全局路径规划的第一讲。

今天咱们聊一个非常经典的算法——Dijkstra。说实话，我在工业AGV项目里，最开始接触的路径规划算法就是它。那时候刚入行，觉得这算法太简单了，不就是个最短路径吗？后来在实际产线上跑起来才发现，里面的门道还真不少。

好，咱们直接进入正题。

4.1 算法核心思想：贪心 + 松弛

Dijkstra算法要解决什么问题？说白了，就是在一个带权重的图里，找到从起点到所有其他节点的最短路径。

它的核心思想就两个词：贪心和松弛。

贪心：每次从「未确定最短路径」的节点中，选一个距离起点最近的节点，把它标记为已确定。
松弛：选完节点后，看看能不能通过这个节点，让它的邻居节点距离起点更近。如果能，就更新邻居的距离。

嗯，这里要注意：Dijkstra要求图中不能有负权边。为什么？因为一旦有负权边，贪心策略就失效了。我当年在调试一个仓储AGV时，就因为地图里有个负权重的「捷径」逻辑，导致算法一直跑不出正确路径。后来排查了半天才发现，是建模时把某个下坡路段的代价设成了负数。从那以后，我对负权边就特别敏感。

适用场景：

地图中所有边的权重都是非负数（比如距离、时间、能耗）
需要从单一起点到所有节点的最短路径
节点数量在几千以内（再大就需要考虑A*或分层规划了）

4.2 算法步骤详解

咱们用一个简单的例子来走一遍流程。假设有5个节点，起点是A。

步骤	当前节点	已确定集合	距离记录（A到各点）
初始化	-	{A}	A:0, B:∞, C:∞, D:∞, E:∞
1	A	{A}	A:0, B:2, C:5, D:∞, E:∞
2	B	{A, B}	A:0, B:2, C:4, D:7, E:∞
3	C	{A, B, C}	A:0, B:2, C:4, D:6, E:9
4	D	{A, B, C, D}	A:0, B:2, C:4, D:6, E:8
5	E	{A, B, C, D, E}	全部确定

你想想看，这个过程像不像我们平时用导航软件？每次到一个路口，就看看有没有更近的路。只不过Dijkstra把这个过程数学化了。

4.3 Python实现：从零手写

我个人习惯，学算法一定要手写一遍。光看别人代码，永远学不会。下面是我在项目中常用的一个实现版本，用到了优先队列（heapq）来优化性能。

import heapq
import math

def dijkstra(graph, start):
    """
    graph: 邻接表表示的图，格式 {node: [(neighbor, weight), ...]}
    start: 起点节点
    返回: (distances, predecessors)
    """
    # 初始化距离字典，起点为0，其他为无穷大
    distances = {node: math.inf for node in graph}
    distances[start] = 0
    
    # 前驱节点字典，用于回溯路径
    predecessors = {node: None for node in graph}
    
    # 优先队列，存储 (距离, 节点)
    pq = [(0, start)]
    
    # 已确定最短路径的节点集合
    visited = set()
    
    while pq:
        current_dist, current_node = heapq.heappop(pq)
        
        # 如果当前节点已经处理过，跳过
        if current_node in visited:
            continue
        
        # 标记为已确定
        visited.add(current_node)
        
        # 遍历邻居节点
        for neighbor, weight in graph[current_node]:
            if neighbor in visited:
                continue
            
            # 计算新的距离
            new_dist = current_dist + weight
            
            # 如果新距离更短，更新
            if new_dist < distances[neighbor]:
                distances[neighbor] = new_dist
                predecessors[neighbor] = current_node
                heapq.heappush(pq, (new_dist, neighbor))
    
    return distances, predecessors

def reconstruct_path(predecessors, start, goal):
    """回溯路径"""
    path = []
    current = goal
    while current is not None:
        path.append(current)
        current = predecessors[current]
    path.reverse()
    
    # 如果起点不在路径中，说明不可达
    if path[0] != start:
        return []
    return path

避坑指南：

我曾经在项目里犯过一个低级错误——忘记检查节点是否已访问。结果在环形地图里，算法陷入了死循环。后来我养成了一个习惯：每次从优先队列取出节点后，先判断它是否在visited集合里。这个判断虽然简单，但能避免很多诡异的问题。

4.4 可视化：让算法「看得见」

光看代码和表格，你可能觉得Dijkstra很抽象。我建议你把它可视化出来。下面是一个简单的可视化实现，用matplotlib和networkx库。

import matplotlib.pyplot as plt
import networkx as nx

def visualize_dijkstra(graph, start, goal):
    """可视化Dijkstra算法的搜索过程"""
    # 创建图对象
    G = nx.Graph()
    for node, neighbors in graph.items():
        for neighbor, weight in neighbors:
            G.add_edge(node, neighbor, weight=weight)
    
    # 计算最短路径
    distances, predecessors = dijkstra(graph, start)
    path = reconstruct_path(predecessors, start, goal)
    
    # 布局
    pos = nx.spring_layout(G, seed=42)
    
    # 绘制所有节点和边
    plt.figure(figsize=(10, 8))
    nx.draw_networkx_nodes(G, pos, node_color='lightblue', 
                          node_size=500)
    nx.draw_networkx_edges(G, pos, edge_color='gray', width=1)
    nx.draw_networkx_labels(G, pos, font_size=12)
    
    # 高亮最短路径
    if path:
        path_edges = list(zip(path[:-1], path[1:]))
        nx.draw_networkx_nodes(G, pos, nodelist=path, 
                              node_color='lightgreen', node_size=500)
        nx.draw_networkx_edges(G, pos, edgelist=path_edges, 
                              edge_color='red', width=3)
    
    # 标记起点和终点
    nx.draw_networkx_nodes(G, pos, nodelist=[start], 
                          node_color='green', node_size=600)
    nx.draw_networkx_nodes(G, pos, nodelist=[goal], 
                          node_color='red', node_size=600)
    
    # 显示边权重
    edge_labels = nx.get_edge_attributes(G, 'weight')
    nx.draw_networkx_edge_labels(G, pos, edge_labels=edge_labels)
    
    plt.title(f"Dijkstra: {start} → {goal}")
    plt.axis('off')
    plt.show()

# 测试用例
if __name__ == "__main__":
    # 一个简单的图
    graph = {
        'A': [('B', 2), ('C', 5)],
        'B': [('A', 2), ('C', 2), ('D', 5)],
        'C': [('A', 5), ('B', 2), ('D', 2), ('E', 5)],
        'D': [('B', 5), ('C', 2), ('E', 2)],
        'E': [('C', 5), ('D', 2)]
    }
    
    visualize_dijkstra(graph, 'A', 'E')

可视化能帮你看到什么？

绿色节点：已确定最短路径的节点
红色边：最终的最短路径
灰色边：算法探索过但未选中的路径

你会发现，Dijkstra的搜索范围像一个不断扩大的圆。从起点开始，一圈一圈往外扩散。这就是为什么它叫「广度优先搜索的加权版本」。

4.5 工业场景中的实际考量

讲完了原理和代码，咱们聊聊实际应用。在工业AGV里，Dijkstra不是直接拿来用的。为什么？

地图太大：一个中型仓库可能有上千个节点，Dijkstra要遍历所有节点，效率太低。我一般用它做局部规划，或者作为其他算法的基准测试。
动态障碍物：Dijkstra假设地图是静态的。但AGV运行中，可能有其他车辆、人员突然出现。这时候需要结合局部规划（如DWA）来避障。
多AGV协同：如果多台AGV同时运行，Dijkstra算出的路径可能冲突。需要引入交通管制或路径预留机制。

注意：Dijkstra算出的路径是「最短」的，但不一定是「最优」的。比如，它不考虑转弯次数、道路宽度、AGV的转弯半径。在实际项目中，我通常会在Dijkstra的基础上，对路径做平滑处理，或者加入转弯代价。

4.6 小结与思考

好了，这一讲的内容就到这里。咱们总结一下：

Dijkstra的核心是贪心+松弛，每次选最近节点，更新邻居距离
用优先队列实现，时间复杂度O((V+E)logV)
可视化能帮你直观理解算法的搜索过程
工业应用中需要结合实际情况做优化

下一讲，咱们会聊A*算法。它比Dijkstra多了个启发式函数，搜索效率高得多。但Dijkstra是基础，基础不牢，地动山摇。建议你把今天的代码跑一遍，改改参数，看看效果。

有问题随时交流。咱们下节课见。