企业官网建设流程全解析

这两年只要聊到自动驾驶规划模块，Lattice Planner 几乎是绕不开的名字。它把“在道路上生成一条可行驶轨迹”这个问题，拆解成参考线、Frenet 坐标系、多项式拟合、采样与代价评估几个清晰步骤，思路简洁，工程落地性强。这篇文章是“从零学 Lattice 规划”的上篇，重点把最基础也最容易绕晕的三块讲透：参考线怎么来、Frenet 标架到底在算什么、多项式拟合怎么把一堆边界条件变成一条平滑轨迹。

适合谁看？刚入门自动驾驶规划算法、准备面试、或者想把自己机器人导航项目里的路径生成换成更规范方案的开发者都合适。下篇我会继续梳理采样策略和代价函数，但这篇只要吃透，后面基本就是水到渠成的事。

1. 整体设计思路：Lattice规划到底在解决什么问题

1.1 从全局规划到局部规划

自动驾驶的规划通常分两层：全局路径规划负责给出从起点到终点的宏观路线，比如“前方 2 公里右转进入主路”，它不关心方向盘每时每刻怎么打；局部轨迹规划则要在当前车道、障碍物、限速、舒适性之间做权衡，生成未来 3 到 8 秒内一条具体可执行的轨迹，也就是每个时刻的横向位置、纵向速度甚至加速度。

Lattice Planner 属于局部规划，核心思路很朴素：在 Frenet 坐标系下，把横向（左右偏移）和纵向（沿路前进）解耦，分别采样一系列可能的终点，然后各自用多项式拟合出一簇候选轨迹，最后通过代价函数挑选最优解。

这种“采样 + 拟合 + 评估”的框架，相比直接在高维状态空间暴力搜索，复杂度低很多。而且它天然适合结构化道路：车道线清晰、主方向明确，把“沿着路走”和“在路内偏移”分开处理，条件反射式地知道什么轨迹是符合交规的。

1.2 为什么大家都选Frenet坐标系做轨迹规划

如果你第一次接触轨迹规划，很自然会想在笛卡尔坐标系（通常叫 XY 坐标系）里直接生成轨迹。问题是道路不是直线，弯道、坡度、车道宽度变化都会让 XY 坐标系下的轨迹描述变得非常别扭。

举个例子：一条半径为 50 米的弯道，车辆如果严格沿车道中心线走，在 XY 坐标系下横向偏差是 0，但 x、y 坐标却一直在变化。如果加一个向左 0.3 米的偏移，你会得到一组和原来完全不同的 x、y 表达式。换句话说，笛卡尔坐标系里，“偏离车道多少”不是一个直接可用的变量。

Frenet 坐标系则聪明地把问题搬到“道路自己的坐标系”里：用弧长 s 表示沿参考线走了多远，用横向偏移 l 表示离参考线的距离。这样一来，“沿车道中心走”就是 l = 0，“向左变道”就是 l 从 0 变到 3.5 米，语义极其明确。

而且更关键的是，Frenet 坐标系下横向运动与纵向运动可以近似解耦。变道过程主要关心 l(t) 的变化，跟车和加减速主要关心 s(t) 的变化，这样就能分别用低阶多项式拟合，最后再合成一条完整轨迹，计算量小，调试也直观。

2. 参考线：给Frenet坐标系打地基的关键步骤

2.1 参考线到底是什么

Frenet 坐标系需要一个“坐标系原点移动的轴”，这个轴就是参考线（Reference Line）。它是全局路径在当前场景中的局部映射，通常是一条经过平滑处理的曲线，表达为弧长参数化形式：参考线上每个点都有对应的笛卡尔坐标、切线方向角、曲率。

参考线质量几乎决定了 Lattice Planner 的上限。参考线抖一下，后面的 Frenet 转换、多项式拟合、轨迹评估全部会跟着抖。业内有个经验说法：规划模块 80% 的诡异问题，追根溯源都在参考线不够平滑。

