4. 全局路径规划进阶：Hybrid A*算法，Lattice Planner，状态栅格与运动基元，路径平滑
全局路径规划做到一定程度，你会发现一个尴尬的事：A* 搜出来的路径，车根本走不了。为什么？因为 A* 不考虑车辆的运动学约束——它假设你可以原地掉头、直角转弯。但现实中的车，有最小转弯半径，有朝向角，有速度限制。
所以这一章，我们来聊聊真正能落地的全局规划方法。我个人在量产项目中用过其中两种，踩过不少坑，今天一并倒出来。
4.1 从 A* 到 Hybrid A*：把「车能不能走」放进搜索
Hybrid A* 和普通 A* 最大的区别在哪？
普通 A* 的节点是离散的网格坐标 (x, y)。Hybrid A* 的节点是连续状态 (x, y, θ, v, δ)——位置、朝向、速度、前轮转角。搜索时，它用车辆运动学模型往前推一段，生成一系列可达状态。
核心差异一句话： A* 搜的是「格子路径」，Hybrid A* 搜的是「可行驶轨迹」。
我在项目中遇到过一个问题：Hybrid A* 的搜索空间太大，实时性扛不住。后来怎么解决的？两个技巧：
- Reeds-Shepp 曲线做启发式：用不考虑障碍物的最短可行驶路径作为启发代价，比欧氏距离准得多。
- 控制节点扩展数量：每次只扩展 6-8 个控制动作（油门、转向），别贪多。
4.2 Lattice Planner：把规划问题拆成「横纵」
Lattice Planner 的思路很有意思——它把路径规划拆成横向（左右偏移）和纵向（前后速度）两个独立问题。你想想看，这样是不是简单多了？
具体做法：
- 在 Frenet 坐标系下，分别采样横向偏移量和纵向速度。
- 对每组 (d, v) 生成一条候选轨迹。
- 用代价函数选最优。
代价函数我一般这么配：
| 代价项 | 权重 | 说明 |
|---|---|---|
| 横向偏差 | 0.3 | 偏离参考线越远，代价越高 |
| 纵向加速度 | 0.2 | 保证乘坐舒适性 |
| 横向 jerk | 0.2 | 减少方向盘抖动 |
| 障碍物距离 | 0.3 | 安全第一 |
我的经验： 横向 jerk 的权重别设太低。我曾经为了追求路径平滑把 jerk 权重降到 0.05，结果车在高速上走 S 形，乘客差点晕车。
4.3 状态栅格与运动基元：让搜索更「聪明」
状态栅格（State Lattice）是什么？说白了，就是在状态空间里预先生成一组「运动基元」（Motion Primitives）。每个基元是一段短的可行驶轨迹，比如「直行 2 米」「左转 30° 前进 1.5 米」。
搜索时，你不需要实时算运动学——直接从基元库里挑，拼起来就是一条完整路径。
运动基元的设计有几个要点：
- 完备性：基元集合要能覆盖所有常见驾驶场景。
- 连续性：相邻基元的朝向、曲率必须连续。
- 效率：基元数量别太多，我一般控制在 20-30 个。
注意： 运动基元是离线生成的，但生成时一定要考虑车辆的实际参数。我见过有人用标准乘用车参数生成基元，结果装在卡车上完全不能用——转弯半径差太多了。
4.4 路径平滑：让轨迹「顺眼」
不管用哪种方法搜出来的路径，大概率是折线。直接给控制模块，车会一顿一顿的。所以需要平滑。
常用的两种方法：
4.4.1 B 样条曲线
B 样条的好处是局部可控——你改一个控制点，只影响附近一段曲线，不会全局变形。这在避障场景下特别有用。
代码实现片段：
import numpy as np
from scipy.interpolate import BSpline
# 控制点（来自 Hybrid A* 或 Lattice Planner 的输出）
control_points = np.array([[0, 0], [2, 1], [4, 3], [6, 2], [8, 0]])
# 生成 B 样条
knots = [0, 0, 0, 1, 2, 3, 4, 4, 4] # 3 次 B 样条
spline = BSpline(knots, control_points, 3)
# 采样平滑路径
t = np.linspace(0, 4, 100)
smooth_path = spline(t)4.4.2 贝塞尔曲线
贝塞尔曲线更简单，但全局可控——改一个控制点，整条曲线都变。适合做起点到终点的平滑连接。
嗯，这里要注意：贝塞尔曲线的阶数别太高。超过 5 阶，曲线容易振荡，反而不好。
我的建议： 实际项目中，我通常先用 B 样条做全局平滑，再用贝塞尔曲线做局部微调（比如泊车入位的最后一段）。
4.5 本章知识体系总览
下面这张图把四种方法的关系画清楚了：
4.6 避坑指南
最后，分享几个我踩过的坑：
- Hybrid A* 的离散化粒度：角度分辨率别设太大。我试过 15° 一步，结果搜出来的路径像锯齿。建议 5° 以内。
- Lattice Planner 的采样密度：横向采样太多，候选轨迹爆炸；太少，找不到好路径。我一般横向采 5-7 条，纵向采 3-5 条。
- 路径平滑的曲率约束：平滑完一定要检查最大曲率是否超过车辆极限。我曾经平滑出一条曲率 0.3 的路径，结果车的最小转弯半径只能到 0.15——直接报错。
嗯，全局路径规划进阶就聊到这儿。这些方法各有适用场景，关键是根据你的车型、算力和场景需求来选。没有银弹，只有最适合的。