3、路径规划基础：图搜索算法（Dijkstra、A*）、工程实现中的数据结构（堆、图）、A*算法的工程优化技巧

3.1 图搜索的本质：我们到底在找什么？

说实话，很多刚入行的同学会把图搜索想得很玄乎。其实说白了，就是在一个地图上，找到一条从起点到终点的路。你想想看，我们开车导航时，手机里跑的不就是这个东西吗？

我个人习惯把路径规划拆成两个问题：「怎么表示地图」和「怎么找路」。前者是数据结构的事，后者是算法的事。今天我们就一个一个来啃。

3.2 Dijkstra：教科书里的完美主义者

Dijkstra 算法，我当年在学校里学的时候觉得它完美无缺。直到第一次在实车上跑，才发现理想很丰满，现实很骨感。

它的核心逻辑很简单：从起点开始，一层一层往外扩，每次选当前代价最小的节点继续扩展。就像在水面上扔一颗石子，波纹一圈圈往外推，直到碰到终点。

伪代码长这样：

function Dijkstra(Graph, start, goal):
    open_set = {start}          // 待探索的节点
    g[start] = 0                // 起点到自身的代价
    parent = {}                 // 记录路径
    
    while open_set is not empty:
        current = 从 open_set 中取出 g 值最小的节点
        
        if current == goal:
            return reconstruct_path(parent, goal)
        
        for each neighbor of current:
            new_cost = g[current] + cost(current, neighbor)
            if new_cost < g[neighbor]:
                g[neighbor] = new_cost
                parent[neighbor] = current
                open_set.add(neighbor)
    
    return failure  // 没找到路

嗯，这里要注意：Dijkstra 的「最优」是有代价的。它需要遍历大量无关节点。我在项目中遇到过，在一个 500m×500m 的城区地图上，Dijkstra 跑了 200ms 才出结果。对于高速场景，这个时间是不可接受的。

3.3 A*：带「方向感」的搜索

A* 算法，说白了就是在 Dijkstra 的基础上加了一个「指南针」。这个指南针就是启发式函数 h(n)，它告诉算法：「嘿，往这个方向走更有可能到终点！」

核心公式就一个：

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

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

你想想看，如果 h(n) 始终为 0，A* 就退化成 Dijkstra。如果 h(n) 恰好等于真实距离，A* 就能直线到达终点，一步弯路都不走。

工程上最常用的启发式函数是欧几里得距离和曼哈顿距离：

// 欧几里得距离（直线距离）
h_euclidean = sqrt((x1-x2)^2 + (y1-y2)^2)

// 曼哈顿距离（城市街区距离）
h_manhattan = |x1-x2| + |y1-y2|

3.4 工程实现中的数据结构：堆与图

算法讲完了，但代码怎么写才能跑得快？这就涉及到数据结构的选择了。我见过太多人算法原理倒背如流，一写代码就卡在「open_set 怎么实现」上。

3.4.1 优先队列（堆）

A* 和 Dijkstra 都需要频繁地从 open_set 中取出 f 值最小的节点。如果每次都用数组遍历找最小，复杂度是 O(n)。当节点数上万时，这简直是灾难。

解决方案：用二叉堆（Binary Heap）。插入和取出最小值的复杂度都是 O(log n)。

// C++ 中使用优先队列（默认是大顶堆，需要自定义比较器）
struct Node {
    int id;
    double f;
    bool operator>(const Node& other) const {
        return f > other.f;
    }
};

std::priority_queue<Node, std::vector<Node>, std::greater<Node>> open_set;

嗯，这里有个坑：优先队列不支持更新节点优先级。当你发现一个更短的路径到达某个节点时，你不能直接修改堆中该节点的 f 值。我的做法是：直接把新节点 push 进去，然后在取出时检查这个节点是否已经被处理过。

3.4.2 图的表示

自动驾驶中的地图，说白了就是一个有向加权图。每个路口是一个节点，每条道路是一条边，边的权重可以是距离、时间、能耗等。

我推荐用邻接表来存储图：

// 邻接表表示图
std::unordered_map<int, std::vector<Edge>> graph;

struct Edge {
    int to;         // 目标节点 ID
    double cost;    // 通行代价（距离/时间）
    int lane_id;    // 车道 ID（用于后续轨迹规划）
};

为什么不用邻接矩阵？因为自动驾驶地图通常很稀疏（一个路口只连接 2-4 条路），邻接矩阵会浪费大量内存。我见过一个城市级地图有 10 万个节点，邻接矩阵需要 10^10 个元素，而邻接表只需要几十万个。

3.5 A* 算法的工程优化技巧

理论讲完了，来点干货。这些优化技巧，都是我踩过坑之后总结出来的。

3.5.1 双向 A* 搜索

从起点和终点同时开始搜索，当两个搜索区域相遇时，路径就找到了。这能把搜索空间从 O(b^d) 降到 O(2 * b^(d/2))，效果非常明显。

我在项目中实测过：单向 A* 搜索 5000 个节点，双向 A* 只搜索了 1200 个节点，时间从 80ms 降到了 25ms。

3.5.2 JPS（Jump Point Search）

如果你用的是网格地图（比如栅格地图），JPS 是 A* 的超级加速版。它的核心思想是：在直线方向上，不需要逐个格子搜索，直接「跳」到关键转折点。

举个例子：如果前方是一段直路，没有障碍物，A* 会逐个格子检查，而 JPS 会直接跳到路的尽头或障碍物前。这能减少 90% 以上的节点扩展。

3.5.3 权重调整与次优解

有时候，我们不需要绝对最优路径。比如在高速上，多绕 5% 的路程可能换来 50% 的计算时间节省。这时候可以用权重 A*（Weighted A*）：

f(n) = g(n) + w * h(n)   // w > 1

w 越大，算法越「贪心」，搜索越快，但路径可能不是最优的。我一般把 w 设在 1.2 到 1.5 之间，既能保证路径质量，又能显著提升速度。

3.5.4 路径平滑与后处理

A* 找出来的路径往往是折线，车辆没法直接沿着走。我习惯在 A* 之后加一个路径平滑步骤：

• 冗余点剔除：如果三个连续点共线，去掉中间那个
• 拐角圆滑：用贝塞尔曲线或样条曲线拟合拐角
• 碰撞检查：平滑后的路径不能碰到障碍物

// 简单的冗余点剔除
std::vector<Point> simplifyPath(const std::vector<Point>& path) {
    if (path.size() < 3) return path;
    std::vector<Point> result = {path[0]};
    for (size_t i = 1; i < path.size() - 1; ++i) {
        if (!isCollinear(path[i-1], path[i], path[i+1])) {
            result.push_back(path[i]);
        }
    }
    result.push_back(path.back());
    return result;
}

3.6 小结：选对工具，事半功倍

最后，我给大家一个选择建议：

场景	推荐算法	原因
小规模地图（<1000节点）	Dijkstra	实现简单，保证最优
中等规模路网	A* + 双向搜索	速度快，路径质量好
大规模网格地图	JPS	极速，适合栅格地图
实时性要求极高	Weighted A* (w=1.5)	牺牲少量最优性，换取速度

记住一句话：没有最好的算法，只有最合适的算法。我在项目中换过无数次方案，每次都是根据实际场景来选。你想想看，如果高速上 100ms 就要出一版路径，你还敢用 Dijkstra 吗？