第三章 全局路径规划：基于图搜索的A*、Dijkstra、混合A*算法原理与工程实现，以及车道级导航地图的使用

全局路径规划是自动驾驶系统决策层的起点，其核心任务是在已知的高精地图环境中，从起点到终点搜索出一条满足车辆运动学约束、安全且高效的可行路径。本章聚焦于三类最经典的图搜索算法——Dijkstra、A* 与 Hybrid A*，并深入探讨它们在车道级导航地图（Lane-Level HD Map）上的工程化落地。我们将从算法原理出发，结合代码级实现细节、地图数据结构以及真实道路场景中的性能调优，帮助学员建立从理论到量产级代码的完整认知。

3.1 核心原理：从图搜索到运动约束

图搜索算法的本质是在离散化的状态空间中寻找一条从起点到终点的最优路径。在自动驾驶场景中，状态空间通常由车道级地图的拓扑结构或栅格地图构成。Dijkstra算法采用广度优先策略，以起点为中心向外层层扩展，每次选择当前累计代价g(n)最小的节点进行扩展，直到访问到终点。该算法保证在非负权重的图中找到全局最优解，但搜索范围呈圆形扩散，在大型地图中计算开销巨大。A*算法在Dijkstra的基础上引入启发式函数h(n)，通过f(n)=g(n)+h(n)引导搜索方向。当h(n)满足可采纳性（即不大于真实代价）时，A*既能保证最优性，又能显著减少搜索节点数。在实际工程中，常用欧几里得距离或曼哈顿距离作为启发式，但需注意在车道级地图中，由于道路拓扑的约束，简单的几何距离可能高估实际代价，导致搜索效率下降。

Hybrid A*算法则进一步考虑了车辆的运动学模型。与普通A*在离散网格中搜索不同，Hybrid A*在连续状态空间（x, y, yaw）中进行搜索，每个节点代表车辆的一个位姿。扩展节点时，通过转向控制指令（如最大前轮转角）生成符合车辆转弯半径的弧线轨迹，而非简单的四邻域或八邻域移动。这使得搜索出的路径天然满足车辆的最小转弯半径约束，但代价是状态空间维度增加，计算复杂度显著上升。Hybrid A*通常不保证最优性，但能快速找到一条可行路径，后续再通过优化算法（如共轭梯度下降）进行平滑处理。

3.2 判断方法与操作流程：算法选型与工程实现

在实际项目中，算法选型需要根据应用场景和计算资源进行权衡。下表总结了三种算法的关键特性与适用场景：

工程实现流程通常包含以下步骤：首先，解析车道级地图数据（如Lanelet2或OpenDRIVE格式），提取车道中心线、车道边界、交通规则（限速、禁行）等信息，构建有向拓扑图。其次，根据规划需求选择算法：若在结构化道路上且计算资源充足，优先采用Hybrid A*；若在简单园区或低速场景，A*配合栅格化地图即可满足需求。然后，设计代价函数，需综合考虑路径长度、车道偏移量、曲率变化、与障碍物的距离等因素。最后，对搜索出的路径进行后处理，包括路径平滑（如B样条拟合）、时间戳分配和速度规划接口对接。

struct Node {
    int x, y;           // 栅格坐标
    double g, h, f;     // 代价
    Node* parent;       // 父节点指针
    bool isClosed;      // 是否已扩展
};

// 优先队列按f值排序
std::priority_queue<Node*, std::vector<Node*>, CompareF> openList;

// 启发式函数（欧几里得距离）
double heuristic(int x1, int y1, int x2, int y2) {
    return std::sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
}

// 主循环
while (!openList.empty()) {
    Node* current = openList.top(); openList.pop();
    if (current->x == goal.x && current->y == goal.y) {
        return reconstructPath(current);
    }
    current->isClosed = true;
    // 遍历8邻域节点
    for (auto& dir : directions) {
        Node* neighbor = getNode(current->x + dir.x, current->y + dir.y);
        if (neighbor->isClosed || !isWalkable(neighbor)) continue;
        double tentative_g = current->g + distance(current, neighbor);
        if (tentative_g < neighbor->g) {
            neighbor->g = tentative_g;
            neighbor->h = heuristic(neighbor->x, neighbor->y, goal.x, goal.y);
            neighbor->f = neighbor->g + neighbor->h;
            neighbor->parent = current;
            openList.push(neighbor);
        }
    }
}

3.3 案例拆解：城市复杂路口混合A*路径规划

我们以某城市中心十字路口为例，该路口包含左转待转区、非机动车道和行人斑马线，车道宽度3.5米，限速40km/h。车辆从南向北行驶，需左转进入西侧道路。初始定位精度±0.3米，航向角误差±2°。使用Hybrid A*算法进行规划，车辆轴距2.8米，最大前轮转角35°，最小转弯半径约5.3米。

在实现过程中，我们首先将车道级地图转换为栅格地图，分辨率设为0.5米/像素，并将车道边界、路缘石和静态障碍物（如电线杆）标记为不可通行区域。Hybrid A*的搜索步长设为1.5米，航向角离散化为15°间隔。代价函数设计为：总代价 = 路径长度代价 + 车道偏移代价（偏离车道中心线每米增加0.8）+ 转向代价（每度转角增加0.05）+ 倒车惩罚（倒车一次增加10）。

实际运行中，搜索空间约为200×200×24（x,y,yaw），节点总数约96万个。在Intel i7-8700 CPU上，平均搜索时间为180ms，生成路径长度约85米，包含12个航向变化点。路径平滑后，最大曲率从初始的0.19/m降至0.12/m，满足车辆动力学约束。对比纯A*算法（不考虑运动学），Hybrid A*生成的路径可直接用于控制层跟踪，无需额外的轨迹重规划，但计算时间增加了约3倍。

3.4 车道级导航地图的使用与数据融合

车道级导航地图（Lane-Level HD Map）是全局路径规划的基础数据源。与标准导航地图不同，它包含车道中心线几何、车道边界线类型（实线/虚线）、车道连接关系、交通标志标线、限速值等精细化信息。在工程实践中，常用的地图格式包括Lanelet2（Autoware社区标准）和OpenDRIVE（商业工具链标准）。Lanelet2采用XML格式存储，每个车道（Lanelet）由左右边界点集构成，并包含属性标签（如车道类型、速度限制）。OpenDRIVE则使用道路（Road）和车道（Lane）的层级结构，支持复杂的道路交叉口建模。

地图数据在使用前需经过严格的坐标转换和拓扑校验。首先，将地图从WGS84经纬度坐标转换为局部UTM坐标或ENU坐标，确保与车辆定位系统（如RTK-GPS）坐标系一致。其次，检查车道拓扑的连通性，包括车道之间的衔接、环岛进出口、匝道分合流点等。一个常见的工程陷阱是地图数据中的车道中心线在交叉口处存在断裂或重叠，导致搜索算法无法找到可行路径。解决方法是编写拓扑修复工具，自动检测并连接断点，或人工标注关键连接关系。

在路径搜索过程中，地图数据不仅提供几何信息，还提供语义约束。例如，在车道级规划中，算法需要优先选择当前车道或允许变道的相邻车道，避免跨越实线。这可以通过在代价函数中加入车道变更惩罚来实现。此外，地图中的限速信息可用于动态调整路径的时间代价，使规划结果更符合实际驾驶行为。