4. Lattice Planner：状态采样、轨迹生成、代价评估、参数调优
Lattice Planner，说白了就是“格子规划器”。
我第一次听到这个名字，还以为是在棋盘格上走迷宫。后来做项目才明白，它是在时空维度上撒下一张网，然后从网里捞出一条最优轨迹。嗯，这个比喻可能不太严谨，但你想想看，自动驾驶的规划问题，本质上就是在时间和空间里找一条安全、舒适、合法的路径。Lattice Planner 就是干这个的。
4.1 状态采样：撒网的艺术
状态采样是第一步。我们要在 Frenet 坐标系下，对横向和纵向运动进行离散化采样。
为什么要用 Frenet 坐标系？因为道路是弯曲的。在笛卡尔坐标系下，一条弯道上的规划问题会变得非常复杂。Frenet 坐标系把问题分解成“沿着路走”和“偏离路多少”两个独立维度，这就简单多了。
我个人的习惯是，横向采样关注 d（横向偏移）和 d'（横向速度），纵向采样关注 s（纵向位移）和 s'（纵向速度）。采样密度直接决定了轨迹的精细程度和计算开销。
- 横向采样：d 范围 [-3.5m, 3.5m]，步长 0.5m；d' 范围 [-1.0m/s, 1.0m/s]，步长 0.2m/s
- 纵向采样：s 范围 [当前 s + 10m, 当前 s + 80m]，步长 5m；s' 范围 [0m/s, 目标速度 + 2m/s]，步长 1m/s
- 时间采样：T 范围 [4s, 10s]，步长 1s
我在项目中遇到过一个问题：采样步长设得太小，轨迹数量爆炸，实时性完全扛不住。后来我学乖了，根据场景动态调整采样密度——直道稀疏采样，弯道和路口加密采样。效果立竿见影。
4.2 轨迹生成：从点到线
采样得到的是一个个状态点。轨迹生成，就是把这些点用平滑的曲线连接起来。
我们通常用五次多项式来拟合横向轨迹，用四次或五次多项式来拟合纵向轨迹。为什么是五次？因为五次多项式可以保证位置、速度、加速度的连续性。你想想看，如果加速度不连续，乘客会是什么感受？
// 横向轨迹生成示例（C++风格伪代码）
// 输入：起始状态 (d0, d0_dot, d0_ddot)，目标状态 (d1, d1_dot, d1_ddot)，时间 T
// 输出：五次多项式系数 a0-a5
std::vector<double> GenerateLateralTrajectory(
double d0, double d0_dot, double d0_ddot,
double d1, double d1_dot, double d1_ddot,
double T) {
// 构建矩阵方程 A * x = b
// 这里省略了具体的矩阵求解过程
// 实际项目中我会用 Eigen 库来解
std::vector<double> coeffs(6);
coeffs[0] = d0;
coeffs[1] = d0_dot;
coeffs[2] = d0_ddot / 2.0;
// ... 计算 a3, a4, a5
return coeffs;
}
纵向轨迹类似，但目标状态通常是巡航速度或跟车距离。我记得有一次调试，纵向轨迹的终点速度设得太高，导致车辆在路口前急刹车。后来我加了一个“目标速度合理性检查”，才把这个问题解决。
4.3 代价评估：给轨迹打分
轨迹生成了，但哪条最好？这就需要代价评估。
代价函数通常包含以下几个维度：
| 代价项 | 权重 | 说明 |
|---|---|---|
| 横向偏移代价 | 10.0 | 偏离车道中心越远，代价越高 |
| 纵向速度代价 | 5.0 | 偏离目标速度越远，代价越高 |
| 加速度代价 | 3.0 | 加速度越大，舒适性越差 |
| 加加速度代价 | 1.0 | 加加速度（jerk）影响乘坐体验 |
| 碰撞代价 | 1000.0 | 与障碍物有碰撞风险，代价极高 |
代价评估的公式很简单：total_cost = w1 * cost_lateral + w2 * cost_longitudinal + w3 * cost_accel + ...
但真正难的是权重的设定。我曾经在一个项目中，横向偏移代价设得太高，导致车辆在变道时犹豫不决，总是贴着车道线走。后来我把权重从 10.0 降到 6.0，变道就果断多了。
4.4 参数调优：实战中的血泪史
参数调优，是 Lattice Planner 最磨人的部分。没有之一。
我总结了一套参数调优的流程：
- 先调安全参数：碰撞代价权重、安全距离阈值。确保车辆在任何情况下都不会撞。
- 再调舒适参数：加速度和加加速度权重。让乘客感觉不到顿挫。
- 最后调效率参数：目标速度偏差权重、横向偏移权重。让车辆能高效地到达目的地。
嗯，这里要注意一点：参数调优没有银弹。同样的参数，在高速场景和城区场景下表现可能完全不同。我建议为不同场景准备多套参数配置，运行时动态切换。
举个例子，我在高速上会把横向采样步长设得大一些（1.0m），因为车道宽，不需要太精细的横向控制。但在城区窄路，步长会缩小到 0.3m，否则车辆会频繁调整方向，乘客会晕车。
4.5 知识体系总览
下面这张图是我自己整理的 Lattice Planner 知识体系，你可以把它当作一个快速参考。
说实话，Lattice Planner 不是万能的。它在结构化道路上表现很好，但在复杂路口和密集车流中，有时会显得力不从心。这时候我会结合行为规划层的决策，比如先决定“我要不要变道”，再让 Lattice Planner 去生成具体的轨迹。
好了，关于 Lattice Planner 的核心内容就这些。记住，理论是基础，但真正让你成长的是在实车调试中踩过的坑。多调几次参数，你就能找到感觉了。