这两年只要聊到自动驾驶规划模块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(i1) - 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::Matrixdouble, 6, 6 A; Eigen::Matrixdouble, 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 参考线预计算的核心步骤假设手头有一条参考线的散点按顺序排好。预处理流程可以固定为这几步去除距离过近的点比如小于 0.05 米的点合并用三次样条对点列插值重新采样成均匀弧长的点列采样间隔建议 0.1 到 0.5 米累积计算每个采样点的 s用相邻点差分计算 theta_r并做平滑滤波用三点法或对 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 避坑清单实测总结整理一份我踩过的、比较有代表性的坑供参考问题症状处理方式参考线重采样间隔不均匀弯道处轨迹质量差用累积弧长均匀插值忽略了曲率平滑参考线曲率高频跳变对 kappa_r 做低通滤波投影点全局搜索s 在弯道上跳变改用上一帧最近点窗口搜索多项式系数顺序颠倒轨迹形状完全错误统一约定低次到高次顺序太短的 s_target 也采样轨迹中段凸起设定最短轨迹长度阈值直接差分求曲率数值噪声大先平滑切线角再求导不检查曲率上限轨迹转不过弯增加曲率校验6.5 排查定位的小技巧调试 Lattice 规划时我强烈建议把中间结果逐层可视化出来参考线、Frenet 转换后的采样点、每一簇多项式曲线、最终选中的轨迹、代价分数。信息越全定位越快。另外把每个候选轨迹在笛卡尔坐标系里用不同颜色画出来肉眼观察“哪条轨迹是怪物比看任何调试日志都直观。很多时候多项式系数本身没错是某个采样点的边界条件给得不合理整体看起来就像一团乱码。我个人在实际调试中的体会是这套算法链路上八成问题都出在参考线和坐标转换这两个最基础的环节多项式拟合反而是最不容易出错的那块。所以刚上手的朋友如果发现轨迹不对先别怀疑五次多项式回去检查参考线平不平滑、投影点找得对不对往往半小时就能找到根因。后面有时间我再把采样策略、代价函数设计以及如何把候选轨迹真正接上控制模块这半部分写完。到时候你会发现当参考线、Frenet、多项式这三块地基扎稳之后上面长出来的东西都很快。