Hybrid A* Path Planning · Articulated Vehicle
本演示采用改进型 Hybrid A* 算法规划铰接式车辆（牵引车+挂车）的泊车轨迹。所有场景使用同一套规划器，无逐场景手工调参。以下逐层展开算法实现细节。
采用自行车模型（Bicycle Model）建模牵引车-挂车系统。状态量为 4 维：
其中 (x, y) 为铰接点（第五轮）位置，θ₁ 为牵引车纵轴航向角，θ₂ 为挂车纵轴航向角。铰接角 β = θ₁ − θ₂。
连续时间运动学方程（v 为速度，δ 为前轮转向角）：
数值积分采用前向欧拉法，步长 dt = 0.05s。倒车时 v < 0，铰接角 β 的变化率取决于 sin(β)——当 β ≈ 0 时挂车航向几乎不变，这正是倒车精修阶段的难点。
| 参数 | 牵引车 | 挂车 | 乘用车 |
|---|---|---|---|
| 轴距 | L₁ = 3.5m | L₂ = 8.0m | L = 2.8m |
| 最大转向角 | δ_max = 0.40 rad (22.9°) | 0.55 rad (31.5°) | |
| 车宽 | 2.5m | 1.8m | |
| 状态维度 | 4D (x, y, θ₁, θ₂) | 3D (x, y, θ) | |
Hybrid A* 在连续状态空间中搜索，同时用离散网格进行节点去重。每个搜索节点的状态 (x, y, θ₁, θ₂) 是连续值，但用网格键进行量化以避免无限扩展：
其中 g_xy = 0.4m, g_θ = 4°, g_β = 4°。closed set 记录每个网格键的最优代价 g，新节点仅在 g 更优时入队。
每次节点扩展时，对所有可用的 (转向角 δ, 方向 v) 组合执行一段运动基元（motion primitive）：以固定 δ 和 v 积分 seg = 0.7m 距离，生成子节点。代价函数：
其中倒车惩罚 λ₁ = 0.1，转向变化惩罚 λ₂ = 0.2，大铰接角惩罚 λ₃ 在 |β| > 15° 时线性增加。这些惩罚项引导搜索优先选择平稳的前进轨迹，避免不必要的折叠。
启发式函数 h(n) 采用两种策略的较大值：
铰接车辆倒车时的折叠效应（jackknife）是最大的安全约束。当 |β| 过大时，继续错误方向的转向会导致挂车与牵引车折叠，造成不可逆的危险姿态。
关键公式——中性转向角 δ_neutral 将舵角空间分为"增大 β"和"减小 β"两个半平面：
分级限制策略：
| 铰接角范围 | 转向限制 | 说明 |
|---|---|---|
| |β| < 20° (BETA_WARN) | 全范围 [−δ_max, +δ_max] | 安全区间，系统稳定 |
| 20° ≤ |β| < 35° | 仅允许 β 减小方向的半区间 | 限制到 δ_neutral 的安全侧 |
| |β| ≥ 35° (BETA_MAX) | 节点直接拒绝 | 硬限制，防止折叠 |
这一改进基于 Cao et al. (2025) 的工作。相比固定转向集，配置相关限制在搜索过程中动态裁剪不安全分支，大幅减少了因折叠导致的无效扩展和回溯。
Hybrid A* 的网格分辨率（g_θ = 4°）限制了可达到的终态角度精度。为达到亚度级精度，算法在搜索过程中周期性尝试连续弧线连接——从当前节点出发，用固定 (δ, v) 积分一条连续轨迹，检查是否直达目标。
双级目标判定（at_goal）策略：
当网格搜索到达松容差目标时，先尝试一次解析展开。如果找到精确连接，使用连续轨迹（亚度级精度）；否则接受网格节点（由 endgame 精修）。
当 A* 到达目标区域但角度精度不足时，进入 endgame 阶段。这是一个多阶段闭环控制流程：
为什么需要 Shunt？ 当 β ≈ 0 时，θ̇₂ = v·sin(0)/L₂ ≈ 0——挂车航向被"冻结"。无论怎么调整 δ，都无法改变 θ₂。前进 shunt 创造一个非零 β，使 θ₂ 重新可控，然后 Phase 1 的 PD 利用这个 β 修正角度。
combined_err 用于评估路径质量（位置误差 + 加权角度误差）：
车辆几何体建模为有向边界框（OBB），障碍物为轴对齐边界框（AABB）。碰撞检测使用分离轴定理（Separating Axis Theorem）：
牵引车和挂车分别检查碰撞（各自独立的 OBB）。在运动基元积分中每 2 步检测一次碰撞（每 0.1m）。
| 场景 | 搜索节点 | θ₁误差 | θ₂误差 | β终态 | 位置误差 | 耗时 |
|---|---|---|---|---|---|---|
| 直线倒车 | 2,349 | 0.48° | 0.26° | 0.22° | 0.25m | 1s |
| 偏角倒车 | 115,400 | 0.34° | 0.17° | 0.17° | 0.21m | 61s |
| 对比-重卡 | 90,400 | 0.25° | 0.06° | 0.19° | 0.21m | 49s |
| 垂直入库-重卡 | 110,330 | 0.29° | 0.06° | 0.36° | 0.18m | 155s |
| 窄通道-重卡 ★ | CasADi/IPOPT | 0.00° | 0.00° | 0.00° | 0.00m | 11s |
所有卡车场景终态：θ₁、θ₂、β 均 < 0.5°，位置误差 < 0.25m。窄通道场景使用 CasADi/IPOPT 直接最优控制，精度达机器零。规划为离线执行，结果序列化为 JSON 内嵌于页面，浏览器端仅负责动画播放。
12 米窄通道 90° 入库是 Hybrid A* 的物理极限场景：A* 能在 300 秒内收敛角度（θ₂ 误差 < 2°），但位置偏差锁定在 11+ 米——因为离散搜索无法发现需要先向东行驶 14m 再折返的轨迹拓扑。
窄通道场景改用 CasADi/IPOPT 直接最优控制：将完整轨迹参数化为 N=120 步连续控制序列，用非线性规划求解器同时优化所有控制输入。
问题建模：
权重设计：转向平滑项权重（1.0）远大于转向幅值项（0.01），目标是优先保证方向盘变化平缓（乘客舒适），而非追求最短路径。速度平滑权重 0.5 次之，避免急刹急冲。
碰撞约束实现：对每个时间步，检查牵引车和挂车的 12 个关键点（4 个牵引车角点 + 沿挂车 4 个截面的左右边沿）。每个点必须位于所有障碍物 AABB 之外（含 0.4m 安全裕度）。使用 CasADi 的 fmax 算子表达析取约束（disjunctive constraint），IPOPT 在非光滑点处自动处理。
求解过程：IPOPT（Interior Point OPTimizer）使用内点法在 ~100 次迭代内收敛，总耗时 ~11 秒。初始猜测为 5 段线性插值（前向→调转→倒车→对齐→入位），IPOPT 从粗略猜测出发自动优化出最优轨迹。
关键发现：最优解为两段式轨迹——卡车先前向大弧行驶至 (14, 4) 完成方向调转（70 步），再直线倒车入位（50 步），仅 1 次方向切换。这解释了 A* 失败的原因：离散搜索在前向大弧阶段因网格分辨率不足而无法发现这条路径。