参考线来源常见有三种：全局路径直接截取、高精地图车道中心线、或者感知融合生成的可行驶区域中心线。无论哪种来源，原始点列通常都带噪声、毛刺或者不连续，所以必须要做平滑和后处理。

2.2 参考线的生成与平滑处理

假设你已经拿到一组全局路径点 P0, P1, ..., Pn，接下来要做的核心工作是三件事：去重、插值、平滑。

去重容易理解，相邻点太近会导致后续计算曲率时数值不稳定；插值是为了让参考线变成稠密、均匀的点序列，常用三次样条或分段贝塞尔；平滑则是消除折线和锯齿。

平滑方法我实际用过两类，效果差异很大：

第一类是简单滑动平均或 Savitzky-Golay 滤波。思路是把每个点用邻域点的加权平均替换。优点是实现快、调参少，缺点是在曲率变化大的弯道容易“切弯”，把真实路径抹平太多，导致轨迹严重侵入相邻车道。

第二类是基于优化的平滑，比较典型的是构造一个目标函数，包含平滑项和贴近原始路径项：

J = w1 * sum(||P(i+1) - 2*P(i) + P(i-1)||^2) # 平滑项（二阶导近似） + w2 * sum(||P(i) - Q(i)||^2) # 贴近项（不能偏离原始点太远）

用梯度下降或者稀疏矩阵求解器迭代得到平滑后的点列。w1 和 w2 的比值就是平衡“顺滑”和“贴近”的旋钮。一般 w1 较大时弯道更顺但可能严重偏离原路径，w2 较大时能贴合原路径但平滑效果有限。实操中我习惯先调 w1 到弯道不抖为止，再适当增加 w2 防止切弯太多。

平滑完成后还要重新累积计算弧长 s，并计算每个点处的切线角 theta_r 和曲率 kappa_r。切线角可以直接用相邻点差分的反正切，曲率则需要用三点法或者对切线角再做一次差分导数，直接差分噪声大，建议先对 theta_r 做一次平滑再求导。

2.3 参考线坐标系的数学基础

弧长 s 的累积公式很简单：

s[0] = 0 s[i] = s[i-1] + hypot(x[i] - x[i-1], y[i] - y[i-1])

之后任意参考线点可以表示为 r(s) = (r_x(s), r_y(s), theta_r(s), kappa_r(s))。这里 theta_r(s) 是切线方向角，kappa_r(s) 是曲率。

为什么要保留曲率？因为 Frenet 与笛卡尔坐标转换、以及后续车辆运动学约束检查，都依赖曲率。比如后面计算横向速度时会出现 (1 - kappa_r * l) 这个因子，如果参考线曲率突变，这个因子可能异常跳动，轨迹就会在某个时刻突然“扯一下”。

所以在参考线平滑阶段，就应当把 kappa_r 一并平滑。我见过不少刚入门的朋友只平滑了 x、y 坐标，没平滑曲率，最后在弯道入口处轨迹总是跳变，排查了很久才发现根因在这。

3. Frenet标架：把复杂的道路约束变成简单的加减法

3.1 Frenet坐标系定义

Frenet 坐标系给每个参考线点定义了一组正交基：切向量（沿参考线前进方向）和法向量（垂直参考线指向左侧）。于是车辆位置可以表示为：

s：车辆在参考线上投影点的弧长 l：车辆到投影点的横向距离，左正右负

光看定义可能觉得抽象，可以类比地铁沿轨道跑：s 是地铁已经跑了多少公里，l 是车厢里的人离轨道中心线的距离。对自动驾驶来说，s 描述“跑多远”，l 描述“偏多少”。

注意 l 的正负号大家用得比较乱。有的资料规定左正右负，有的相反。建议在自己代码里统一注释清楚，否则多人协作时一个符号问题能排查一下午。

3.2 笛卡尔坐标系到Frenet坐标系的转换

