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×420 条。这里有个经验偏移量集合的粒度要根据车速调整。高速时相邻偏移量间隔可以大一些比如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的难点不在算法本身而在代价函数的调参和工程化落地。算法原理半天就能看懂但把行为调得像个老司机需要大量数据回放和反复微调。别指望一次调好做好打持久战的准备。另外五阶多项式虽然经典但也不是万能遇到极端场景比如紧急避障可能需要结合其他规划方法别死守一种方案。