做自动驾驶规划的人绕不过Lattice Planner。就算现在很多团队转向了更复杂的采样方案或者直接走learning-based路线Lattice Planner依然是理解轨迹规划到底在解什么问题最好的教材。我第一次啃开源代码里lattice_planner模块时最先劝退我的不是各种模板类和工厂模式而是一大坨Frenet坐标换算好好的x、y不用为什么非要搞一个s和一个d出来这篇文章打算把这条链路彻底捋一遍参考线怎么定义Frenet坐标Frenet坐标下的轨迹怎么用多项式拟合生成Lattice Planner如何在采样—生成—评分—筛选的循环里跑起来最后给出我在实车和仿真项目里沉淀下来的调参思路与踩坑记录。内容适合刚接手规划模块的工程师也适合那些已经能把轨迹跑起来、但总感觉差点意思的调参选手。1. 先搞懂参考线Frenet坐标系的地基到底怎么打1.1 直角坐标系在道路场景下的尴尬回到最基本的提问为什么不在笛卡尔坐标系里直接做规划在开阔的静态场景里直角坐标系确实够用。但一旦车在高速公路上跑前方是一个接一个的弯道麻烦就来了。车道线在x-y平面上是一条复杂的曲线你想表达车辆保持在车道中间就需要实时计算车道中心线每个点的朝向再把车的横向偏差换算成世界坐标下的约束。更麻烦的是道路结构天然是沿着某个方向延伸的这个方向在弯道里每时每刻都在变用固定不动的x、y轴描述一辆顺着道路走的车等于硬把一条蛇拉直了量长度。Frenet坐标系解决的就是这个不匹配。它把车辆的位姿从全局坐标转换到以参考线为骨架的局部坐标规划问题因此被分解成两个更直觉的子问题纵向——沿着路走了多远、多快横向——偏离路的中心线多远。1.2 参考线如何定义s和dFrenet坐标系的两个坐标量是sstation和dlateral offset它们完全依附于参考线存在s车辆当前位置在参考线上的投影点从参考线起点沿弧长方向量到投影点的距离。d从投影点指向车辆实际位置的有向垂距通常约定参考线行进方向的左侧为正、右侧为负。具体做法是在参考线上找到离车辆位置最近的点作为投影点记参考线在该点的切线方向角为θ单位切线向量为t(cosθ, sinθ)单位左法向量为n(-sinθ, cosθ)。那么从Frenet坐标s, d反算回笛卡尔坐标的关系是x x_r(s) - d * sin(θ(s)) y y_r(s) d * cos(θ(s))其中(x_r(s), y_r(s))是参考线在弧长s处的点。反过来从(x, y)求(s, d)则需要做一次投影即找到一个s使得参考线上点与车辆位置的连线垂直于参考线切线这个s对应的距离就是d。关键点在于整个Frenet坐标系的精度上限取决于参考线的质量。参考线一旦有毛刺投影位置就会抖动s、d跟着跳下游所有轨迹都跟着遭殃。这就是为什么网上搜frenet坐标系和参考线的关系最终都会落到同一个结论——参考线不是规划模块的附属品它就是Frenet坐标系的坐标轴。坐标轴本身不光滑你在坐标系里做的所有运算都不可信。1.3 参考线质量决定Frenet坐标质量实际项目中参考线通常来自两条路径全局路由Routing给出的粗参考线以及局部规划模块自己做的平滑参考线。HD地图里的道路中心线点云往往比较密但曲率不连续路由模块输出的是折线角点处曲率直接突变。这两类输入都不能直接当Frenet坐标系的骨架用。我常用的处理方式是先对参考线做平滑常见方案有两种一是用二次规划做曲率约束下的平滑目标函数里同时惩罚参考线与原始线偏离、相邻点间距变化以及曲率变化率二是用样条曲线对离散点做拟合强制保证C2连续。两种方案我都验证过前者在参考线需要贴合车道中心并避开障碍物时更灵活后者在计算速度上有优势。无论哪种最后都得看一眼曲率曲线是否连续这是最容易偷懒也最容易埋雷的一步。实操提示判断参考线平滑度别只看位置点连起来好不好看直接画出s-曲率曲线。曲率曲线如果像锯齿说明平滑没到位这时候做的Frenet转换、轨迹生成都没意义。2. 多项式拟合的数学本质五次多项式为什么是规划界的默认答案2.1 从边界条件到多项式系数拿到了Frenet坐标系之后Lattice Planner的核心工作就变成给定起点状态和一系列候选终点状态生成一条满足边界条件的轨迹曲线。在Frenet框架下横向运动d(t)和纵向运动s(t)被解耦各自独立拟合。横向轨迹d(t)用五次多项式d(t) a0 a1*t a2*t² a3*t³ a4*t⁴ a5*t⁵为什么是五次因为一条横向轨迹需要满足6个边界条件起点的位置d(0)、速度ḋ(0)、加速度d̈(0)以及终点的位置d(T)、速度ḋ(T)、加速度d̈(T)。六个未知系数对应六个约束方程五次多项式正好是能唯一确定一条满足全部边界条件曲线的最低阶次。更深一层说五次多项式不是拍脑袋选的。在最优控制理论里最小化加速度变化率jerk平方积分min ∫ (d³d/dt³)² dt在给定两端位置、速度、加速度约束的条件下最优解恰好就是五次多项式。这解释了为什么几乎所有基于采样的规划器都把五次多项式当默认曲线——它不是恰好能用而是等价于在追求乘坐舒适性jerk最小化意义上的最优解。纵向轨迹的逻辑相同但终点约束通常少一个。比如巡航场景只约束终点的速度不约束终点位置这时候四阶多项式就够五个系数对应起点s、ṡ、s̈和终点ṡ、s̈五个条件。如果是定点停车场景需要同时给定停车位置和速度为0边界条件变成六个纵向轨迹也得上五次多项式。这个细节很多入门教程没讲透等你自己写代码时会发现系数矩阵维度对不上就是因为在纠结到底用四阶还是五阶。2.2 五次多项式系数的实际求解很多资料把公式摆出来就结束了这里我把求解过程写完整方便你直接照抄。设横向轨迹起终点条件为起点d(0)d0, ḋ(0)ḋ0, d̈(0)d̈0 终点d(T)d1, ḋ(T)ḋ1, d̈(T)d̈1带入五次多项式前三个系数直接由起点条件给出a0 d0 a1 ḋ0 a2 d̈0 / 2剩下三个系数由终点条件联立求解T³*a3 T⁴*a4 T⁵*a5 d1 - d0 - ḋ0*T - 0.5*d̈0*T² 3T²*a3 4T³*a4 5T⁴*a5 ḋ1 - ḋ0 - d̈0*T 6T*a3 12T²*a4 20T³*a5 d̈1 - d̈0这是一个3×3的线性方程组系数矩阵只和时间T有关起终点状态只影响右端项。所以工程实现上可以预先对不同的T求矩阵的逆或者做LU分解运行时只需要做几次乘加运算几百条候选轨迹的系数求解在毫秒级完成。实操提示在实现时建议把系数求解封装成独立函数入参是起点状态、终点状态、时间T出参是六个系数。写完之后一定要做一次反向验证——把算出的系数代回原函数检查t0和tT时的位置、速度、加速度是否和输入一致。这类矩阵求解代码最容易出符号错误反向验证能省下大量调试时间。2.3 多项式拟合做不到的事必须承认多项式拟合只是补间它自己不知道障碍物在哪、不知道路面限速、不知道车辆转弯半径上限。它保证的只有一件事从起点状态平滑过渡到终点状态。所以它生成的曲线可能在中间出现很大的横向位移过冲overshoot也可能在道路曲率大的地方提出一个根本转不过去的弯。所以在Lattice Planner里多项式拟合得出的曲线之后还有两道工序碰撞检测和可行性检查。这两道工序把数学上光滑的曲线变成物理上可执行且安全的轨迹。另外一条多项式曲线只在[t0, T]区间内定义实际使用时需要按固定时间步长通常0.1秒离散成一系列路径点再逐个点判断车辆位姿和周围障碍物的关系。3. Lattice Planner完整流水线从离散采样到轨迹筛选3.1 在Frenet框架下的采样策略Lattice Planner的核心思想是采样终点、拟合曲线、评估选择。它不直接随机生成整条曲线而是把问题简化成规划周期开始时车辆状态已知接下来采样未来某个时刻车辆应该在什么位置、什么速度然后用多项式把当前状态和候选终点连起来。横向采样围绕车道中心线展开。假设车道宽3.5米以车道中心线为基准在[-3.5, 3.5]米范围内每隔0.5米采一条横向目标线就得到15个候选横向终点d值每个终点d的横向速度ḋ和横向加速度d̈都设为0表示车辆到达目标横向位置时已经稳定。纵向采样则根据场景不同分成几类巡航场景采样目标速度比如[0, 1, 2, ..., V_max]跟车场景采样与前方障碍物保持的相对距离定点停车场景采样停车位置。每一组横向目标纵向目标时间T组合起来就是一条候选轨迹的终点状态。这里最关键的参数是时间T。同一个横向目标d0用T2秒和用T8秒拟合出来的轨迹性格完全不同T短意味着车辆激烈地打方向快速回正T长意味着缓缓靠过去。所以Lattice Planner的时间采样不是单值而是在一个范围内分层采样比如2秒到8秒每0.5秒一层。3.2 轨迹生成、坐标回投与离散化拿到一组候选终点状态后用上一节的多项式拟合方法横向、纵向各解出一条曲线再把它们逐点合成。合成不是简单地相加而是利用Frenet与笛卡尔坐标的转换关系把每个时刻的(s(t), d(t))回投到世界坐标系得到(x(t), y(t))和航向角。回投时要用到参考线在s(t)处的切线方向角θ和曲率κ参考线平滑度在这里第三次影响结果。轨迹生成完毕按时间步长离散化。比如时间采样T6秒步长0.1秒一条轨迹就有60个路径点。每个路径点左右要挂上车辆轮廓——常见做法是把车辆包围成沿车身均匀分布的若干个圆用圆和障碍物做距离判断比精确的矩形碰撞检测快得多安全裕量通过圆的半径来控制。3.3 代价函数如何反映驾驶偏好碰撞检测只负责淘汰不能走的轨迹候选轨迹里可能仍有几十条能走但性格不同的代价函数负责给它们排序。典型代价函数由以下几项加权求和代价项含义影响方向横向jerk积分∫(d )²dt值越大转向越猛舒适性越差纵向jerk积分∫(s )²dt值越大加减速越突兀横向偏移代价(d_end)²或∫d²dt惩罚偏离车道中心速度偏差代价(v_end - v_target)²惩罚达不到目标车速时间代价(T - T_expected)²倾向于尽快到达障碍物距离代价1/d_obstacle² 类函数越靠近障碍物代价越高每一项的权重系数直接编码了这辆车在道路上应该表现出的性格。权重调大jerk项轨迹会更平顺但车辆会表现出不愿变道的迟钝感权重调大偏移项车辆会死死贴在车道中心哪怕旁边有障碍物也倾向于硬着头皮走权重调大速度项车辆会尽量顶到目标车速超车、并入场景下表现得激进。评估时还要考虑可行性约束轨迹上任一点曲率不能超过车辆最小转弯半径对应的曲率上限加速度不能超过轮胎附着极限jerk不能超过人体舒适阈值。超出约束的候选轨迹直接丢弃不参与排序。3.4 一次完整的周期流程把上面所有环节串起来一个规划周期大概是这样的获取当前车辆状态位置、速度、加速度、航向角转换到Frenet坐标。确定可行车道考虑变道意图、前方障碍物、交通规则。在s-t平面和d-t平面分别采样终点状态。对每个终点状态做横向、纵向多项式拟合和坐标回投生成完整轨迹。逐条做碰撞检测、可行域约束检查。对剩余轨迹计算代价函数选出代价最低的一条。把选中的轨迹前一小段比如0.5秒交给控制模块执行下一个周期重新规划。这种滚动窗口机制保证车辆总能基于最新环境信息重新规划即使上一个周期的轨迹已经过时影响也只有半个控制周期这正是Lattice Planner在动态环境中能稳定工作的原因。4. 调参这件事参数、权重的物理意义与一套推荐调法4.1 时间域参数规划时域与采样粒度Lattice Planner最核心的时间参数是时域下界T_min和上界T_max以及时间采样步长ΔT。T_min决定车辆最早能在什么时候到达某个横向目标它直接限制了转向的激进程度。T_min设得太小比如1秒车辆会为了快速变道而拉出极大的横向速度jerk爆表T_min设得太大比如4秒碰到近距离障碍物时可能来不及规划出避让轨迹。我的经验是城区场景T_min取2至2.5秒比较平衡高速场景可以适当放大到2.5至3秒因为车速高时小幅横向位移就会带来很大的横摆角速度给车辆留更多时间更安全。T_max决定规划器看得多远。T_max越大规划越有前瞻性但候选轨迹数量也越多计算量线性上涨。更重要的是T_max超过一定值后远期的轨迹点依赖参考线外推参考线一旦有偏差远期轨迹反而不可信。T_max不建议超过10秒高速公路场景8秒就够城区场景6秒是常用值。时间采样步长ΔT影响的是候选轨迹在时域上的分辨率。步长0.5秒意味着每个横向终点会搭配十几条不同时长的纵向轨迹。注意候选数量是乘数关系15条横向线乘以10层时域再乘以8档目标速度轻轻松松上千条轨迹。如果每周期都暴力跑这么多条CPU和延迟都受不了。实际项目里城区简单场景可以把横向采样减到7条时域只采4层速度只取3档先把计算量压下来再逐步加密。4.2 空间采样参数横向范围、横向间距、速度步长横向采样范围和道路宽度强相关。城市单车道宽度约3.5米车道线到路肩还有个缓冲区采样范围取[-3.5, 3.5]米是合理的。但如果是窄路掉头或者园区场景道路宽度不到5米采样范围还取这么宽就会有一半轨迹直接撞路沿白算。我推荐先量一下实际道路宽度把横向采样范围设成比车道宽度单侧多出0.5米。横向采样间距决定车辆能精细到多少厘米地选择横向位置。间距0.5米是大多数场景的甜点值能覆盖车道内大部分可行位置而且候选数量可控。间距调到0.2米会让轨迹更贴合障碍物边缘窄缝通行成功率提高但候选数量增长明显评估阶段计算量涨得很快。间距调到1米以上会漏掉关键路径比如两辆静止障碍物之间只有1.2米的间隙1米间距很可能两边都采不到规划器直接判定无路可走。纵向速度采样步长影响车速选择的精度。步长1米/秒意味着候选速度以3.6公里/小时为粒度对巡航控制够用跟车场景建议细分到0.5米/秒因为前车速度波动时太粗的速度档位会让本车加减速呈阶梯状乘坐体验很差。4.3 代价权重一组可以直接上手的起点值和调整策略代价函数权重没有放之四海皆准的数值但有一个直接可用的起点组合。以下是我在多个项目里验证过的基础权重归一化后权重项归一化起始值调整方向参考横向jerk权重1.0变道太生硬则调大响应太慢则调小纵向jerk权重1.0刹车点头/启动窜动则调大横向偏移代价1.5频繁压线则调大绕障空间不足则调小速度偏差代价1.0车速长期达不到规划值则调大时间代价0.4希望更快到达则调大障碍物距离代价3.0贴近障碍物时感到心慌则调大这套起始值的含义是障碍物距离是安全底线权重先给足横向偏移其次保证正常行驶时车辆不压线jerk权重起舒适性调节作用不要一开始就把舒适性权重拉满否则车辆会变得过于佛系在需要果断变道的场景里犹犹豫豫。调整权重的正确姿势是每次只动一个参数在固定场景下对比前后轨迹差异。我在实践中发现一个特别有用的技巧把每一条候选轨迹的各项代价子项都记录下来画出直方图。如果发现某一项代价在所有候选轨迹上都大得离谱说明该项权重过高或者采样范围本身就不对。比如横向偏移代价普遍偏高可能不是权重问题而是横向采样范围根本没覆盖到车道中心线附近——这种时候调权重是南辕北辙。4.4 一套可落地的调参顺序我总结的调参顺序按重要性排序先修参考线。参考线光滑度不达标后面全白调。画出曲率曲线确认没有锯齿。再定采样边界。根据道路宽度、限速、场景类型确定横向范围、速度范围、时域范围确保采样覆盖所有理论上可走的轨迹。后调可行性约束。最大加速度、最大jerk、最大曲率按车辆手册填写不要凭感觉放大否则选出的轨迹控制模块执行不了。最后调权重。在一个典型场景集上跑仿真对比基准权重和调整后的轨迹差异确认每个权重改变带来的行为变化符合预期。做回归。跑一遍全场景回归测试防止某个权重调好了一个场景却破坏了一批场景。这套顺序的关键逻辑是采样范围决定可选空间约束决定可行空间权重只在可行空间内部排序。如果前面两步没做好权重再怎么调都是矮子里拔将军。5. 实战中躲不开的坑从坐标换算到可视化调试5.1 参考线不光滑导致的规划抖动我踩过最深的坑就是参考线曲率不连续。现象是车辆在直线行驶前方有一个轻微弧度的弯规划出的轨迹会突然往一侧甩一下然后又回正。看代码逻辑完全没问题最后定位到是参考线在某个点处曲率发生了突变Frenet转换后的d值在同一段路上出现不连续多项式拟合被迫用一个跳跃的起点去生成轨迹。排查方法很简单把所有规划输入和输出画到同一张图上包括原始参考线、平滑后参考线、当前车辆的Frenet坐标s, d、规划出的轨迹。只要看到轨迹和参考线之间出现莫名的横向拉扯十有八九是参考线平滑问题。我在这个坑里浪费了整整两天后来养成了先画参考线曲率曲线再谈其他的习惯。5.2 符号约定不一致左右方向的隐形炸弹Frenet坐标的d值正负号约定在不同代码库里不统一。有的约定参考线行进方向左侧为正有的约定右侧为正笛卡尔坐标系的航向角有的用atan2(y, x)相对x轴有的用相对参考线切线方向。这两处一混轨迹会以镜像方式生成看起来方向反了。实车表现是规划器明明想向右变道控制出来的动作却向左。这类问题在仿真里不容易暴露因为左右对称场景下代价几乎一致车会随机选一边一旦上了带语义的真实道路比如必须在右侧行驶问题立刻放大。我的经验是在代码里统一用左侧为正约定并在Frenet与笛卡尔转换函数入口处写一条注释标明约定再用一个简单的起点重合、目标偏移单元测试锁定行为。5.3 离散化太粗导致碰撞检测漏检一条轨迹在数学上连续穿过障碍物但因为离散点间隔过大每个离散点都恰好避开了障碍物碰撞检测返回安全结果实车撞上去。这是典型的tunneling问题。举例车速20米/秒72公里/小时轨迹离散步长0.5秒两个相邻路径点间距10米。一个2米宽的障碍物完全可能落在两个路径点之间圆形车辆包络扫过时恰好擦到它但检测点都没落在障碍物内。解决办法有两个方向一是把时间步长缩小到0.1秒但高速场景下相邻点间距仍有2米不保险二是改用位置步长离散比如每隔0.2米取一个检测点同时对相邻两个路径点做线段与障碍物圆的相交检测。我在项目里两者都做了才把这类漏检清零。5.4 可视化调试三板斧最后分享我认为最有效的三套可视化手段每套都在实际调试中救过我**第一板斧候选轨迹簇全景图。**把所有候选轨迹画在笛卡尔坐标系下按代价从低到高着色低代价绿色、高代价红色。这张图能一眼看出代价函数是不是瞎了眼——比如有一堆轨迹明明在绕远路代价却最低说明权重该调了。**第二板斧s-t图和d-t图。**把选中轨迹的纵向运动画成s-t图横向运动画成d-t图。s-t图的斜率就是速度曲线陡峭说明加速猛d-t图的弯曲程度对应横向jerk。这两张图比笛卡尔轨迹更直观地暴露舒适性问题因为笛卡尔图会把纵向和横向运动混在一起看不清谁在发力。**第三板斧代价分解表。**每周期把最低代价轨迹的各项子代价打成日志。如果某天车辆行为异常拉出日志一看就能定位到底是障碍物距离代价突然升高还是速度偏差代价失控。调参时我也依赖这张表它让每个权重调整都有了可量化的反馈而不是靠感觉好像更顺了。我在实际项目中的体会是Lattice Planner的难点从来不是某个环节的公式有多难而是整条链路每个环节之间的相互影响。参考线影响FrenetFrenet影响拟合拟合影响代价代价影响行为——任何一环的微小误差都会在下一环被放大。所以调试时永远从上游开始查先看参考线再看坐标转换再看轨迹生成最后才是代价权重。按这个顺序走一遍大多数莫名其妙的规划问题都会现出原形。