1. 五阶轨迹规划到底在解决什么问题
1.1 从“能开”到“开得舒服”的分水岭
做自动驾驶决策规划这一块的人，迟早会撞上同一个瓶颈：路径规划给出的结果在几何上是通的，但车坐起来就是不舒服。要么方向盘来回小幅修正，要么过弯时横向加速度忽大忽小，要么在避障绕行时轨迹曲率突变，乘客体验直接崩掉。这个问题的根源，往往不在决策层，而在轨迹规划的平滑度和连续性上。
LatticePlan（栅格/格点规划）是轨迹规划里非常经典的一类方法，它的核心思路是：在Frenet坐标系下，沿纵向和横向分别采样，生成一组候选轨迹，再用代价函数挑出最优的那条。而“五阶”指的是横向和纵向的多项式阶数为五次，也就是用五次多项式去拟合轨迹，保证位置、速度、加速度三个量在起止点都连续。为什么是五阶而不是三阶、七阶？这背后有明确的数学约束，后面会展开算。
这个内容适合谁看：已经了解Frenet坐标系、做过基础路径规划、想深入理解轨迹平滑和采样策略的开发者；也适合正在做决策规划模块、被横向抖动和曲率不连续折磨的同行。我会把五阶多项式的推导、采样策略、代价函数设计、以及实际调试中踩过的坑都摊开讲。
1.2 为什么LatticePlan在工业界一直有生命力
现在端到端、强化学习规划很热，但LatticePlan在量产项目里依然大量存在，原因很实在：可解释、可调试、边界可控。你采样出来的每一条轨迹，横向偏移、纵向速度、加速度都是显式参数，出了问题能定位到具体是哪一项代价权重不对，或者哪个采样区间太粗。相比之下，神经网络输出的轨迹一旦抖动，排查成本极高。
LatticePlan的另一个优势是天然支持多目标约束。避障、跟车、限速、舒适性，全都可以写成代价项加权求和，调参虽然繁琐，但逻辑清晰。对于功能安全要求高的场景，这种“白盒”特性非常关键。
2. 五阶多项式的数学底子与选型逻辑
2.1 从三阶到五阶：阶数是怎么定的
先看一个基本事实：如果只要求起止点的位置和速度连续，三阶多项式就够了，因为三阶有四个系数，正好对应起止位置和起止速度四个约束。但实际乘车体验里，加速度的突变（jerk，加加速度）才是让人晕车的元凶。要让加速度也连续，就需要至少五阶多项式，因为五阶有六个系数，可以满足起止位置、速度、加速度共六个约束。
用公式表达，横向轨迹 ( l(t) ) 关于时间 ( t ) 的五阶多项式：
[ l(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 ]
对时间求一阶导得到横向速度，二阶导得到横向加速度。给定初始状态 ( (l_0, \dot{l}_0, \ddot{l}_0) ) 和终止状态 ( (l_1, \dot{l}_1, \ddot{l}_1) )，以及时间跨度 ( T )，六个方程解六个未知数，系数唯一确定。
那为什么不用七阶？七阶能约束到jerk的导数（snap），理论上更平滑，但计算量上升，而且高阶多项式容易在中间段产生过冲（Runge现象），反而让轨迹偏离预期。实测中五阶已经能把jerk控制在合理范围，性价比最高。这是工业界普遍选五阶的核心原因。
2.2 五次多项式系数的闭式解推导
直接给结论，方便你抄作业。设初始状态向量 ( S_0 = [l_0, \dot{l}_0, \ddot{l}_0] )，终止状态 ( S_1 = [l_1, \dot{l}_1, \ddot{l}_1] )，时间 ( T )。系数解为：
- ( a_0 = l_0 )
- ( a_1 = \dot{l}_0 )
- ( a_2 = \ddot{l}_0 / 2 )
- ( a_3 = (20(l_1-l_0) - (8\dot{l}_1 + 12\dot{l}_0)T - (3\ddot{l}_0 - \ddot{l}_1)T^2) / (2T^3) )
- ( a_4 = (30(l_0-l_1) + (14\dot{l}_1 + 16\dot{l}_0)T + (3\ddot{l}_0 - 2\ddot{l}_1)T^2) / (2T^4) )
- ( a_5 = (12(l_1-l_0) - (6\dot{l}_1 + 6\dot{l}_0)T - (\ddot{l}_0 - \ddot{l}_1)T^2) / (2T^5) )
这套闭式解的好处是不用迭代求解，直接代入即可，单条轨迹生成耗时在微秒级。我实测过，在普通车载算力平台上，采样几百条轨迹加代价评估，单周期耗时可以压在几毫秒内，完全满足10Hz以上的规划频率。
注意：T不能取0，实际代码里要做保护，T小于某个阈值（比如0.5秒）时直接跳过该采样，否则系数会爆炸。
2.3 纵向也用五阶吗
纵向轨迹（s方向）的处理要分场景。如果是跟车或者巡航，纵向通常用四阶或五阶多项式，约束位置、速度、加速度。但在换道、避障这类需要横向纵向协同的场景，纵向一般用四次多项式就够了，因为纵向的jerk容忍度比横向高。不过为了代码统一，很多实现干脆横纵向都用五阶，代价是计算量略增，但逻辑简单不易出错。我倾向于统一用五阶，维护成本低。
3. Frenet坐标系下的采样策略设计
3.1 为什么一定要转到Frenet坐标系
在笛卡尔坐标系下做采样，横向纵向耦合严重，一条轨迹的横向偏移会同时影响纵向进度，代价函数很难写。转到Frenet坐标系后，以道路中心线为参考，纵向s表示沿道路的进度，横向l表示偏离中心线的距离，两者解耦。这样采样就变成：固定一组纵向终点，对每个纵向终点采样一组横向偏移，组合成候选轨迹集合。
这个解耦是LatticePlan高效的根本。你可以独立控制纵向的跟车距离和横向的避障偏移，代价函数也能分开设计，调试时定位问题非常直接。
3.2 横向采样：偏移量和时间双维度
横向采样一般有两个维度：目标横向偏移 ( l_1 ) 和到达时间 ( T )。偏移量通常取 ( [-1.5, -0.75, 0, 0.75, 1.5] ) 米这样的对称集合，覆盖左右换道和保持车道。时间T取 ( [2, 3, 4, 5] ) 秒，对应不同的换道激进程度。
组合起来，横向候选就有 5×4=20 条。这里有个经验：偏移量集合的粒度要根据车速调整。高速时相邻偏移量间隔可以大一些（比如1米），因为高速下小幅横向移动意义不大；低速时（比如拥堵跟车）间隔要细到0.3米，否则避障精度不够。
3.3 纵向采样：巡航、跟车、停车三种模式
纵向采样不是简单撒点，而是按行为模式来。巡航模式下，纵向终点速度设为目标车速，终点位置按当前速度外推；跟车模式下，终点位置设为前车位置减去期望车距，终点速度设为前车速度；停车模式下，终点位置设为停止线，终点速度为0。
每种模式生成若干条纵向轨迹，再和横向轨迹做笛卡尔积，得到完整的候选轨迹集合。这里要注意组合爆炸：横向20条乘纵向10条就是200条，每条都要算代价，计算量不小。实际工程里会做剪枝，比如先按纵向模式筛选，再在每种模式下只保留少数横向候选。
3.4 采样密度与计算量的权衡
采样太稀，可能漏掉最优解；采样太密，算力吃不消。我的经验是：横向偏移采样间隔取0.5米左右，时间采样间隔取1秒，基本能覆盖绝大多数场景。如果算力充裕，可以在障碍物密集区域动态加密采样，在空旷路段稀疏采样。这种自适应采样策略能把计算量降低30%以上，同时不牺牲安全性。
4. 代价函数设计：决定轨迹好坏的关键
4.1 代价项的构成与物理意义
代价函数是LatticePlan的灵魂，直接决定选出来的轨迹像不像老司机。常见的代价项包括：
| 代价项 | 物理意义 | 典型权重 |
|---|---|---|
| 横向偏移代价 | 偏离车道中心的程度 | 中 |
| 横向速度代价 | 横向移动的激烈程度 | 中 |
| 横向加速度代价 | 乘坐舒适性 | 高 |
| 纵向加速度代价 | 加减速平顺性 | 高 |
| jerk代价 | 加加速度，晕车元凶 | 高 |
| 障碍物距离代价 | 安全性 | 极高 |
| 到达目标代价 | 是否接近期望终点 | 中 |
| 速度保持代价 | 是否维持期望车速 | 中 |
每个代价项都是轨迹状态的函数，加权求和得到总代价。权重需要反复调试，没有万能值，但有个原则：安全相关权重必须远大于舒适性权重，舒适性权重又大于效率权重。
4.2 权重调参的实战经验
调权重是个体力活，但有几个技巧能少走弯路。第一，先把障碍物代价权重设到极大，保证任何情况下都不会撞，然后再逐步降低到合理值。第二，横向加速度和jerk的权重不要一开始就设很高，否则轨迹会过于保守，换道磨磨蹭蹭。第三，用同一段路测数据反复回放，观察不同权重下选出的轨迹差异，找到那个“既不激进也不肉”的平衡点。
我踩过的一个坑：早期把横向偏移代价权重设得过高，导致车辆死活不肯偏离车道中心去避障，明明旁边有空间也不走。后来把偏移代价降下来，障碍物代价提上去，行为就正常了。这说明权重之间是相互制约的，不能孤立地看。
4.3 代价归一化的重要性
不同代价项的量纲不一样，横向偏移是米，加速度是米每二次方秒，jerk是米每三次方秒。直接加权求和，量纲大的项会主导结果。所以每项代价都要做归一化，除以一个参考值，变成无量纲的分数。参考值可以取该项在典型场景下的最大值，或者用统计方法确定。归一化之后，权重才有可比性，调参才有意义。
5. 完整实操流程与关键代码环节
5.1 整体流程梳理
一个完整的LatticePlan周期大致分六步：
- 接收上游的参考线、障碍物信息、自车状态、决策指令。
- 将自车和障碍物投影到Frenet坐标系。
- 按行为模式生成纵向采样集合。
- 对每个纵向采样，生成横向采样集合，组合成候选轨迹。
- 对每条候选轨迹计算代价，做碰撞检测。
- 选出代价最低且无碰撞的轨迹，输出给控制模块。
这个流程在10Hz的规划周期内完成，意味着每100毫秒要跑完一轮。代码效率很关键，尤其是碰撞检测和代价计算，要用向量化操作，避免Python式的循环。
5.2 五次多项式生成的代码实现
用Python示意核心逻辑：
import numpy as np def quintic_coeffs(s0, s1, T): # s0, s1 为 [pos, vel, acc] p0, v0, a0 = s0 p1, v1, a1 = s1 a0_c = p0 a1_c = v0 a2_c = a0 / 2.0 a3_c = (20*(p1-p0) - (8*v1 + 12*v0)*T - (3*a0 - a1)*T**2) / (2*T**3) a4_c = (30*(p0-p1) + (14*v1 + 16*v0)*T + (3*a0 - 2*a1)*T**2) / (2*T**4) a5_c = (12*(p1-p0) - (6*v1 + 6*v0)*T - (a0 - a1)*T**2) / (2*T**5) return np.array([a0_c, a1_c, a2_c, a3_c, a4_c, a5_c]) def eval_quintic(coeffs, t): # 返回位置、速度、加速度 c = coeffs pos = c[0] + c[1]*t + c[2]*t**2 + c[3]*t**3 + c[4]*t**4 + c[5]*t**5 vel = c[1] + 2*c[2]*t + 3*c[3]*t**2 + 4*c[4]*t**3 + 5*c[5]*t**4 acc = 2*c[2] + 6*c[3]*t + 12*c[4]*t**2 + 20*c[5]*t**3 return pos, vel, acc这段代码是核心，采样时对每个候选终点调用一次quintic_coeffs，然后按固定时间步长（比如0.1秒）调用eval_quintic得到离散轨迹点。离散点用于碰撞检测和代价计算。
5.3 碰撞检测的加速技巧
碰撞检测是性能瓶颈。最朴素的做法是把自车和障碍物都当成矩形，做分离轴定理检测。但200条轨迹乘上几十个障碍物，计算量爆炸。实际工程里用几种加速手段：
- 用包围圆代替矩形做粗筛，快速排除明显不碰撞的轨迹。
- 对障碍物做空间索引，只检测轨迹附近一定范围内的障碍物。
- 轨迹点做降采样，比如每隔0.2秒取一个点做检测，而不是每个0.1秒的点都测。
我实测下来，这几种手段叠加能把碰撞检测耗时降低一个数量级，从十几毫秒降到一两毫秒。
5.4 轨迹选择与后处理
选出最优轨迹后，不能直接丢给控制。要做几件事：检查轨迹的曲率是否超过车辆物理极限，如果超过要降速或者重新规划；检查轨迹末端是否平滑衔接下一周期，避免周期切换时抖动；对轨迹做时间重采样，保证输出给控制的点间隔均匀。
提示：轨迹末端衔接是个容易被忽略的坑。如果每个周期独立规划，末端状态不连续，控制模块会感受到跳变。解决办法是在代价函数里加一项末端状态平滑代价，惩罚与上一周期末端状态的偏差。
6. 常见问题与排查技巧实录
6.1 轨迹抖动与曲率不连续
这是最常见的投诉。表现是方向盘小幅高频修正，或者过弯时轨迹曲率有台阶。排查思路：
- 先看采样密度，横向偏移间隔太大，最优解在两条候选之间跳来跳去，就会抖。加密采样或者加平滑滤波。
- 再看代价函数，如果横向加速度权重太低，选出的轨迹可能曲率变化剧烈。提高权重试试。
- 最后看周期衔接，末端状态不连续也会导致抖动，加末端平滑代价。
6.2 避障时轨迹过于保守或激进
保守表现为明明有空间却不敢走，激进表现为贴着障碍物过。根因都在障碍物代价函数的设计。代价函数通常用距离的反比或者指数衰减，参数没调好就会走极端。我的经验是：用分段函数，安全距离内代价急剧上升，安全距离外代价平缓下降，这样既有安全边界又不至于过度保守。
6.3 计算超时导致规划失败
规划周期100毫秒，如果某次计算超时，输出不了轨迹，控制就会降级。超时通常发生在障碍物特别多或者采样特别密的时候。解决办法：设一个候选轨迹数量上限，超过就按优先级裁剪；碰撞检测做提前退出，一旦发现碰撞立即丢弃该轨迹，不继续算代价；用多线程并行计算代价，充分利用多核。
6.4 常见问题速查表
| 问题现象 | 可能原因 | 排查方向 |
|---|---|---|
| 方向盘高频抖动 | 采样太稀或末端不连续 | 加密采样、加末端平滑代价 |
| 换道磨蹭 | 横向加速度权重过高 | 降低权重、增大采样时间跨度 |
| 避障贴太近 | 障碍物代价衰减太慢 | 调整代价函数衰减参数 |
| 规划超时 | 候选轨迹过多 | 限制数量、并行计算、提前退出 |
| 轨迹曲率超限 | 未做曲率约束 | 加曲率检查与降速逻辑 |
| 周期切换跳变 | 末端状态不连续 | 加末端状态平滑代价 |
6.5 几个独家避坑技巧
第一，采样集合不要用固定值，要根据车速和道路曲率动态调整。直道高速时横向采样可以粗，弯道低速时要细。第二，代价函数里的jerk项不要直接算三阶导，数值微分噪声大，用解析式算更稳。第三，调试时把每条候选轨迹都可视化出来，用不同颜色标代价高低，一眼就能看出代价函数哪里不合理。第四，保留一份“黄金轨迹”数据集，每次改代码后回放对比，防止改出新问题。
7. 性能优化与工程化落地建议
7.1 从原型到量产的差距
实验室里跑通LatticePlan不难，难的是在车载嵌入式平台上稳定跑。原型阶段用Python或者MATLAB，量产要转C++，还要考虑内存分配、浮点精度、实时性。我的建议是原型阶段就把算法结构设计好，把计算密集部分抽象成独立函数，方便后续移植。别等到移植时才发现代码耦合太深，改不动。
7.2 内存与实时性优化
车载平台内存有限，规划模块不能频繁动态分配内存。候选轨迹的存储要用预分配的内存池，避免运行时malloc。浮点运算尽量用单精度，除非精度不够。循环展开、SIMD指令这些底层优化，在算力紧张的平台上能带来明显提升。我做过对比，同样的算法，优化前后耗时能差两三倍。
7.3 与其他模块的接口设计
LatticePlan的输入输出接口要稳定。输入包括参考线、障碍物列表、自车状态、决策指令，输出是轨迹点序列加时间戳。接口设计要预留扩展字段，比如后续加个“轨迹置信度”或者“规划模式标记”，不用改接口定义。接口不稳定是工程化的大忌，上下游模块会跟着遭殃。
7.4 测试与验证策略
规划模块的测试分三层：单元测试验证五次多项式系数计算正确；集成测试验证采样、代价、碰撞检测流程通畅；场景测试用真实路测数据回放，验证行为合理。场景测试最重要，要覆盖跟车、换道、避障、停车、拥堵等典型场景，每个场景准备多组数据，观察轨迹是否稳定、舒适、安全。
我个人在实际操作中的体会是，LatticePlan的难点不在算法本身，而在代价函数的调参和工程化落地。算法原理半天就能看懂，但把行为调得像个老司机，需要大量数据回放和反复微调。别指望一次调好，做好打持久战的准备。另外，五阶多项式虽然经典，但也不是万能，遇到极端场景（比如紧急避障）可能需要结合其他规划方法，别死守一种方案。
