做自动驾驶规划的同学最后大概率都会遇到Lattice Planner。我第一次听到这个词时以为是什么高深莫测的黑科技结果发现它本质就一句话在Frenet坐标系下撒一批由多项式拟合出来的候选轨迹再按代价函数挑一条最顺眼的。真正动手去改参数、看实车表现之后才发现难的不是算法流程而是坐标系理解和调参的手感。这篇文章把我从零跑通Lattice Planner的经验写出来覆盖Frenet坐标系与参考线的关系、多项式拟合细节、采样与代价设计最后是大家最关心的调参指南。适合刚入门运动规划、或者已经在用开源方案但对参数含义一知半解的同学。1. 先说清楚Frenet坐标系和参考线到底是什么关系1.1 为什么规划层要“自找麻烦”换个坐标系我们平时最习惯的是笛卡尔坐标系x、y一标位置清清楚楚。但放在自动驾驶的规划场景里笛卡尔坐标系有一个很尴尬的问题道路是弯的车道线不是横平竖直的直线你很难用一组x-y表达式去描述“车辆离车道中心线多少米”。如果硬要在笛卡尔坐标系下生成轨迹那“不压线”这个约束会变得很复杂每个位置的可行区域都要去地图里查查完还得算距离整套逻辑绕来绕去。Frenet坐标系干脆换了个思路把“沿路走多远”和“偏离参考线多少”拆成两个独立变量。s表示沿参考线的弧长d表示到参考线的有向距离通常左侧为正、右侧为负。这样一来换道问题就变成了“d从0变到3.5米”跟车巡航问题就变成了“s以什么速度前进”两个维度可以分别做规划再合并成实际轨迹。这种解耦大大简化了问题表达也让Lattice Planner这种“采样拟合”的思路成为可能。1.2 参考线Frenet坐标系的“地基”没有参考线就没有Frenet坐标系。参考线可以来自高精地图的车道中心线、导航路径的平滑结果甚至在某些场景下用上一时刻规划出来的轨迹当参考线。但在实车应用里参考线通常是一串离散点直接拿来做投影会出问题离散点一个微小的抖动就会让车辆在Frenet系下的d值产生明显波动最终导致规划轨迹左右跳。所以工程上一般会对参考线做平滑处理。做得比较多的是离散点平滑、B样条拟合、五次多项式分段平滑这几类目标都是让参考线具备连续的曲率保证投影时“最近点”搜索稳定。我的经验是参考线平滑度对最终轨迹的影响往往比后面要讲的代价权重还大。你想想坐标系的x轴都在抖你在这套坐标系里规划出来的曲线怎么可能稳1.3 用生活类比理解s和d可以把参考线想象成一条公路的里程标s就是“你已经开了多少公里”d就是“你离右侧车道线有多少米”。在直道上你可能会觉得Frenet坐标系是脱裤子放屁但到了盘山路上这种解耦的优势就非常明显你只需要让d保持在车道边界内再让s根据前方车辆和限速变化规划问题就从“二维曲线”变成了“两个一维问题”计算和调试都友好得多。实际计算时每个规划周期第一步就是把车辆位置和障碍物位置投影到Frenet坐标系。投影的核心是“在参考线上找最近点”该点的弧长就是s车辆到最近点的有向距离就是d。这个最近点搜索是高频操作工程上一般会用KD树或者预计算的累积弧长表加速。如果参考线本身是用样条表达的还可以用牛顿迭代细化。刚开始看代码的同学容易在这里迷路看到d的一堆导数分不清是对s求导还是对t求导。我的建议是先记住结论——纵向规划通常直接用s(t)横向规划既可以用d(t)也可以用d(s)但不管哪种最后拼轨迹时要保证横向和纵向在时间轴上对齐否则会出现“横向已经换完道纵向还没到位”的割裂感。2. 多项式拟合一条轨迹是怎么长出来的2.1 为什么要选五次多项式确定了Frenet坐标系下的起点状态和终点状态接下来要在两个状态之间生成一条连续、平滑的轨迹。这个“连接”动作工程上最常用的是多项式拟合。最简单的自然是三次多项式位置、速度四个边界条件刚好能定下四个系数。但三次多项式只能保证位置和速度连续加速度在端点处可能突变坐车的人会觉得像被人踹了一脚。五次多项式是另一个经典选择d(t) a0 a1t a2t^2 a3t^3 a4t^4 a5*t^5它一共有6个系数正好能匹配“初始位置、初始速度、初始加速度、终点位置、终点速度、终点加速度”这6个边界条件。用了五次多项式之后轨迹内部的三阶导——也就是jerk加加速度是连续的端点处的加速度也能主动约束舒适性明显提升。高次多项式理论上也能用但实际中容易产生多余振荡数值稳定性也不好所以工程上五次是绝对主流。2.2 纵向与横向轨迹的边界条件纵向轨迹s(t)的起点来自当前车辆状态包括位置s0、速度ds0、加速度dds0。终点则来自行为决策层前方没车就朝着目标巡航速度跑前方有静止障碍就设定终点的速度为0让车稳稳停住。如果终点时间T也参与采样那么每一个(T, s1, ds1)组合都会生成一条不同的纵向轨迹。横向轨迹d(t)的起点是当前车辆距离参考线的偏差终点是目标车道中心线的位置。变道完成后我们通常希望横向速度、横向加速度都为0否则车会在车道里飘。所以横向轨迹的终点条件一般写为d1目标车道偏移量dd10ddd10。这里有个容易忽视的点横向和纵向的T必须保持同一组值否则时间轴对不上。比如纵向用T6秒、横向用T4秒拼出来的轨迹就是灾难现场。2.3 六元一次方程组别自己手推直接求解很多人一看五次多项式就想背系数公式我的建议是别背。你只需要把边界条件拼成一个线性方程组让Eigen、NumPy这类库去解。初始t0时方程很直观a0等于初始位置a1等于初始速度2*a2等于初始加速度。终点tT时把T代入五次多项式和它的导数表达式再令它们等于终点位置、速度、加速度就行。六行六列求逆即得。import numpy as np def quintic_coeffs(y0, dy0, ddy0, y1, dy1, ddy1, T): A np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5], [0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], [0, 0, 2, 6*T, 12*T**2, 20*T**3] ]) b np.array([y0, dy0, ddy0, y1, dy1, ddy1]) return np.linalg.solve(A, b)有一个数值细节值得专门说T的量级一般在1到10秒T的五次方可能到100000矩阵条件数会很大小扰动下系数会抖。工程上更稳的做法是令taut/T先求多项式关于tau的系数再换算回t。我在实车上吃过这个亏同样的采样未归一化时偶尔会冒出诡异的轨迹归一化之后症状消失。所以建议你从一开始就写个带时间归一化的工具函数别图省事。3. Lattice Planner核心流程采样、检查、选优3.1 在Frenet空间里“撒点”Lattice Planner的核心思想是“采样选择”。每个规划周期算法会根据行为决策给出的目标——巡航、跟车、停车、换道——确定一个采样空间然后在空间里撒一批候选终点再结合当前起点状态用多项式拟合出轨迹。横向采样通常是在当前车道中心线和目标车道中心线附近取离散的d值。比如左右各取-3.5米、-3.0米、-2.5米……一直到3.5米间隔0.5米或1米。纵向采样则围绕“当前车速在T秒内能跑多远”来取比如T取4秒、5秒、6秒、7秒、8秒对应每个T再取几个不同的终点速度或终点位移。每个“横向目标d1 纵向目标s1 终点时间T 终点速度ds1”的组合就是一个候选轨迹端点。采样密度直接决定算法效果。太密候选轨迹数量暴涨计算量顶不住而且相邻轨迹的代价很接近最后挑出来的轨迹容易来回跳太疏又可能漏掉最优解。我在结构化道路上常用的做法是横向间隔1米靠近障碍物或变道场景时局部加密到0.5米时间间隔1秒左右先粗后细跑起来之后再根据场景调整。3.2 可行性筛查先过滤再评价生成的候选轨迹不能直接进代价函数必须先做一轮可行性筛查。这步如果漏了后面代价函数再漂亮选出来的轨迹也可能让车辆直接失控。筛查主要分三块。一是运动学可行性也就是每个离散点上的曲率不能超过车辆最小转弯半径对应的曲率上限速度不能超过车辆极限。二是舒适性加速度、加加速度要控制在合理阈值内比如纵向加速度一般限制在2到3 m/s^2加加速度限制在1到2 m/s^3具体看车型和乘坐感受。三是碰撞检测把轨迹上的车辆轮廓用多边形或圆包络表示与障碍物在每个时刻的位置做相交测试。因为规划周期通常是10Hz甚至20Hz所有候选轨迹要在几十毫秒内筛完所以工程上会用“先粗筛后细检测”的策略先用AABB包围盒快速排除明显碰撞的轨迹再对剩下的一小批做精确多边形碰撞。还有一个很容易踩的坑是碰撞检测必须结合时间轴来做。某个障碍物这会在你前面下个时刻可能就到侧面了如果你只检查终点时刻是否碰撞很有可能选出一条中间过程怼上障碍物的轨迹。3.3 代价函数各项之间怎么平衡筛选之后可能还剩几十条轨迹最终挑哪条靠的是代价函数。典型代价包括横向偏移量、换道次数、纵向加加速度平方和、向心加速度、总时间、终点速度误差、与障碍物的安全距离裕度等等。每一项前面的权重就是调参时最常动的东西。我踩过最大的坑是权重量纲不统一。横向偏移的量级是米加加速度的量级是m/s^3速度误差的量级是m/s它们平方之后数值范围完全不同直接乘权重会导致某一项彻底压过其它项。比如横向偏移0.1米平方后是0.01加加速度0.5平方后是0.25如果两者权重都设为1最终轨迹会拼命压低加加速度宁可绕远路也不肯做一点大动作。所以写代码时最好先把每一项做归一化或者用“平方和的均值”再乘权重这样调参才可控。还要特别强调一点碰撞风险不要放进可交换的代价里应该作为硬约束。如果你把“离障碍物太近”也变成一项小代价算法完全可能选一条擦着障碍物边缘但其它项很优的轨迹。安全红线不能跟舒适性、效率放在同一个天平上博弈。4. 调参指南重点4.1 几个核心参数及其直观影响调参的第一步是知道每个参数在动什么。我整理了一张表方便你对着排查参数含义调大后的表现调小后的表现参考方向横向采样间隔候选轨迹偏离中心线的密度轨迹更丰富但计算量增大相邻轨迹差异小候选少可能漏掉贴边绕障的轨迹普通道路1m复杂场景0.5m纵向采样距离间隔终点位置分布密度停车/跟车位置选择更细腻位置选择粗糙可能急刹结合车速动态调整终点时间范围T完成规划的时间跨度轨迹更平缓、舒适但响应慢动作更激进容易产生大jerk低速场景拉大高速场景收窄横向偏移代价权重抑制偏离参考线的能力轨迹更贴中心线不爱变道轨迹更愿意绕行和贴边过大容易误伤避障轨迹加加速度权重对jerk的惩罚加速/减速更柔和起步刹车更果断但体感差乘用车舒适阈值约1-2总时间权重对到达时间的惩罚更快到达但可能牺牲舒适更佛系容易跟车过慢按场景限速取平衡曲率上限车辆可执行性约束允许更急的弯但可能超车辆极限更保守绕路概率增大根据车型最小转弯半径安全距离阈值碰撞检测的膨胀距离更安全但窄路通行失败率上升更激进容易贴障碍物不小于车辆半宽余量这张表不是让你照抄而是帮你建立“动哪个参数会有什么反应”的直觉。实际项目里参数之间是耦合的经常出现动A没效果、动B反而把A的问题解决了的情况。4.2 常见问题与排查思路调试Lattice Planner的过程基本就是反复和“不听话的轨迹”作斗争。我归纳了下面几个高频问题每条都附了优先排查方向现象可能原因优先排查/解决轨迹横向左右抖参考线不平滑、横向采样过密、横向偏移代价过小先平滑参考线再调大横向偏移权重最后考虑降低采样密度纵向一窜一窜终点时间T范围过小、加加速度权重低、速度采样步长过大拉大T上限提高jerk权重缩小速度采样步长变道太频繁缺少换道惩罚、横向偏移代价过小在代价里加“换道次数”惩罚适当提高横向偏移权重刹不住或停车点太远终点速度没设0、纵向采样距离不够、T_max太小检查终点速度采样增加纵向位置采样范围拉大T_max轨迹贴着障碍物走安全距离阈值太小、碰撞检测只查终点增大膨胀距离确保逐时刻碰撞检测每帧轨迹不连续参考线抖动、时间一致性处理不好平滑参考线保存上一帧轨迹并做拼接或限制选择范围低速大转角时曲率爆表曲率筛查没生效、参考线本身曲率过大确认曲率检查在可行性筛查里重新平滑参考线这些都是我实际调车时遇到过的不是抄文档抄出来的。特别说一下“每帧轨迹不连续”这个问题如果你的规划模块是10Hz跑上一帧选了变道轨迹下一帧因为采样噪声换了一条更靠左的轨迹车身就会猛拉一下。解决办法是给代价函数加入“与上一帧轨迹的一致性项”或者限制每一帧只能在上一帧轨迹附近一定范围内选。这个坑越早处理越省心。4.3 我踩过的几个调参大坑第一个坑是参考线不平滑。我最初在仿真里用高精地图的原始点列当参考线车辆在弯道里轨迹一直抖我还以为是采样密度问题折腾了两天才发现是参考线里有两个点位置差了一厘米导致投影出来的d在小范围内突变。平滑参考线之后同样一套代价参数轨迹突然就乖了。所以如果你发现轨迹莫名抖动第一反应应该去看参考线而不是调权重。第二个坑是同时改太多参数。有一阵子我为了追求舒适度同时把加加速度权重、横向偏移权重、末端时间范围全改了结果轨迹确实平缓了但变道变得非常磨叽。后来我一条一条参数回退才发现是时间范围拉得太大导致的。调参这件事必须一次只动一个量改完跑同一组基准场景记录轨迹数据和体感再决定下一步。第三个坑是照搬开源工程里的权重。Apollo这类开源方案给的默认权重大概率是测试过很多场景的但那是基于特定车型、特定参考线质量调出来的。你的传感器延迟、控制精度、车辆轴距都不一样权重直接搬过来往往水土不服。我建议把默认权重当成“初始值”然后围绕你自己的典型场景重新标定。5. 写给后来人的几条实战心得调参本质是在车辆动力学、乘员舒适性和场景通过性之间找平衡。没有一套参数能通吃所有路况所以工程实现上最好做成“场景-参数集”的映射不同场景加载不同权重和采样配置而不是全网一套参数硬跑。我个人习惯的做法是先把结构稳定下来再碰权重。也就是先保证参考线平滑、采样空间合理、可行性筛查完整确认这几层不出问题后才去调代价函数里的数字。如果轨迹已经出现明显压线、碰撞、突变那绝对不是权重问题前面哪一层一定漏了。最后分享一个小技巧给每个候选轨迹打一个可视化的代价分解标签。做调试时把横向代价、jerk代价、时间代价分开画在图上而不是只看最终选中的那条轨迹。你会很快发现是某一项代价在悄悄左右结果定位问题的速度能快好几倍。Lattice Planner没那么玄把坐标系、拟合、采样、评价这条链路理解透它就是一个你随时可以按需调整参数的工具。