企业官网建设流程全解析

做自动驾驶规划的人，绕不过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和d

Frenet坐标系的两个坐标量是s（station）和d（lateral 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，出参是六个系数。写完之后一定要做一次反向验证——把算出的系数代回原函数，检查t=0和t=T时的位置、速度、加速度是否和输入一致。这类矩阵求解代码最容易出符号错误，反向验证能省下大量调试时间。

2.3 多项式拟合做不到的事

必须承认，多项式拟合只是"补间"，它自己不知道障碍物在哪、不知道路面限速、不知道车辆转弯半径上限。它保证的只有一件事：从起点状态平滑过渡到终点状态。所以它生成的曲线可能在中间出现很大的横向位移过冲（overshoot），也可能在道路曲率大的地方提出一个根本转不过去的弯。

所以在Lattice Planner里，多项式拟合得出的曲线之后还有两道工序：碰撞检测和可行性检查。这两道工序把"数学上光滑"的曲线变成"物理上可执行且安全"的轨迹。另外，一条多项式曲线只在[t=0, 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。同一个横向目标d=0，用T=2秒和用T=8秒拟合出来的轨迹性格完全不同：T短意味着车辆激烈地打方向快速回正，T长意味着缓缓靠过去。所以Lattice Planner的时间采样不是单值，而是在一个范围内分层采样，比如2秒到8秒每0.5秒一层。

3.2 轨迹生成、坐标回投与离散化

拿到一组候选终点状态后，用上一节的多项式拟合方法，横向、纵向各解出一条曲线，再把它们逐点合成。合成不是简单地相加，而是利用Frenet与笛卡尔坐标的转换关系，把每个时刻的(s(t), d(t))回投到世界坐标系，得到(x(t), y(t))和航向角。回投时要用到参考线在s(t)处的切线方向角θ和曲率κ，参考线平滑度在这里第三次影响结果。

轨迹生成完毕，按时间步长离散化。比如时间采样T=6秒，步长0.1秒，一条轨迹就有60个路径点。每个路径点左右要挂上车辆轮廓——常见做法是把车辆包围成沿车身均匀分布的若干个圆，用圆和障碍物做距离判断，比精确的矩形碰撞检测快得多，安全裕量通过圆的半径来控制。

3.3 代价函数如何反映驾驶偏好

碰撞检测只负责淘汰"不能走"的轨迹，候选轨迹里可能仍有几十条"能走但性格不同"的，代价函数负责给它们排序。典型代价函数由以下几项加权求和：

每一项的权重系数直接编码了"这辆车在道路上应该表现出的性格"。权重调大jerk项，轨迹会更"平顺"，但车辆会表现出不愿变道的迟钝感；权重调大偏移项，车辆会死死贴在车道中心，哪怕旁边有障碍物也倾向于硬着头皮走；权重调大速度项，车辆会尽量顶到目标车速，超车、并入场景下表现得激进。

评估时还要考虑可行性约束：轨迹上任一点曲率不能超过车辆最小转弯半径对应的曲率上限，加速度不能超过轮胎附着极限，jerk不能超过人体舒适阈值。超出约束的候选轨迹直接丢弃，不参与排序。

3.4 一次完整的周期流程

把上面所有环节串起来，一个规划周期大概是这样的：

1. 获取当前车辆状态（位置、速度、加速度、航向角），转换到Frenet坐标。
2. 确定可行车道（考虑变道意图、前方障碍物、交通规则）。
3. 在s-t平面和d-t平面分别采样终点状态。
4. 对每个终点状态做横向、纵向多项式拟合和坐标回投，生成完整轨迹。
5. 逐条做碰撞检测、可行域约束检查。
6. 对剩余轨迹计算代价函数，选出代价最低的一条。
7. 把选中的轨迹前一小段（比如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权重起舒适性调节作用，不要一开始就把舒适性权重拉满，否则车辆会变得"过于佛系"，在需要果断变道的场景里犹犹豫豫。

调整权重的正确姿势是每次只动一个参数，在固定场景下对比前后轨迹差异。我在实践中发现一个特别有用的技巧：把每一条候选轨迹的各项代价子项都记录下来，画出直方图。如果发现某一项代价在所有候选轨迹上都大得离谱，说明该项权重过高，或者采样范围本身就不对。比如横向偏移代价普遍偏高，可能不是权重问题，而是横向采样范围根本没覆盖到车道中心线附近——这种时候调权重是南辕北辙。

4.4 一套可落地的调参顺序

我总结的调参顺序，按重要性排序：

1. 先修参考线。参考线光滑度不达标，后面全白调。画出曲率曲线确认没有锯齿。
2. 再定采样边界。根据道路宽度、限速、场景类型确定横向范围、速度范围、时域范围，确保采样覆盖所有"理论上可走的轨迹"。
3. 后调可行性约束。最大加速度、最大jerk、最大曲率按车辆手册填写，不要凭感觉放大，否则选出的轨迹控制模块执行不了。
4. 最后调权重。在一个典型场景集上跑仿真，对比基准权重和调整后的轨迹差异，确认每个权重改变带来的行为变化符合预期。
5. 做回归。跑一遍全场景回归测试，防止某个权重调好了一个场景却破坏了一批场景。

这套顺序的关键逻辑是：采样范围决定"可选空间"，约束决定"可行空间"，权重只在可行空间内部排序。如果前面两步没做好，权重再怎么调都是矮子里拔将军。

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的难点从来不是某个环节的公式有多难，而是整条链路每个环节之间的相互影响。参考线影响Frenet，Frenet影响拟合，拟合影响代价，代价影响行为——任何一环的微小误差都会在下一环被放大。所以调试时永远从上游开始查：先看参考线，再看坐标转换，再看轨迹生成，最后才是代价权重。按这个顺序走一遍，大多数"莫名其妙"的规划问题都会现出原形。