这是上篇最核心的内容。假设车辆当前位置为 p = (x, y)，速度 v，航向角 theta，加速度 a。要转换到 Frenet 坐标系，第一步是找到车辆在参考线上的最近投影点，也就是求解：

s = argmin( || r(s) - p || )

因为参考线已离散化，实操中先用遍历找最近离散点，再用相邻几段做细粒度优化，否则每次转换都要做全局搜索，实时性扛不住。

找到投影点后，横向偏差 l 和相对角度差可以这样算：

l = sign( (p - r) 与法向量点积 ) * ||p - r|| 相对角度差 = theta - theta_r

速度分解到 s 方向和 l 方向是关键：

s_dot = v * cos(theta - theta_r) / (1 - kappa_r * l) l_dot = v * sin(theta - theta_r)

这里 s_dot 不是简单等于 v，因为车辆沿曲线运动时，参考线上投影点移动速度不等于车辆线速度，分母的 (1 - kappa_r * l) 正是这个几何关系的修正因子。

如果再对 l_dot 除以 s_dot，就得到横向偏移对弧长的一阶导 l_prime，也就是 l 关于 s 的变化率：

l_prime = l_dot / s_dot

二阶导 l_pp 在多项式拟合和代价函数里很常用，公式更复杂一些：

l_pp = -( kappa_r_prime * l + kappa_r * l_prime ) * tan(theta - theta_r) + a * sin(theta - theta_r) / (v * cos^2(theta - theta_r)) * (1 - kappa_r * l)

第一次看到这个公式容易劝退，但实际写代码时，可以先在数值上验证再固化：用微小扰动差分替代解析公式，对拍几轮确认一致。后续就可以放心用解析公式，性能更好。

3.3 从Frenet回到笛卡尔坐标系

生成一条 Frenet 轨迹后，最终执行还要还原成 XY 轨迹，因为控制模块通常吃笛卡尔坐标。转换公式反而更简洁：

x = r_x(s) - l * sin(theta_r(s)) y = r_y(s) + l * cos(theta_r(s))

这个公式几何意义非常直观：从参考线上的点出发，沿法向量方向移动 l 距离。注意法向量方向是 theta_r 逆时针转 90 度，对应到坐标分量就是 (-sin(theta_r), cos(theta_r))。

还原轨迹点的航向角时，还要考虑横向运动带来的额外角度偏移：

theta = atan2(l_prime, 1 - kappa_r * l) + theta_r

如果不加 l_prime 这一项，车辆在变道过程中计算出的朝向会和真实运动方向对不上，后续做碰撞检测时包围盒方向会偏。

3.4 一辆车在弯道上的两个视角

为了把抽象转换具体化，我拿一个常见场景模拟一下。

车辆行驶在半径 100 米的右弯道参考线上，当前实际横向偏移 l = 0.5 米（偏左），车速 10 m/s，车身航向与参考线切线夹角为 5 度。

用上面的公式计算：

s_dot = 10 * cos(5°) / (1 - (1/100) * 0.5) ≈ 9.98 / 0.995 ≈ 10.03 m/s l_dot = 10 * sin(5°) ≈ 0.87 m/s

也就是说，虽然车速是 10，但沿参考线投影点的移动速度略高于 10，同时车辆还在缓慢横向偏移。如果这时候忽略分母修正，直接用 10 当作 s_dot，累计几百米后 s 的误差就会明显暴露出来，轨迹前后段会出现衔接错位。

这类误差在直线道路上几乎看不出来，一上弯道就原形毕露。所以我建议在做坐标转换模块时，一定要用带曲率和横向偏移修正的完整公式，不要贪图简单省略。

4. 多项式拟合：用6个边界条件生成一条平滑轨迹

4.1 为什么用多项式，选择几阶合适

参考线、Frenet 都准备好了，接下来要解决的核心问题是：给定起点状态和终点状态，怎么生成一条连接二者且平滑的轨迹。

