4. 路径规划基础算法：Dijkstra算法

聊到全局路径规划，Dijkstra算法是个绕不开的起点。我个人习惯把它称作「最朴素的寻路算法」——它不玩花活，但绝对可靠。当年我在做园区低速无人车项目时，第一次在实车上跑通Dijkstra，看着小车沿着最短路径稳稳开到目标点，那种感觉还挺奇妙的。

4.1 算法原理：说白了就是「贪心+松弛」

Dijkstra算法的核心思想，用一句话就能讲清楚：从起点开始，每次选一个离起点最近且没处理过的节点，然后看看能不能通过它让别的节点离起点更近。

嗯，这里要注意——它要求图中所有边的权重不能为负数。为什么？因为一旦有负权边，你之前认为「最近」的节点可能就不是最近的了，整个贪心逻辑就崩了。我在实际项目中遇到过有人拿Dijkstra去处理带负权的地图，结果路径完全不对，排查了半天才发现是这个问题。

算法维护两个关键数据结构：

• dist[]：记录从起点到每个节点的当前最短距离
• visited[]：标记节点是否已经确定了最短路径

你想想看，这个过程像不像我们平时找人问路？你站在起点，先问旁边的人「到XX怎么走」，记下距离。然后找到最近的那个路口，再问那里的人「到XX怎么走」……一步步扩散出去，直到找到终点。

4.2 算法流程：一步步拆解

我习惯把Dijkstra的流程分成五步，这样写代码时思路特别清晰：

1. 初始化：起点的dist设为0，其他节点设为无穷大。visited全部设为false。
2. 找最小：从所有未访问的节点中，选出dist值最小的那个，标记为当前节点u。
3. 标记访问：将u的visited设为true。这意味着u的最短路径已经确定了。
4. 松弛操作：遍历u的所有邻居v，如果通过u到v的距离比当前dist[v]更小，就更新dist[v]。
5. 重复：回到第2步，直到所有节点都被访问过，或者找到了目标节点。

代码实现其实很直观，下面是我常用的一个版本：

def dijkstra(graph, start):
    n = len(graph)
    dist = [float('inf')] * n
    visited = [False] * n
    dist[start] = 0
    
    for _ in range(n):
        # 找未访问节点中dist最小的
        u = -1
        min_dist = float('inf')
        for i in range(n):
            if not visited[i] and dist[i] < min_dist:
                min_dist = dist[i]
                u = i
        
        if u == -1:  # 剩下的节点都不可达
            break
        
        visited[u] = True
        
        # 松弛操作
        for v in range(n):
            if graph[u][v] > 0 and not visited[v]:
                new_dist = dist[u] + graph[u][v]
                if new_dist < dist[v]:
                    dist[v] = new_dist
    
    return dist

4.3 复杂度分析

复杂度这块，咱们分两种情况来看：

V是节点数，E是边数。为什么朴素版是O(V²)？因为每次找最小dist节点要遍历所有V个节点，一共要执行V次，所以是V×V。堆优化版每次取最小是O(logV)，每条边最多松弛一次，所以总复杂度是O((V+E)logV)。

说实话，在自动驾驶的全局路径规划场景里，路网节点数通常不会太大（几百到几千个），朴素版其实够用。但如果你在做高精地图的路径规划，节点数可能上万，那就必须上堆优化了。

4.4 优缺点分析

优点：

• 保证最优解：只要图里没有负权边，Dijkstra一定能找到最短路径。这一点在自动驾驶里特别重要——你不能给用户一条「差不多最短」的路线。
• 实现简单：逻辑清晰，代码量少，调试起来也方便。我记得第一次在ROS里集成Dijkstra，从写代码到跑通只用了半天。
• 稳定性好：不会出现震荡或者不收敛的情况，每次跑出来的结果都一样。

缺点：

• 不能处理负权边：这是硬伤。如果地图里有负权边（比如某些下坡路段被赋予了负的代价），Dijkstra会给出错误结果。这时候得用Bellman-Ford算法。
• 搜索范围大：Dijkstra是「地毯式」搜索，会向所有方向均匀扩散。如果起点和终点离得很远，中间很多节点其实没必要探索。我曾经在一条20公里的高速路段上跑Dijkstra，结果它把高速两侧5公里范围内的所有小路都搜了一遍……
• 实时性一般：对于动态变化的场景（比如实时交通流），Dijkstra每次都要重新计算，效率不高。

4.5 在自动驾驶中的应用场景

Dijkstra在自动驾驶里最常见的用途是全局路径规划。具体来说：

• 导航路线计算：给定起点和终点，在路网中找到最短路径。这是最基础的应用。
• 多目标点规划：比如无人配送车需要依次访问多个站点，可以用Dijkstra计算每两个站点之间的最短路径。
• 代价地图路径规划：把地图栅格化，每个格子赋予一个通行代价（比如障碍物代价高、道路代价低），然后用Dijkstra找代价最小的路径。

不过说实话，在实际的自动驾驶系统中，纯Dijkstra用得并不多。大家更常用的是A*算法——它本质上是在Dijkstra的基础上加了一个启发式函数，让搜索更有方向性。但Dijkstra作为基础，理解透了再看A*就轻松多了。

好了，Dijkstra就聊到这儿。下一节咱们会讲A*算法，看看怎么在Dijkstra的基础上加个「指南针」，让搜索效率翻倍。