3、全局路径规划基础：Dijkstra与A*算法

全局路径规划，说白了就是在地图上找一条从起点到终点的路。我刚开始做自动驾驶时，觉得这不就是导航软件干的事吗？后来才发现，车规级的路径规划和手机导航完全是两码事——你得考虑车辆的运动约束、道路的拓扑结构，还有实时性要求。

这一章，我们重点啃两个经典算法：Dijkstra和A*。它们虽然年代久远，但直到今天，很多商业级规划系统里依然能看到它们的影子。

3.1 Dijkstra算法：最朴素的“地毯式搜索”

Dijkstra算法的思路特别简单：从起点开始，一层一层往外扩，直到找到终点。它保证能找到最短路径，但代价是——慢。

算法步骤

1. 初始化：起点距离为0，其他节点距离为无穷大。
2. 从优先队列中取出距离最小的节点u。
3. 遍历u的所有邻居v，计算新距离 = dist[u] + cost(u, v)。
4. 如果新距离小于dist[v]，更新dist[v]并记录前驱节点。
5. 重复步骤2-4，直到队列为空或终点被取出。

代码实现（Python）

import heapq

def dijkstra(graph, start, goal):
    # graph: 邻接表 {node: [(neighbor, cost), ...]}
    pq = [(0, start)]
    dist = {node: float('inf') for node in graph}
    dist[start] = 0
    prev = {start: None}
    
    while pq:
        current_dist, u = heapq.heappop(pq)
        if u == goal:
            break
        if current_dist > dist[u]:
            continue
        for v, cost in graph[u]:
            new_dist = dist[u] + cost
            if new_dist < dist[v]:
                dist[v] = new_dist
                prev[v] = u
                heapq.heappush(pq, (new_dist, v))
    
    # 回溯路径
    path = []
    node = goal
    while node is not None:
        path.append(node)
        node = prev.get(node)
    return path[::-1]

3.2 A*算法：带“方向感”的搜索

A*算法在Dijkstra的基础上加了一个启发式函数，说白了就是给搜索过程装了个“指南针”。它优先探索那些看起来更有可能通向终点的方向。

为什么会这样？因为Dijkstra是“无差别”扩散，而A*知道终点的大致方位，可以少走很多弯路。

核心公式

f(n) = g(n) + h(n)

• g(n)：从起点到当前节点n的实际代价。
• h(n)：从节点n到终点的估计代价（启发式函数）。
• f(n)：总估计代价，优先队列按f值排序。

代码实现

import heapq
import math

def heuristic(a, b):
    # 欧几里得距离
    return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

def a_star(graph, start, goal, positions):
    # positions: 节点坐标字典 {node: (x, y)}
    pq = [(0, start)]
    g = {node: float('inf') for node in graph}
    g[start] = 0
    prev = {start: None}
    
    while pq:
        _, u = heapq.heappop(pq)
        if u == goal:
            break
        for v, cost in graph[u]:
            new_g = g[u] + cost
            if new_g < g[v]:
                g[v] = new_g
                f = new_g + heuristic(positions[v], positions[goal])
                heapq.heappush(pq, (f, v))
                prev[v] = u
    
    # 回溯路径
    path = []
    node = goal
    while node is not None:
        path.append(node)
        node = prev.get(node)
    return path[::-1]

3.3 启发式函数的设计与调优

启发式函数是A*的灵魂。选对了，搜索效率翻倍；选错了，可能比Dijkstra还慢。

常用启发式函数

函数	公式	适用场景	特点
欧几里得距离	√((x₁-x₂)² + (y₁-y₂)²)	自由移动（无网格约束）	可采纳，但搜索范围较大
曼哈顿距离	|x₁-x₂| + |y₁-y₂|	四方向网格	可采纳，速度快
对角线距离	max(|dx|, |dy|)	八方向网格	可采纳，适合斜向移动
加权欧几里得	ε × 欧几里得距离	需要更快搜索	不可采纳，但速度快

调优技巧

• 权重因子ε：当ε > 1时，搜索更快但可能非最优。我习惯在实时性要求高的场景用ε=1.2，路径质量下降不多，但搜索节点减少30%以上。
• 动态启发式：根据地图密度调整h(n)的权重。比如在开阔区域用大权重，在狭窄区域用小权重。
• 预处理：对于重复使用的场景，可以离线计算所有节点对之间的最短距离作为启发式——这叫“地标启发式”，效果极好。

3.4 Dijkstra vs A*：怎么选？

对比维度	Dijkstra	A*
最优性	保证全局最优	可采纳启发式下保证最优
搜索效率	低（无方向性）	高（有方向性）
适用场景	小地图、离线预处理	大地图、实时规划
内存占用	较高（需维护所有节点）	较低（只探索部分节点）
实现复杂度	简单	中等（需设计启发式）

我个人习惯：如果地图节点数少于1000，直接用Dijkstra，省心。如果地图很大（比如城市级路网），必须上A*，而且得花时间调启发式函数。

3.5 算法流程对比图

下面这张图展示了Dijkstra和A*在相同地图上的搜索过程差异。红色节点是已探索区域，绿色是最终路径。

3.6 实战建议

• 小地图（< 500节点）：直接用Dijkstra，代码简单，不易出错。
• 中等地图（500-5000节点）：用A* + 欧几里得距离，注意检查启发式的可采纳性。
• 大地图（> 5000节点）：A* + 分层规划。先在高精度地图上粗搜，再在局部细搜。
• 动态障碍物：考虑D* Lite或RRT*，但那是后面章节的内容了。