多项式是最直观的参数化方案。它连续可导、求导简单、计算开销小，且在起点终点约束下可以解析求解。选择几阶多项式取决于你要约束多少个边界条件。

横向轨迹 l(s) 与纵向轨迹 s(t) 通常都选择五次多项式。原因很直接：我们需要约束位置、速度、加速度三个量，每个量在起点和终点各有一个约束，一共 6 个约束，一个五次多项式有 6 个待定系数，刚好匹配。

如果只约束位置和速度，四次甚至三次多项式也够用，但加速度不连续会导致轨迹在某些衔接点产生冲击，车辆执行时容易顿挫。所以做轨迹规划，五次多项式几乎是默认起点。

4.2 五次多项式系数求解

以横向轨迹为例，设 l 对 s 的五次多项式为：

l(s) = a0 + a1*s + a2*s^2 + a3*s^3 + a4*s^4 + a5*s^5

一阶导和二阶导分别为：

l'(s) = a1 + 2*a2*s + 3*a3*s^2 + 4*a4*s^3 + 5*a5*s^4 l''(s) = 2*a2 + 6*a3*s + 12*a4*s^2 + 20*a5*s^3

已知起点 s = 0 时的 l0、l0_prime、l0_pp，以及终点 s = s_target 时的 l1、l1_prime、l1_pp，代入可得 6 个线性方程：

l(0) = a0 l'(0) = a1 l''(0) = 2*a2 l(s_target) = a0 + a1*st + a2*st^2 + a3*st^3 + a4*st^4 + a5*st^5 l'(s_target) = a1 + 2*a2*st + 3*a3*st^2 + 4*a4*st^3 + 5*a5*st^4 l''(s_target) = 2*a2 + 6*a3*st + 12*a4*st^2 + 20*a5*st^3

这是个标准的 6 元一次方程组，可以直接写成矩阵形式，用线性代数库求解。C++ 里我喜欢用 Eigen 的 ColPivHouseholderQR 或者 PartialPivLU 求解，数值稳定性足够。伪代码长这样：

Eigen::Matrix<double, 6, 6> A; Eigen::Matrix<double, 6, 1> b; A << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 1, st, st*st, st^3, st^4, st^5, 0, 1, 2*st, 3*st^2, 4*st^3, 5*st^4, 0, 0, 2, 6*st, 12*st^2, 20*st^3; b << l0, l0_prime, l0_pp, l1, l1_prime, l1_pp; Eigen::VectorXd coeffs = A.colPivHouseholderQr().solve(b);

纵向轨迹 s(t) 完全同理，只是自变量从 s 换成时间 t，约束换成初始位置、速度、加速度和目标位置、速度、加速度。

4.3 系数解出来后必须做哪些校验

算出系数不等于轨迹可用，我每次生成完多项式都会先做三件事。

第一件事：检测终点附近采样密度是否正常。如果 s_target 很小（比如不到 2 米），五次多项式在中间段可能产生夸张的凸起，即使两端约束都满足。此时应缩小采样间隔，或者直接丢弃这种太短的轨迹。

第二件事：计算轨迹中间若干点的曲率是否出现异常尖峰。多项式虽然保证了一阶、二阶导数连续，但不保证曲率有界。曲率太大意味着车辆根本转不过来弯。检查方法很简单，在轨迹上密集采样，用三点法算曲率，超过车辆最小转弯半径阈值的直接淘汰。

第三件事：检查 l(s) 是否超出道路边界。有些多项式在起点终点之间会先冲出去再折返回来，看起来像个甩尾动作，而端点约束完全正常。这种轨迹必须通过边界检查筛除。

4.4 横向与纵向轨迹为什么要分开拟合再合成

既然横向 l(s) 和纵向 s(t) 都各自用一个五次多项式，那么合成最终轨迹时怎么统一？

