3. 全局路径规划：A*算法原理与实现、Dijkstra算法对比、基于车道图的全局规划

各位同学，今天我们来聊聊全局路径规划。说实话，这是自动驾驶里最「接地气」的部分之一。你想想看，车要往哪开？怎么开？这就是全局规划要回答的问题。

我个人习惯把全局规划比作「看地图找路」。你从北京到上海，高德会给你几条路线。自动驾驶也一样，只不过它看的不是普通地图，而是带有车道信息的高精地图。

3.1 从Dijkstra到A*：为什么我们需要启发式搜索？

先说说Dijkstra算法。这玩意儿是1959年提出的，经典得不能再经典。它的核心逻辑很简单：从起点开始，一层层往外扩散，直到找到终点。

我在项目中遇到过一个问题：用Dijkstra做全局规划，地图稍微大一点，计算时间就蹭蹭往上涨。为什么？因为它像个无头苍蝇，四面八方都搜一遍。明明终点在东边，它还要往西边搜半天。

来看个简单的代码示例：

// Dijkstra伪代码
function Dijkstra(graph, start, goal):
    openSet = {start}
    gScore[start] = 0
    
    while openSet is not empty:
        current = node in openSet with lowest gScore
        if current == goal:
            return reconstructPath(cameFrom, current)
        
        openSet.remove(current)
        for neighbor in graph.neighbors(current):
            tentative_gScore = gScore[current] + dist(current, neighbor)
            if tentative_gScore < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_gScore
                if neighbor not in openSet:
                    openSet.add(neighbor)
    return failure

你看，它只用了g(n)——从起点到当前点的实际代价。没有方向感。

那A*算法呢？它加了一个启发式函数h(n)，用来估算当前点到终点的距离。说白了，就是给搜索过程装了个「指南针」。

// A*伪代码
function AStar(graph, start, goal):
    openSet = {start}
    gScore[start] = 0
    fScore[start] = heuristic(start, goal)  // 关键区别在这里
    
    while openSet is not empty:
        current = node in openSet with lowest fScore
        if current == goal:
            return reconstructPath(cameFrom, current)
        
        openSet.remove(current)
        for neighbor in graph.neighbors(current):
            tentative_gScore = gScore[current] + dist(current, neighbor)
            if tentative_gScore < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_gScore
                fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)
                if neighbor not in openSet:
                    openSet.add(neighbor)
    return failure

3.2 启发式函数怎么选？

嗯，这里要注意。启发式函数h(n)的选择直接影响A*的性能和正确性。

我建议你记住三个关键性质：

性质	含义	影响
可采纳性	h(n) ≤ 实际代价	保证找到最优解
一致性	h(n) ≤ c(n,m) + h(m)	保证效率，避免重复计算
信息性	h(n)越接近实际代价越好	搜索速度越快

常用的启发式函数有：

• 曼哈顿距离：适合四方向移动的网格地图
• 欧几里得距离：适合任意方向移动
• 对角线距离：适合八方向移动

3.3 基于车道图的全局规划

好了，现在我们把A*放到自动驾驶的语境里。你想想看，车不能像人一样随便走。它必须沿着车道走，不能逆行，不能压双黄线。

这就是「车道图」的概念。我们把高精地图里的车道抽象成图结构：

• 每个车道段是一个节点
• 车道之间的连接关系是边
• 边的权重可以是距离、时间、或者综合代价

我曾经踩过一个坑：直接把道路中心线连起来当路径。结果呢？路径确实最短，但车需要连续变道，根本没法安全执行。嗯，从那以后我学乖了——全局规划必须考虑车道拓扑。

来看一个典型的车道图结构：

// 车道图节点
struct LaneNode {
    int id;                    // 车道ID
    vector<Point> centerline;  // 车道中心线
    double length;             // 车道长度
    int speed_limit;           // 限速
    vector<int> predecessors;  // 前驱车道
    vector<int> successors;    // 后继车道
    vector<int> left_lanes;    // 左侧车道
    vector<int> right_lanes;   // 右侧车道
};

3.4 A* vs Dijkstra：到底选哪个？

说实话，这个问题没有标准答案。我个人的经验是：

• 地图小（节点数 < 1000）：用Dijkstra就够了，实现简单，不用操心启发式函数
• 地图大（节点数 > 10000）：必须用A*，否则计算时间受不了
• 实时性要求高：A* + 合适的启发式函数
• 需要最优解：两者都能保证最优，但A*更快

在Autoware里，默认用的是A*。为什么？因为城市道路的全局规划，地图动辄几万个节点，Dijkstra根本跑不动。

3.5 本章知识体系

下面这张图展示了全局路径规划的核心逻辑：

3.6 实战建议

最后，给你几个我在Autoware项目里摸爬滚打总结出来的建议：

1. 先跑通再优化：别一上来就想着怎么加速。先用小地图把整个流程跑通，确保路径生成正确。
2. 可视化调试：我习惯把搜索过程可视化出来。每一步扩展了哪些节点，最终路径长什么样，一目了然。
3. 注意边界情况：比如起点和终点在同一个车道段怎么办？地图数据有缺失怎么办？这些都要提前想好。
4. 性能监控：在代码里加上计时器，记录每次规划的时间。如果超过100ms，就要考虑优化了。

好了，这一章的内容就到这里。全局路径规划是自动驾驶的「大脑」，决定了车往哪开。下一章我们会深入局部路径规划，看看车怎么在动态环境中安全行驶。