3、混合A*算法：Reeds-Shepp曲线、节点扩展、碰撞检测、在规划中的应用

说实话，混合A*算法是我在自动驾驶规划里最常用的算法之一。它不像传统A*那样只考虑位置，而是把车辆的朝向也纳入了搜索空间。说白了，就是让规划出来的路径更符合车辆的运动学约束。

我记得第一次在实车上跑纯A*规划时，车根本走不出那些直角转弯。后来换成混合A*，效果立竿见影。今天我就把这块的核心内容掰开揉碎了讲给你听。

3.1 为什么需要混合A*？

传统A*算法在网格地图上搜索路径，每个节点只记录(x, y)坐标。但车辆不是全向移动的，它有最小转弯半径、有朝向约束。你想想看，一个路径规划出来，车却开不过去，那规划还有什么意义？

混合A*的核心改进在于：每个节点不仅包含位置，还包含朝向角θ。搜索空间从2维变成了3维(x, y, θ)。代价就是计算量上去了，但换来的是路径的可执行性。

核心区别一句话总结：

A*：找最短路径，不管车能不能走
混合A*：找最短且车能走的路径

3.2 Reeds-Shepp曲线：让路径更平滑

Reeds-Shepp曲线，简称RS曲线。它解决的是这样一个问题：给定起点和终点的位姿(x, y, θ)，找出一条最短的、满足车辆运动学约束的路径。

RS曲线由若干段圆弧和直线拼接而成。圆弧对应方向盘打到最大角度，直线对应方向盘回正。一共有48种组合模式，但常用的也就那么几种。

我在项目中遇到过一个问题：直接用RS曲线做全局规划，路径倒是平滑了，但遇到障碍物就傻眼了。因为RS曲线不考虑障碍物，它只负责连接两个位姿。所以实际应用中，RS曲线通常作为混合A*的启发式函数或者终点连接段来使用。

我的经验：

当搜索节点距离终点较近时（比如5米以内），直接尝试用RS曲线连接。如果无碰撞，就可以提前终止搜索。这招能省下大量计算时间。

3.3 节点扩展：不只是上下左右

混合A*的节点扩展和传统A*完全不同。传统A*可以往8个方向走，混合A*只能按照车辆的运动模型来扩展。

具体来说，每个节点扩展时，我会生成一组控制量：

速度：前进、后退（通常取固定值）
方向盘转角：左最大、右最大、回正（或者中间再加几个档位）

然后根据车辆运动学模型（比如自行车模型），向前模拟一小段距离，得到新的位姿。这就是一个子节点。

举个例子，我常用的扩展参数：

控制量	取值	说明
速度	1.0 m/s	前进
速度	-0.5 m/s	后退
方向盘转角	-0.6 rad	左满舵
方向盘转角	0.0 rad	回正
方向盘转角	0.6 rad	右满舵

这样组合下来，每个节点最多扩展出6个子节点（2个速度 × 3个转角）。当然，你可以根据场景调整，比如泊车场景我会增加后退的档位。

3.4 碰撞检测：不能碰就是不能碰

碰撞检测是规划算法的底线。混合A*里，每扩展出一个新节点，都要做碰撞检测。

怎么做？我一般用包围盒法：

把车辆近似为一个矩形（长宽已知）
根据当前位姿，计算矩形四个顶点的坐标
检查这些顶点是否落在障碍物网格内

代码实现大概是这样的：

bool isCollision(const Node& node, const GridMap& map) {
    // 车辆参数
    double length = 4.5;  // 车长
    double width = 2.0;   // 车宽
    
    // 计算四个顶点
    double theta = node.theta;
    double cos_t = cos(theta);
    double sin_t = sin(theta);
    
    // 前左、前右、后左、后右
    vector<Point> corners = {
        {node.x + length/2*cos_t - width/2*sin_t,
         node.y + length/2*sin_t + width/2*cos_t},
        // ... 其他三个顶点
    };
    
    // 检查每个顶点是否在障碍物上
    for (auto& pt : corners) {
        if (map.isOccupied(pt.x, pt.y)) {
            return true;  // 碰撞
        }
    }
    return false;  // 安全
}

注意：

我曾经因为只检测四个顶点，忽略了车辆侧边中间部分，导致规划出来的路径擦着障碍物边缘过去。后来我改成每隔0.5米取一个检测点，虽然慢了一点，但安全多了。

3.5 在规划中的应用流程

好了，前面把各个模块都讲清楚了。现在把它们串起来，看看混合A*在实际规划中怎么用。

整体流程是这样的：

初始化：把起点加入open list
循环搜索：
- 从open list中取出代价最小的节点
- 如果该节点距离终点很近，尝试用RS曲线连接
- 如果RS曲线无碰撞，搜索成功
- 否则，扩展该节点的子节点
- 对每个子节点做碰撞检测
- 无碰撞的子节点加入open list
回溯路径：从终点反向回溯到起点，得到完整路径

下面这张图展示了混合A*的核心逻辑：

嗯，这里要注意一点：代价函数的设计直接影响搜索效果。我一般用三个代价的加权和：

g(n)：从起点到当前节点的实际行驶距离
h(n)：从当前节点到终点的启发式距离（用RS曲线长度）
penalty：倒车惩罚、转向惩罚等

倒车惩罚我设得比较高，因为倒车不仅慢，而且不安全。但完全禁止倒车也不行，有些场景（比如狭窄的停车场）必须倒车才能出来。

避坑指南：

我曾经把倒车惩罚设得太高，结果在停车场里规划出来的路径需要打十几把方向才能停进去。后来我把惩罚系数调低了一点，路径就顺畅多了。这个参数需要根据实际场景反复调试。

3.6 实际项目中的注意事项

最后，分享几个我在实际项目中踩过的坑：

网格分辨率：太粗了路径不精确，太细了计算量爆炸。我一般用0.5米左右，泊车场景用0.2米。
角度离散化：角度不需要太细，15度一个档位就够用了。太细了节点数会指数增长。
搜索超时：一定要设置最大搜索时间。我一般设0.5秒，超时了就返回当前最优路径，或者降级到其他算法。
路径平滑：混合A*出来的路径虽然可执行，但不够平滑。后面通常还要接一个优化步骤，比如用二次规划或者样条曲线做平滑。

混合A*算法，说白了就是在搜索空间和路径质量之间做权衡。理解了这个权衡，你就能根据实际场景灵活调整参数了。