企业官网建设流程全解析

做自动驾驶规划绕不开Lattice Planner，而Lattice规划里最劝退新手的往往不是采样和代价函数，反而是它前置的三个基础概念：参考线、Frenet标架、多项式拟合。很多资料一上来就甩公式，我当年也是一边看一边怀疑自己是不是数学没学好。这个“上篇”专门把这几个地基概念拆开揉碎，不讲虚的，直接讲它们是什么、为什么需要、以及你手写代码时会踩到哪些坑。适合刚接触路径规划、想自己从零撸一套规划器的人，也适合想复习一遍经典Lattice框架的工程师。

1. 整体设计与思路拆解

1.1 Lattice规划到底在干什么

先说人话。Lattice规划解决的是“在一个结构化环境里，从当前状态出发，未来几秒内车应该怎么走”的问题。它把空间和时间切成格子（Lattice本意就是晶格），在每个格子里生成候选轨迹，然后用代价函数打分，挑一条最优的。整体框架不复杂，复杂的是“怎么把车的位置和速度放到一个好算的坐标系里”。

这就要聊到Lattice的经典思路了：车辆运动不管怎么复杂，都可以分解成两个方向的运动——沿着道路方向跑多远（纵向），以及偏离道路中心多少（横向）。纵向决定“什么时候到哪”，横向决定“走哪条车道线、避不避障”。两个方向解耦以后，规划问题就从“二维曲线搜索”降维成“两个一维曲线的生成”，计算量和可解释性都好了非常多。

1.2 三个核心概念为什么缺一不可

参考线是Lattice的地基。车辆不是在一张白纸上开，而是在道路环境里开，总得有个“理想路径”作为参照系。这个参照系可以是车道中心线、导航路径投影，也可以是实时感知生成的可通行走廊中心线。没有参考线，横向偏移d就没有参照物，后面全崩。

Frenet标架是这个框架的坐标系统。它的核心思想是：不站在地面坐标系看车，而是站在参考线上看车。车的位置用两个量表示，沿参考线走过的弧长s，和偏离参考线的距离d。这个坐标系对道路场景极其友好，因为道路的几何形状被“拉直”了，约束条件变得非常直观。

多项式拟合则是“画轨迹”的数学工具。有了起点和终点状态，怎么把一条连续、平滑、满足车辆运动学约束的轨迹表达式写出来？工程上最常用的就是多项式，尤其是四次和五次多项式。系数少、求导方便、边界约束可以直接代数求解，是Lattice轨迹生成里性价比最高的选择。

这三个概念是一条完整的链路：参考线给坐标系提供骨架，Frenet标架让状态描述变得合理，多项式拟合负责把状态边界条件变成一条可执行的平滑曲线。搞清楚这条链路，你再看Lattice采样、代价评估、碰撞检测，逻辑就会顺很多。

2. 参考线：整个规划的骨架

2.1 参考线从哪来、长什么样

参考线的来源通常有三类：全局导航路径、高精地图车道中心线，以及实时局部规划产生的可通行路径。无论哪一类，最后都要变成一条离散点序列，比如每隔0.5米或1米一个点，每个点包含位置坐标、朝向角、曲率信息。这就是给Frenet标架用的原始输入。

这里有个容易被忽略的问题：全局路径往往是粗糙的，直接拿来做参考线，曲率可能到处跳变，甚至出现不连续的折角。Frenet坐标转换依赖在参考线上找投影点，如果参考线曲率突变，投影点可能跳变、多解，坐标转换结果就会抖得没法用。所以参考线必须先做平滑处理。

平滑的方法有好几种。简单的是滑动平均或者B样条拟合，复杂一点的做法是构造一个优化问题：目标函数里同时考虑“贴合原始路径”和“曲率尽量小、曲率变化率尽量小”，用二次规划或者梯度下降求解。工程上常用的是分段多项式加约束的平滑，每个点之间保证位置连续、朝向连续、曲率连续。

2.2 为什么平滑是参考线的生命线

你可能会想，参考线稍微不平滑就怎么样？实际影响非常大。Frenet转换之后，车辆的所有横向状态都是相对于参考线的，如果参考线本身在某个点突然抖动一下，那车辆的真实横向位置d会被这个抖动放大，轨迹评估和碰撞检测都会失真。更麻烦的是，带曲率的参考线在Frenet坐标转换里需要用到曲率的一阶导数，这要求参考线至少C2连续（位置、朝向、曲率都连续），否则公式里会出现无穷大或者跳变值。

我之前测试过一个场景：参考线直接用全局导航输出的折线，没做平滑，结果在拐角处车辆规划的轨迹出现了明显的横向波动，方向盘来回修正，体感非常差。后来把参考线换成C2连续的样条平滑结果，同样的采样策略和代价函数，轨迹立刻稳了下来。

