Hybrid A* 算法原理
Hybrid A* 算法是一种用于机器人或车辆路径规划的算法，特别是在考虑车辆运动学约束（如非完整性约束）的连续空间中进行规划时非常有效。它结合了离散搜索算法（如 A*）的高效性和连续空间规划的准确性。
1. 核心思想
- 在离散的栅格地图上进行搜索（像传统 A*），但每个搜索节点代表一个连续的状态（例如车辆的位置 $(x, y)$ 和朝向 $\theta$）。
- 节点之间的连接（移动）不是简单的相邻网格移动，而是通过模拟车辆的运动学模型生成的连续轨迹段（如圆弧、直线）。
- 利用启发式函数来引导搜索方向，加速找到可行路径。
2. 关键组成部分
- 状态空间：每个节点包含 $(x, y, \theta)$ 或 $(x, y, \theta, v)$ 等连续状态变量。
- 运动学模型：定义了车辆如何从一个状态移动到另一个邻近状态。通常使用简化模型，如： $$ x_{new} = x + v \cdot \cos(\theta) \cdot \Delta t $$ $$ y_{new} = y + v \cdot \sin(\theta) \cdot \Delta t $$ $$ \theta_{new} = \theta + \frac{v}{L} \cdot \tan(\phi) \cdot \Delta t $$ 其中 $(x_{new}, y_{new}, \theta_{new})$ 是新状态，$v$ 是速度（可能固定或离散化），$\phi$ 是前轮转向角（离散化取值），$L$ 是轴距，$\Delta t$ 是时间步长。
- 碰撞检测：在模拟生成轨迹段时，必须检查该轨迹是否与地图中的障碍物发生碰撞。这是计算开销较大的部分。
- 启发式函数 $h(n)$：
- 通常结合两部分：
- 到目标的欧几里得距离：$h_{euclid} = \sqrt{(x - x_{goal})^2 + (y - y_{goal})^2}$。
- 考虑朝向的 Reeds-Shepp 曲线距离：$h_{rs}$（或 Dubins 曲线距离，如果车辆只能前进）。Reeds-Shepp 曲线是在满足车辆运动学约束下连接两个位姿的最短路径族。
- 最终的 $h(n) = \max(h_{euclid}, h_{rs})$ 或 $h(n) = h_{euclid} + h_{rs}$（需根据实际情况调整）。
- 通常结合两部分：
- 代价函数 $g(n)$：通常是从起点到当前节点 $n$ 的实际行驶距离或时间。
- 评估函数 $f(n) = g(n) + h(n)$：用于优先级队列排序。
3. 与传统 A* 的区别
- 状态连续：节点代表连续状态而非离散网格中心。
- 移动方式：移动是模拟车辆动力学产生的连续轨迹，而非网格邻接。
- 启发式：使用更复杂的 Reeds-Shepp/Dubins 启发式。
- 网格离散化：为了管理搜索空间，通常会将连续状态 $(x, y, \theta)$ 离散化存储在一个状态栅格中（例如，将位置量化为网格，朝向量化为固定角度区间）。只有当新节点的状态落在与现有节点不同的离散单元中，或者其 $g$ 值更小时，才会被加入开放列表。
Hybrid A* 算法 C++ 实现步骤
以下是用 C++ 实现 Hybrid A* 算法的关键步骤和代码框架：
1. 关键函数实现
motionModel(const Node& current, const Action& action, double resolution, double dt):- 根据当前状态和动作（如转向角 $\phi$，速度 $v$），利用车辆运动学方程计算 $\Delta t$ 时间后的新状态 $(x_{new}, y_{new}, \theta_{new})$。
- 通常会将生成的轨迹离散化为若干小段（步长为
resolution），用于后续碰撞检测。