常见的做法是以时间为公共参数。纵向多项式直接给出 s(t)，横向多项式则给出以 s 为自变量的 l(s)。我们需要把两者合并：对每个采样时刻 t，先算 s(t)，再带入横向多项式 l(s)，最后用第 3 章的 Frenet 到笛卡尔转换得到 (x(t), y(t))。

这个流程看似绕，但好处很大：横向多项式只关心“在哪个 s 位置偏移多少”，纵向多项式只关心“什么时刻到达哪个 s”，两者之间通过 s 这个公共变量桥接，天然解耦。

如果直接在笛卡尔坐标系里对 x 和 y 分别做五次多项式，约束 x-y 之间的相关性会非常麻烦，弯道上根本没法保证车辆始终贴合车道方向。这也是“先解耦再合成”方案能成为主流的原因。

5. 实操过程：从参考线到轨迹生成的最小可运行流程

5.1 搭建一个最小的Lattice轨迹生成Demo

我建议初学者不要直接上来读大工程的源码，先自己搭一个最小闭环：输入一条弯道参考线、一个车辆起点状态、若干目标状态，输出一组候选轨迹并可视化。

准备这些就够：

• 一个支持线性代数运算的环境（Python 的 NumPy 即可，C++ 则用 Eigen）
• 一个把参考线离散为点列并预计算 s、theta_r、kappa_r 的模块
• 一个笛卡尔/Frenet 互转模块
• 一个五次多项式求解函数
• 简单绘图工具用来把轨迹画出来，方便肉眼检查

我习惯的开发顺序是：先写参考线预计算，再用纯数值方法（差分）验证坐标转换，然后实现多项式拟合，最后在仿真地图上跑候选轨迹。每步都做可视化验证，不要一口气写完再调。

5.2 参考线预计算的核心步骤

假设手头有一条参考线的散点，按顺序排好。预处理流程可以固定为这几步：

1. 去除距离过近的点（比如小于 0.05 米的点合并）
2. 用三次样条对点列插值，重新采样成均匀弧长的点列，采样间隔建议 0.1 到 0.5 米
3. 累积计算每个采样点的 s
4. 用相邻点差分计算 theta_r，并做平滑滤波
5. 用三点法或对 theta_r 求导得到 kappa_r，再次平滑

这里有个经验：均匀弧长采样要比均匀 x 间隔采样好得多。均匀 x 间隔在弯道内侧和外侧的弧长密度不一致，会导致参考线在不同位置的权重偏差。

5.3 坐标转换模块的数值验证

对于 Frenet 转笛卡尔，验证比较直观。随机在一条参考线上取若干 s 和 l，转换得到笛卡尔点，然后再反向转换回到 s 和 l，误差应该小于 1e-6。如果误差大，优先检查投影点的查找逻辑。

对于笛卡尔转 Frenet，要额外注意投影点的唯一性。对于曲率较大的参考线，车辆可能找到多个距离几乎相等的投影点，此时应选择弧长更合理的那个，避免 s 来回跳变。工程上常用“上一帧最近点附近局部搜索”的策略，而不是每帧重新全局搜索。

5.4 五次多项式求解与候选轨迹生成

生成单条轨迹的伪代码我简化如下：

def generate_trajectory(start_frenet, end_frenet, target_s): l0, l0_prime, l0_pp = start_frenet l1, l1_prime, l1_pp = end_frenet A = build_matrix(target_s) b = np.array([l0, l0_prime, l0_pp, l1, l1_prime, l1_pp]) coeffs = np.linalg.solve(A, b) s_samples = np.linspace(0, target_s, 50) l_samples = np.polyval(coeffs[::-1], s_samples) return to_cartesian(reference_line, s_samples, l_samples)

注意 np.polyval 的系数顺序是高次到低次，和上面线性方程组里 a0 到 a5 的输出顺序相反，这种小坑最容易浪费半天时间。