还有一个实用建议：参考线的长度要覆盖住规划的视野。Lattice规划一般向前看8到10秒的轨迹，高速场景下这意味着至少几百米的参考线。如果参考线长度不够，采样终点的投影就会落在参考线之外，整个Frenet转换就悬空了。所以参考线生成通常要比当前规划距离多留出百分之二十到三十的冗余。

3. Frenet标架：一个坐标系的故事

3.1 为什么不用笛卡尔坐标

在笛卡尔坐标系下描述一条道路，哪怕是一条平缓的弯道，也需要很多参数才能表达清楚，而且车辆与车道边界的距离约束计算起来非常麻烦。你想判断“车偏左了还是偏右了”，直接看坐标x、y并不直观，你得拿坐标和道路边线比对才知道。

Frenet标架把这个事情完全简化了。它把参考线当作“拉直的公路”，车辆的位置用s（沿参考线走了多远）和d（偏离参考线多远）描述。s和d天然对应着“跑了多远、偏了多少”这两个驾驶直觉量。约束的表示也变得直接粗暴：车道边界约束就是d∈[-车道宽/2, +车道宽/2]，避障约束就是某一小段s范围内d不能落入障碍物占用区间。

3.2 Frenet坐标转换的核心公式与直觉

从笛卡尔坐标换到Frenet坐标，核心是先找投影点。假设车辆位置是(x, y)，参考线上找一点使得到该点的弧长s最小，这个点就是投影点，对应参考线上的朝向角θ_r和曲率κ_r。有了这些，横向偏移d的方向自然就是参考线的法向方向，公式为d = (x - x_r) * cos(θ_r) 之类的投影运算。但这里只是一个近似，实际工程中还要考虑投影点处参考线曲率带来的修正项。

更关键的是速度和加速度的转换。车辆实际速度v分解到Frenet坐标里，纵向速度s_dot = v * cos(Δθ) / (1 - κ_r * d)，这里Δθ是车辆朝向与参考线朝向的夹角。这个公式里有个1 - κ_r * d项，实际意义是：如果车在弯道外侧（d为正方向）或内侧，同一个物理速度对应在参考线上的“推进速度”是不同的。很多人在这一步踩坑，因为如果忽略了曲率项，车速越快、弯道越急，误差就会越大。

从Frenet转回笛卡尔坐标相对简单：已知参考线上的位置、朝向、曲率，以及d、d_dot、d_ddot，就可以恢复出笛卡尔坐标、速度和加速度。核心公式是x = x_r - d * sin(θ_r)，y = y_r + d * cos(θ_r)（注意这里d的符号约定与法向方向有关），速度则需要考虑d_dot对s_dot的影响。这一正一反两个转换，是Lattice规划里最核心的几何计算。

3.3 转换过程中最容易翻车的点

第一是投影点搜索。参考线是离散点序列，s是一个连续变量，直接在离散点上找最近点会得到台阶状的s，导数都没法求。工程做法是先对参考线做插值（线性、B样条、三次样条都可以），然后在插值结果上用牛顿迭代或者局部搜索找精确投影点。迭代初值很重要，当前s附近搜索即可，不需要全局扫一遍。

第二是曲率的计算。离散点求曲率用相邻三点拟合圆弧估算，但这样算出来的曲率噪声很大。更好的做法是先把参考线平滑到C2连续，再用参数方程求导得到精确曲率。

第三是符号约定。不同论文里d的正方向定义不一样，有的定义左侧为正，有的用完定义右侧为正。公式里的正负号差一个符号，导致轨迹左右镜像翻转，查半天查不出问题，最后发现是自己参考系的法向量定义和公式不一致。这个建议在一开始就把坐标约定写在代码注释里。

4. 多项式拟合：轨迹是怎么被“捏”出来的

4.1 为什么偏要用多项式来拟合

Lattice规划里，我们通常只关心车辆从一个状态到另一个状态的短时间运动，比如未来3到5秒。这个时间尺度下，车辆的横向运动可以近似为一个平滑的换道/修正过程，纵向运动可以近似为加速/匀速/减速过程。用多项式描述轨迹，核心优势是：多项式任意阶可导，天然满足平滑性，而且边界约束可以转化成线性方程组直接求解，计算开销极小。

如果不用多项式，有人会问能不能用贝塞尔曲线或者B样条。当然可以，B样条在全局路径平滑里面很常见，但在Lattice这种局部轨迹生成中，多项式的表达和求解更简洁，也更容易嵌入代价函数。Lattice的最早期经典论文用的就是多项式轨迹，这已经是经过工程验证的方案。

4.2 边界条件怎么定系数：五次多项式的由来

一条横向轨迹d(t)如果要同时满足起点的位置、速度、加速度和终点的位置、速度、加速度，那就是6个边界条件。一个多项式有多少个未知系数就有多少个自由度，所以5次多项式正好6个系数，1对1解出来。这就是Lattice里横向轨迹常用五次多项式的原因，不是拍脑袋定的，是被边界条件数量决定的。

