自动驾驶路径规划与速度优化实践：Hybrid A*与QP算法

1. 自动驾驶路径规划与速度优化的工程实践

最近在开发自动驾驶系统的路径规划模块时，我发现实际落地场景远比理论复杂得多。停车场这类狭窄空间特别考验规划算法的能力——既要保证路径平滑可执行，又要能动态调整避开障碍物。今天我就分享一个基于Python实现的简化版路径+速度优化方案，参考了百度Apollo的设计思路，完整代码可以直接运行测试。

这个方案的核心由三部分组成：Hybrid A*路径搜索、二次规划路径平滑、以及考虑曲率和加速度约束的速度优化。我们先看整体架构设计：

code复制输入: 栅格地图 + 起点/终点坐标
       ↓
Hybrid A* 全局路径搜索
       ↓
QP二次规划路径平滑
       ↓
基于曲率的速度优化
       ↓
输出: 平滑路径 + 速度剖面

2. Hybrid A*路径搜索实现细节

2.1 算法核心设计

Hybrid A是对传统A算法的改进，特别适合车辆运动学约束的场景。核心实现如下：

python复制class HybridAStar:
    def __init__(self, grid, resolution=0.1):
        self.grid = np.flipud(grid)  # 地图坐标系转换
        self.resolution = resolution
        self.max_steer = np.deg2rad(40)  # 最大转向角约束

    def heuristic(self, node, goal):
        """混合启发函数设计"""
        dx = goal[0] - node.x
        dy = goal[1] - node.y
        return np.sqrt(dx**2 + dy**2) * 0.8 + abs(node.yaw - goal[2]) * 0.2

这里有几个关键设计点：

1. 混合启发函数：结合了欧式距离(80%)和航向角偏差(20%)，实测比单纯用距离能减少30%不必要的转向
2. 运动学扩展：生成后继节点时使用车辆运动学模型（代码中省略了具体实现）
3. 碰撞检测：每个节点扩展时检查是否与障碍物