纵向同理，生成 s(t) 后，在每个时间点把 s 代入横向多项式。这样一条轨迹就完整了。

5.5 参数选择与效果调试记录

我最常用的初始参数参考如下：

• 参考线重采样间隔：0.2 米
• 轨迹采样个数：50 到 100 个点
• 横向终点采样范围：-3.5 到 3.5 米，间隔 0.5 米
• 纵向终点采样距离：10 到 50 米，间隔 5 米
• 多项式终点约束中，速度约束直接取当前速度，加速度约束取 0

这套参数在普通城市道路场景下效果比较稳。如果弯道特别急，要缩短纵向采样距离，否则五次多项式在长距离弯道中段容易跑出奇怪形状。高速场景则要加大纵向采样距离，否则候选轨迹可能全是急减速状态。

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

6.1 参考线不光滑导致轨迹抖动

现象：轨迹在曲线段出现周期性抖动，车辆执行时会像“抽风”一样左右修正。

排查思路：先在可视化里同时画出参考线和生成的轨迹，如果参考线本身就是波浪形的，那问题发生在参考线平滑阶段。重点检查曲率曲线，如果 kappa_r 出现高频振荡，说明平滑力度不够。

我自己的做法是调高平滑目标函数中平滑项的权重，并且对 kappa_r 做一次低通滤波。注意滤波窗口不要太大，否则会把急弯的真实曲率也抹平，导致规划出的轨迹切弯。

6.2 Frenet坐标转换数值异常

现象：笛卡尔坐标转 Frenet 后，l 值莫名其妙变成几十米，或者 s_dot 出现负数。

常见原因有两个：一是投影点查找用了全局最小距离，但参考线存在折返段，车辆可能投影到错误的一侧；二是车辆离参考线距离过远时，直接求解最近点会不稳定。

解决办法是给投影查找加一个“窗口限制”：当前参考线点序号前后限定的范围内搜索最近点，而不是全局搜索。此外对 l 的符号判断要用法向量点积，而不是简单地取绝对值。

6.3 多项式拟合出现振荡

现象：在 s_target 较长（比如 80 米）时，横向多项式在起点附近出现明显大的摆动，车辆轨迹先猛打方向再回正。

原因分析：五次多项式虽然连续光滑，但不保证单调性，端点之间自由振荡完全可能。当终点离得太远而横向偏移变化又比较小，高次项可能会为了匹配端点加速度约束而产生额外弯曲。

解决手段有两个方向：一是把长距离轨迹拆成多段，每段用不超过 20 到 30 米的目标 s，逐段拼接；二是对候选轨迹增加一个“最大横向加速度/曲率检查”，超过阈值的直接淘汰，而不是试图调节多项式系数强行修正。

6.4 避坑清单（实测总结）

整理一份我踩过的、比较有代表性的坑，供参考：

6.5 排查定位的小技巧

调试 Lattice 规划时，我强烈建议把中间结果逐层可视化出来：参考线、Frenet 转换后的采样点、每一簇多项式曲线、最终选中的轨迹、代价分数。信息越全，定位越快。

另外，把每个候选轨迹在笛卡尔坐标系里用不同颜色画出来，肉眼观察“哪条轨迹是怪物比看任何调试日志都直观。很多时候多项式系数本身没错，是某个采样点的边界条件给得不合理，整体看起来就像一团乱码。

我个人在实际调试中的体会是：这套算法链路上，八成问题都出在参考线和坐标转换这两个最基础的环节，多项式拟合反而是最不容易出错的那块。所以刚上手的朋友，如果发现轨迹不对，先别怀疑五次多项式，回去检查参考线平不平滑、投影点找得对不对，往往半小时就能找到根因。

后面有时间我再把采样策略、代价函数设计以及如何把候选轨迹真正接上控制模块这半部分写完。到时候你会发现，当参考线、Frenet、多项式这三块地基扎稳之后，上面长出来的东西都很快。