同理，纵向轨迹s(t)如果只需要满足位置、速度、加速度的边界条件，再加上优化效率相关的约束，可以用四次多项式（5个系数）或五次多项式。少了哪个边界条件，就少一个次数，这是选择阶数的判断标准。

具体求解过程其实不需要复杂的优化器。把6个边界条件写成矩阵形式，这是个典型的线性方程组，用高斯消元或者直接求逆就能得到系数。实际工程中更常用的是预先推导好的闭式解，直接把起点状态和终点状态代入公式算出系数，省去矩阵求逆的数值误差和耗时。

4.3 横向纵向分开拟合的好处

为什么要分成横向d(t)和纵向s(t)两条多项式？因为它们的物理含义不同，约束来源也不同。横向多项式的边界条件是起点和终点的横向位置、横向速度、横向加速度，主要受换道距离、避障偏移量影响；纵向多项式的边界条件是起点和终点的纵向位置（即s）、速度、加速度，主要受跟车距离、限速、红绿灯影响。

分开之后，横向和纵向的代价函数可以分别设计。横向关注平滑性、横向偏移量、是否贴近参考线；纵向关注速度偏差、加速度大小、加加速度（jerk）。最后把两者组合起来得到一个总的代价，挑选最优轨迹。这种解耦设计大大简化了问题，也让每一个维度的行为都便于调试。

4.4 代价函数怎么构造、怎么调

Lattice的代价函数一般分三块：平滑性代价（jerk的平方积分）、目标状态偏差代价（偏离参考速度、偏离参考线）、碰撞/边界惩罚（软约束或者硬约束）。jerk是加速度的变化率，它直接影响乘坐舒适性：jerk太大，乘客会觉得一顿一顿的；jerk很小，车就像老司机在开。

这里一个实用的经验是：横向和纵向的jerk代价权重不要一开始就调得很大。权重太大，轨迹会过度追求平滑，导致该避障的时候避不开、该提速的时候提不起来。我建议的调试顺序是：先把安全约束和边界约束拉满，保证轨迹可用；然后从小到大调jerk权重，找到“够平滑但不至于太肉”的临界值；最后再微调目标速度偏差权重，让轨迹对速度指令的响应更积极。

5. 实操过程与核心环节实现

5.1 一条Lattice规划链路的伪代码

这里给出一份伪代码，覆盖参考线处理、Frenet转换、轨迹生成和选择的主流程。这不是完整的生产代码，但这个结构能帮你把前面的概念串起来。

def lattice_plan(frenet_state, ref_line, obstacles): # 1. 生成候选终点（纵向采样 + 横向采样） s_targets = sample_s(frenet_state.s, T_range) d_targets = sample_d(frenet_state.d, d_range) # 2. 对每个组合，用多项式拟合生成横向和纵向轨迹 trajectories = [] for T in T_range: for d_target in d_targets: # 横向轨迹：五次多项式，边界为起点状态和终点状态 d_traj = quintic_poly(frenet_state.d, d_dot, d_ddot, d_target, 0, 0, T) # 纵向轨迹：五次多项式，边界为起点s状态和终点s状态 s_traj = quintic_poly(frenet_state.s, s_dot, s_ddot, s_target, target_speed, 0, T) # 组合成笛卡尔坐标轨迹 cartesian_traj = frenet_to_cartesian(s_traj, d_traj, ref_line) # 代价评估、碰撞检测 cost = evaluate(cartesian_traj, obstacles, ref_line) trajectories.append((cost, cartesian_traj)) # 3. 排序选择最优轨迹 return min(trajectories, key=lambda x: x[0])

这段逻辑里，采样范围、采样步长、T的集合都会直接影响轨迹质量。我常用的初始参数是：T在[2, 8]秒之间，步长0.5秒；横向d在[-3.5, 3.5]米之间，步长0.5米；纵向s目标在[当前s + 5米, 当前s + 80米]之间，步长5米。你可以根据自己的场景和车速缩放这些范围。

5.2 五次多项式闭式解的参考公式

这里给出横向轨迹的闭式解参考。假设起点时刻t0=0，终点时刻T，起点状态(d0, d0_dot, d0_ddot)，终点状态(dT, dT_dot, dT_ddot)，多项式系数可通过下面这组公式计算：

