在自动泊车系统中，找到车位只是第一步，如何规划一条既安全、又符合车辆运动学特性的轨迹停进去，是路径规划模块的核心任务。Hybrid A* 算法是解决这类问题的常用工具。

1. Hybrid A* 算法剖析

标准的 A* 算法在二维栅格地图上表现出色，但车辆不是像素点，它不能瞬移或原地90度转弯。Hybrid A* 正是为解决这个问题而生。

• 核心思想：将连续的车辆状态空间（x, y, θ）与离散的搜索网格结合。

• 算法流程：

  1. 初始化：

     • 创建 open_list（待探索节点）和 closed_list（已探索节点）。

     • 将车辆的起始状态（x, y, θ, cost）放入 open_list。

  2. 节点选择与扩展：

     • 从 open_list 中取出总成本 f(n) 最小的节点 n。

     • 对该节点 n 应用一组预定义的运动基元（Motion Primitives），如“左转前进”、“直行前进”、“右转前进”，生成一系列后继节点。每个后继节点都代表了车辆真实可达的一个新状态。

  3. 成本计算：f(n) = g(n) + h(n)

     • g(n)：实际成本。从起点到当前节点 n 的实际行驶路径长度。

     • h(n)：启发成本（Heuristic Cost）。这是 Hybrid A* 的精髓。它需要估算从当前节点 n 到终点的最短可行路径。

       • 简单欧氏距离不可取，因为它忽略了车辆的转弯约束。

       • 常用方法：使用 Reeds-Shepp (RS) 曲线 或 Dubins 曲线。这些曲线能计算出在满足最小转弯半径约束下，从当前姿态到目标姿态的最短路径长度（不考虑障碍物）。这个估算值远比直线距离更接近真实成本，能极大提高搜索效率。

  4. 碰撞检测：

     • 每个扩展出的后继节点，都需要将其代表的车辆完整轮廓（Footprint）与障碍物地图（通常是占据栅格地图）进行碰撞检查。

     • 发生碰撞的节点直接丢弃，不放入 open_list。

  5. 路径回溯：

     • 当一个节点的姿态落入目标区域内，则视为搜索成功。

     • 从该目标节点开始，沿着父节点指针一路回溯到起始节点，构建出完整的行驶轨迹。

2. 现实世界的优化：动态目标与卡尔曼滤波

理论算法在实际部署时，必须处理来自真实世界的不确定性。

• 面临的问题：

  1. 感知误差：视觉或超声波检测到的车位，其位置和姿态存在误差。

  2. 定位误差：车辆自身的定位（DR，Dead Reckoning）存在累积误差。

直接使用一个固定的、带误差的目标点进行规划，结果往往不理想。我们的改进策略如下。

• 策略：动态调整终点 + 状态滤波

  1. 粗规划与精定位：

     • 首先，使用初始感知的车位作为目标，进行一次粗略的路径规划。

     • 当车辆沿轨迹行驶，逐渐靠近车位时，传感器（特别是摄像头）能够获得更清晰、更精确的车位图像。

     • 此时，我们可以在规划路径上寻找一个最佳的“泊车切入点”（例如，与车位入口接近且姿态良好的点）。同时，利用近距离的精确观测，对车位的位置进行更新。

  2. 重规划（Re-plan）：

     • 将这个经过优化的“泊车切入点”作为 Hybrid A* 的新终点。

     • 同时，根据高置信度的车位观测，对车辆自身的定位误差进行修正，相当于重置了局部的定位参考系。

     • 以此为基础，重新运行 Hybrid A* 算法，生成一条更精准、更可行的最终泊车轨迹。

• 卡尔曼滤波（Kalman Filter）在其中的作用 为了得到一个稳定、可靠的车位目标，我们使用卡尔曼滤波来持续优化车位的状态。

  • 背景：在统一的全局坐标系（map frame）下进行。

  • 目标：平滑并锁定静态车位的真实位置，滤除感知和定位带来的噪声。

  • 滤波器设计：

    • 状态向量 x：描述车位的四个角点坐标。x = [x1, y1, x2, y2, x3, y3, x4, y4]^T。共8个状态量。

    • 状态转移方程：x_k = F * x_{k-1} + w_k

      • F 矩阵：由于车位是静态的，理论上其在全局坐标系下的位置不变。因此，状态转移矩阵 F 为单位矩阵 I。

      • Q (过程噪声协方差)：理论上车位不动，但我们的模型可能不完美（如地图微小漂移），车辆定位（DR）的误差也会被错误地归因到车位的移动上。Q 主要用于对冲这些由定位误差引入的不确定性。

    • 观测方程：z_k = H * x_k + v_k

      • z_k (观测值)：每一帧从传感器（如摄像头）识别并解算出的车位四角点坐标。

      • H 矩阵：由于我们直接观测状态量本身（角点坐标），所以观测矩阵 H 也为单位矩阵 I。

      • R (观测噪声协方差)：代表了传感器测量的不确定性。其误差来源包括：

        • 像素识别：模型本身的识别误差，图像拼接缝、边缘模糊导致的投影误差。

        • 坐标变换：相机内外参标定不准、地面不平整等因素导致的转换误差。

通过卡尔曼滤波的不断迭代，即使单次测量 z_k 存在较大噪声，滤波后的状态估计 x_k 也能快速收敛并稳定在一个更接近真值的位置。这个稳定的结果，为 Hybrid A* 提供了高质量、高可靠性的规划终点。