def quintic_coeff(d0, d0_dot, d0_ddot, dT, dT_dot, dT_ddot, T): T2 = T * T T3 = T2 * T T4 = T3 * T T5 = T4 * T a0 = d0 a1 = d0_dot a2 = d0_ddot / 2.0 a3 = (20 * dT - 20 * d0 - (8 * dT_dot + 12 * d0_dot) * T - (3 * d0_ddot - dT_ddot) * T2) / (2 * T3) a4 = (30 * d0 - 30 * dT + (14 * dT_dot + 16 * d0_dot) * T + (3 * d0_ddot - 2 * dT_ddot) * T2) / (2 * T4) a5 = (12 * dT - 12 * d0 - (6 * dT_dot + 6 * d0_dot) * T - (d0_ddot - dT_ddot) * T2) / (2 * T5) return [a0, a1, a2, a3, a4, a5]

这个公式我建议自己手推一遍，或者至少对着推导过程看一遍。理解比背公式重要，因为以后你可能会改边界条件的数量，比如只约束终点位置不约束终点速度，那就变成四次多项式了，公式看起来会不一样。

5.3 从Frenet轨迹还原笛卡尔坐标轨迹

一个完整的轨迹不仅要知道s(t)和d(t)，还需要把它变成车能执行的笛卡尔坐标点序列。这一步要遍历每个时间点，先根据s(t)在参考线上插值找到参考位置(x_r, y_r)和朝向θ_r，然后加上横向偏移d(t)的法向修正，得到：

def frenet_to_cartesian(s_traj, d_traj, ref_line): cartesian = [] for t in range(len(s_traj)): s, d = s_traj[t], d_traj[t] x_r, y_r, theta_r, kappa_r = ref_line.interpolate(s) x = x_r - d * math.sin(theta_r) y = y_r + d * math.cos(theta_r) cartesian.append((x, y)) return cartesian

这里同样容易在正负号上出问题。上面的公式假设d以参考线左侧为正、左侧法向量对应角度为theta_r + pi/2。如果你用了不同的约定，就得把sin和cos前面的负号反过来。做一次完整转换后，把结果画出来和直觉对比一下，方向反了好歹能看出来。

6. 常见问题与排查技巧实录

6.1 投影点抖动导致s计算不稳

现象是轨迹明明在直道上，但Frenet转换出来的s却有小幅波动，导致纵向速度也不稳。排查思路是先看参考线在对应区域有没有多段距离很近的重复形状，比如高架桥下的投影点可能在不同层路径之间来回跳。解决方法是给投影搜索加一个s方向的搜索窗口，以上一帧的s为中心，只在这个窗口内找投影点，避免全局搜索。

6.2 曲线曲率突变导致横向加速度异常

参考线是用全局路径直接离散化时，曲率噪声会非常大，在Frenet转换的加速度公式里被放大。我排查过的案例中，最夸张的情况是曲率噪声让横向加速度跳了2个数量级。解决办法有两个：一是参考线平滑时用更高阶的样条约束，二是限制曲率变化率的最大值，超出阈值的点做重平滑。注意这里的阈值不能设得太紧，否则会扭曲原本合理的弯道形状。

6.3 多项式系数数值不稳定

当T很小（比如0.5秒）时，五次多项式求解里的T^5在分母上，哪怕T只差一点点，系数变化也会很剧烈，轨迹末端甚至会出现位置发散的假象。这不是算法错了，是数值条件数太差。实际工程中，我会把T的最小值限制在1.5秒以上，同时在计算系数时用双精度浮点。如果确实需要短时间规划，可以用归一化时间τ = t/T，把多项式定义在[0,1]区间内求解，数值稳定性会好很多。

6.4 代价权重怎么调都不对

出现这个问题的原因通常不是权重本身，而是前面某个环节有bug。比如轨迹生成时边界条件给错了，导致所有候选轨迹都不合理，代价函数怎么调都挑不出正常的轨迹。我排查时有个习惯：先画一条最简单的直线匀速轨迹，看看代价是不是最低；如果不是，再从采样、转换、拟合逐步检查。把简单用例跑通，再上复杂场景，效率比直接调参高得多。

常见问题速查表

7. 一些实操心得

这套Lattice框架我前后调过不少版本，最深的感受是：规划的坑大多不在算法本身，而在坐标系转换的细节和数据的预处理上。参考线多花一点时间做平滑，后面所有环节都会轻松很多；Frenet转换的符号约定能不能统一，决定了你调试时是看一遍就懂，还是对着数据发呆。

另外，多项式拟合的闭式公式最好在工程代码里直接展开写，不要依赖矩阵求逆库。虽然求逆在数学上等价，但实际运行中会因为边界条件里存在数值差异而在微小的条件数上吃亏，尤其是T比较小的时候。闭式解写起来多点行数，换来的是稳定和可排查，值得。

最后说说上篇和下篇的关系。这一篇把参考线、Frenet标架、多项式拟合讲透了，下篇就可以放心聊Lattice的采样策略、代价函数具体设计和碰撞检测了。你可以先照着这篇里的伪代码把候选轨迹生成跑起来，在几个简单场景里画出来看看轨迹形状，等有手感了再往下走。