1. 参考线的质量决定了 Frenet 坐标系的生死先说一个我遇到过很多次的现场。自车在一段半径一百多米的弯道上跟车规划器跑得好好的突然有一帧轨迹末端往左甩了半米下一帧又甩回来乘客体感就是方向盘抖了一下。把日志拉出来一看横向偏移的导数 d 从 0.02 直接跳到 0.9再跳回去。障碍物没变、代价函数没变、采样参数没变变的是参考线——上游模块在那一段做了一次分段拼接接缝处的航向角 θ_r 差了 0.03 rad。这篇文章要聊的就是 Frenet 坐标系下的最优轨迹生成包括参考线怎么影响它、横向纵向怎么解耦、动态障碍物怎么塞进来、代价函数怎么调以及一堆文档里不会写但线上一定会遇到的数值坑。适合已经写过或准备写轨迹规划模块的人看纯新手也能看懂大框架因为我会尽量把公式背后的为什么讲清楚而不是甩一堆推导。1.1 为什么要把规划问题搬到 Frenet 坐标系在笛卡尔坐标里规划你面对的是一个二维平面上的自由问题状态是 (x, y, θ, v)约束来自道路边界、车道线、障碍物、车辆运动学。道路是弯的车道线是弯的障碍物是动的。你想描述保持在本车道内这件事在笛卡尔下要么用车道的多项式边界做不等式约束要么用量化的偏离距离都很别扭而且随着车往前走这个约束的表达式一直在变。Frenet 坐标系做的事本质上是把弯曲的道路拉直。以参考线为纵轴 s以参考线的法向为横轴 d一条蜿蜒的车道在这个新坐标系里就变成了一个矩形带d ∈ [d_left, d_right]边界是两条几乎水平的直线。这件事的价值在于三点。第一问题解耦。在 Frenet 下纵向的 s 方向主要负责速度规划和跟车横向的 d 方向主要负责换道、避障、贴边两者可以先用独立的多项式生成候选再组合起来评估。笛卡尔下这两件事是纠缠的你没法说我只在横向上做一个五次多项式因为横向的定义本身就是随参考线走的。第二约束表达简单。车道边界在 Frenet 下就是 d 的两个上下限静态障碍物在 Frenet 下就是 (s, d) 平面上的一个矩形近似不用再在笛卡尔里做多边形求交。这个简化后面会讲代价但收益先拿到了。第三轨迹族的结构天然合理。人在开车的时候横向动作和纵向动作确实是分开决策的我要不要变道、什么时候开始打方向、变过去之后要不要加速。Frenet 的采样方式直接对应这种决策结构。代价也是有的。Frenet 坐标系是寄生在参考线上的参考线一旦抖、断、弯得太狠整个坐标系跟着一起劣化。这就是它和笛卡尔坐标系最本质的区别笛卡尔是全局的、绝对的Frenet 是局部的、相对的。你在 Frenet 里做出的每一个决策都隐含地假设了参考线是可信的。1.2 参考线和 Frenet 坐标系到底谁定义谁最近被问到一个问题Frenet 坐标系和参考线是什么关系这个问题问得很准因为它戳到了很多人理解上的一个模糊点。准确的答案是参考线不是 Frenet 坐标系里的一个元素参考线是 Frenet 坐标系的定义本身。一个 Frenet 坐标系由三个量完全确定参考线上每个弧长 s 处的位置 r(s) (x_r, y_r)、航向 θ_r(s)、曲率 κ_r(s)。有了这三个量任意笛卡尔点 (x, y) 才能算出它对应的 (s, d)。换句话说换一条参考线就是一个全新的坐标系。你在旧坐标系里算出来的 (s, d)、速度分量、曲率在新坐标系里全都得重算。这件事有几个很实际的推论我在项目里被它们坑过不止一次不要在模块之间共享 (s, d) 数值而不共享参考线 ID。我见过上游发一条参考线下游又自己重新生成了一条差不多的参考线两边各自算 Frenet 转换接缝处差了几厘米结果规划出来的轨迹和控制器执行的轨迹对不上。参考线必须以稳定的方式增量更新。每次重新从地图里抽一条中心线起点、点间距、平滑参数只要有一点变化整条线的 s 分布就变了。正确做法是保持一个滚动窗口新点从尾部追加旧点从头部淘汰或者至少保证前后两帧的参考线在重叠区域上数值一致。参考线的 s 必须是真正的弧长不能是点序号。用点序号当 s 会引入一个随点间距变化的缩放因子所有关于 s 的导数量纲都是错的代价函数里的权重也就失去意义了。1.3 参考线抖动在 Frenet 空间里被放大多少回到开头那个 0.03 rad 的航向跳变。为什么它能把 d 从 0.02 干到 0.9d 的定义是点到参考线的法向距离。当参考线在接缝处的航向发生跳变投影点的位置会突然换到一段不同朝向的线段上法向方向也跟着转了 0.03 rad。对于距离参考线 1.5 米的点法向方向转 0.03 rad投影点沿参考线移动的量级就是 1.5 × tan(0.03) ≈ 0.045 米。听起来不大但 d 是 d 对 s 的导数s 的增量在相邻两帧里可能只有 0.1 米于是 d 的跳变量级就是 0.045 / 0.1 ≈ 0.45。数量级对上了。结论很直接参考线的角度连续性比位置连续性重要一个数量级。位置跳 1 厘米没人看得出来角度跳 0.01 rad 就能让下游的横向规划抖成筛子。所以在做参考线平滑的时候别只盯着平滑后的点是否贴近原始点必须同时检查一阶差分航向和二阶差分曲率的连续性。我一般的判据是相邻点航向差不超过 0.005 rad相邻点曲率差不超过 0.002 1/m超过就说明这段平滑得不够。离散点平滑里有一个常用的做法是在代价里加三项平滑后的点要贴近原始点、相邻线段要有共线倾向用余弦相似度、曲率要小。这三个权重里余弦项和曲率项就是专门用来压制角度和曲率跳变的实际操作中把它们调大一些收益非常明显。2. 从 (x, y) 到 (s, d) 的换算以及那个容易被忽略的 (1 − κ_r d)2.1 投影点搜索窗口比搜索精度更重要把笛卡尔点转到 Frenet第一步是找到它在参考线上的投影点。教科书会告诉你用最近点搜索。工程上绝对不能做全局最近点搜索原因有两个一是 O(N) 的全局扫描在每帧几十上百个点的转换需求下太慢二是全局搜索会跳变——自车在 S 形弯道上或者参考线自身有回折的时候全局最近点可能突然跳到参考线的另一段上去s 一下子跳几十米。正确做法是带先验的窗口搜索用上一帧的 s 作为初值只在 [s_prev − 5, s_prev 5] 米范围内找最近点。窗口大小取多大取决于单帧车辆可能移动的最大距离5 米对乘用车来说有余量了。找到最近点之后还有一步容易被忽略投影点未必在离散步点上要做局部线性化。假设参考线离散点间距 0.5 米最近点落在索引 i 处真实的投影点可能在 i 和 i1 之间的任意位置。用 i 和 i1 两点的位置和切向做一个局部直线/二次拟合求出精确的投影弧长比直接用最近点的 s 精度高一个档次。import numpy as np def cartesian_to_frenet(x, y, ref_x, ref_y, ref_theta, ref_kappa, s_axis, s_prev): 把笛卡尔点 (x, y) 投影到参考线上返回 (s, d)。 ref_* 均为按 s_axis 等间距重采样后的数组。 # 1) 在上一帧 s 附近开窗搜索窗口 5 米 s_min, s_max s_prev - 5.0, s_prev 5.0 i0 int(np.searchsorted(s_axis, s_min)) i1 int(np.searchsorted(s_axis, s_max)) i0 max(i0, 0) i1 min(i1, len(ref_x) - 1) if i1 i0: i0, i1 0, len(ref_x) - 1 seg slice(i0, i1 1) dx ref_x[seg] - x dy ref_y[seg] - y d2 dx * dx dy * dy k int(np.argmin(d2)) i0 # 2) 在最近点附近做二次插值得到亚点间距的投影弧长 # 用最近点处的切向把残差分解到切向/法向 theta_r float(np.interp(s_axis[k], s_axis, ref_theta)) kappa_r float(np.interp(s_axis[k], s_axis, ref_kappa)) cos_t, sin_t np.cos(theta_r), np.sin(theta_r) dxr x - ref_x[k] dyr y - ref_y[k] ds_local dxr * cos_t dyr * sin_t # 沿切向的残差 d -dxr * sin_t dyr * cos_t # 沿法向的残差即横向偏移 s s_axis[k] ds_local * (1.0 / max(1.0 - kappa_r * d, 1e-3)) return float(s), float(d)注意最后那行除以 (1 − κ_r d)。很多人第一次写的时候会直接写成s s_axis[k] ds_local在直道上完全没问题一进弯道就开始累积误差因为切向位移和弧长增量本来就不相等。2.2 坐标与导数的换算公式只算出 (s, d) 是不够的规划器需要的是 (s, d) 以及它们对时间的导数。这里的换算关系是整套方案里最需要背下来的部分。设参考线在 s 处的航向为 θ_r、曲率为 κ_r自车在笛卡尔下的速度分量为 (ẋ, ẏ)加速度分量为 (ẍ, ÿ)。定义航向偏差 Δθ θ − θ_r其中 θ 是自车实际航向。则位置层d 的符号由法向残差的正负决定参考线左侧为正、右侧为负或者反过来全队统一就行最怕的是不同模块符号约定不一致。s 通过上述投影得到。速度层沿参考线切向的速度分量 v_t ẋ cos θ_r ẏ sin θ_r沿参考线法向的速度分量 v_n −ẋ sin θ_r ẏ cos θ_r纵向速度 ṡ v_t / (1 − κ_r d)横向速度变化率 ḋ v_n横向位移的弧长导数 d ḋ / ṡ (1 − κ_r d) tan(Δθ)加速度层纵向加速度 s̈ (ẍ cos θ_r ÿ sin θ_r − (ṡ)² · κ_r · d ... 严格形式涉及多项工程上常用的简化是 s̈ ≈ (ẍ cos θ_r ÿ sin θ_r) / (1 − κ_r d)横向加速度的二阶弧长导数 d 由 d (d̈ − d s̈) / ṡ² 得到其中 ḋ 的时间导数 d̈ 是可直接测量的横向加速度。我把这些关系整理成一张表方便对照物理量笛卡尔表达Frenet 表达关键换算因子位置(x, y)(s, d)投影 法向残差纵向速度ẋ cos θ_r ẏ sin θ_rṡ1 / (1 − κ_r d)横向速度−ẋ sin θ_r ẏ cos θ_rḋ1横向一阶导—d ḋ / ṡ1 / ṡ横向二阶导—d (d̈ − d s̈) / ṡ²1 / ṡ²路径曲率—κ ≈ (κ_r d) / (1 − κ_r d)小航向偏差近似那个(1 − κ_r d)是整张表里最核心的因子它的物理含义是参考线的法向线段在弧长上的伸缩比。参考线往左弯κ_r 0在它右侧 d 0 的地方法向线段被压缩所以同样的切向位移对应更小的弧长增量。直道上 κ_r 0这个因子退化成 1所有公式都变回直觉的样子。2.3 这个因子什么时候会要命(1 − κ_r d) 的问题在于它可能接近 0 甚至变负。举个具体数字。参考线曲率 κ_r 0.02半径 50 米的弯自车横向偏移 d 50 米。这时候 1 − κ_r d 0公式直接除零。当然 50 米的横向偏移在正常驾驶里不可能出现但如果你在做仿真复现、或者在停车场这种参考线曲率极大的场景里跑κ_r 0.2半径 5 米配上 d 5 米一样会炸。防御措施有两层。第一层是在投影阶段就做窗口限制如果算出来的 d 绝对值超过一个合理阈值比如 5 米说明这个点根本不在参考线的有效邻域内要么拒绝这次投影要么退回到全局搜索重试。第二层是在除法处加保护把 (1 − κ_r d) 钳位到一个最小值比如 0.3宁可引入一点误差也不能让整个模块崩掉。但钳位只是止血。真正的解法是保证参考线的曲率不超过某个上限并且在生成参考线时就把 d 的取值范围和 κ_r 绑在一起考虑。一条曲率 0.1 的参考线它的有效横向范围可能只有 ±8 米你要做跨三车道的换道规划就得保证这一段参考线的曲率足够小或者干脆换一条更平缓的参考线。这是规划模块和地图/导航模块之间的一个隐性契约很多团队都是踩了坑才补上的。3. 横向五次、纵向四次多项式阶数是怎么定下来的3.1 边界条件数量决定阶数没有玄学每次有人问为什么横向用五次多项式、纵向用四次我都会反问一句你在两端各想约束几个量一个 n 次多项式有 n1 个系数需要 n1 个独立条件才能唯一确定。横向规划的目标是从当前横向状态平滑地过渡到目标横向状态。当前时刻的横向状态是 (d₀, d₀, d₀)——当前偏移、当前横向偏移变化率、当前横向加速度。目标状态的横向状态是 (d₁, d₁, d₁)——目标偏移比如相邻车道中心以及目标处的横向变化率和横向加速度。一共 6 个条件所以需要五次多项式。纵向规划的目标是从当前纵向状态s₀, v₀, a₀出发在时间 T 内走完一段距离末端达到指定的速度和位置。注意这里不需要约束末端的加速度——跟车场景下末端加速度由障碍物速度决定硬约束反而会引入矛盾。所以是 5 个条件s₀, v₀, a₀, s₁, v₁对应四次多项式。这个逻辑很好用你可以直接拿它推导其他变体。比如想约束末端的加加速度jerk为零那就加一个条件横向变六次、纵向变五次。约束哪些量取决于你想让轨迹在端点处有多顺滑地接上下一段。3.2 系数闭式解直接抄五次多项式 d(t) a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵给定边界条件 (p₀, v₀, a₀) 和 (p₁, v₁, a₁)、时长 T系数闭式解为a₀ p₀ a₁ v₀ a₂ a₀ / 2 a₃ (20p₁ − 20p₀ − (8v₁ 12v₀)T − (3a₀ − a₁)T²) / (2T³) a₄ (30p₀ − 30p₁ (14v₁ 16v₀)T (3a₀ − 2a₁)T²) / (2T⁴) a₅ (12p₁ − 12p₀ − (6v₁ 6v₀)T − (a₀ − a₁)T²) / (2T⁵)四次多项式 s(t) a₀ a₁t a₂t² a₃t³ a₄t⁴给定 (p₀, v₀, a₀) 和 (p₁, v₁)、时长 Ta₀ p₀ a₁ v₀ a₂ a₀ / 2 a₃ (4Δs − (3v₀ v₁)T − a₀T²) / T³ 其中 Δs p₁ − p₀ a₄ (−3Δs (2v₀ v₁)T 0.5a₀T²) / T⁴这两组公式我建议直接做成工具函数用单元测试卡住。测试用例很简单v₀ v₁ v、a₀ 0、Δs vT代入后应该得到 a₃ a₄ 0轨迹退化成匀速直线。如果算出来不是说明公式抄错了。def quintic_coeffs(p0, v0, a0, p1, v1, a1, T): a0_ p0 a1_ v0 a2_ a0 / 2.0 a3_ (20*p1 - 20*p0 - (8*v1 12*v0)*T - (3*a0 - a1)*T*T) / (2*T**3) a4_ (30*p0 - 30*p1 (14*v1 16*v0)*T (3*a0 - 2*a1)*T*T) / (2*T**4) a5_ (12*p1 - 12*p0 - (6*v1 6*v0)*T - (a0 - a1)*T*T) / (2*T**5) return [a0_, a1_, a2_, a3_, a4_, a5_] def quartic_coeffs(p0, v0, a0, p1, v1, T): ds p1 - p0 a0_ p0 a1_ v0 a2_ a0 / 2.0 a3_ (4*ds - (3*v0 v1)*T - a0*T*T) / (T**3) a4_ (-3*ds (2*v0 v1)*T 0.5*a0*T*T) / (T**4) return [a0_, a1_, a2_, a3_, a4_]3.3 采样什么采多密多项式有了接下来是往里面填什么样的边界条件。横向上最常用的采样是把目标偏移 d₁ 按车道宽度铺开。假设车道宽 3.5 米自车当前在车道中心那么可以采样 d₁ ∈ {0, ±3.5, ±7.0}对应保持换一条换两条。目标横向速度 v₁ 和横向加速度 a₁ 通常取 0因为换道结束时我们希望车是直的、没有横向速度的。但这不是死规定——如果换道目标时刻和障碍物预测强相关可以给 a₁ 留一个小的非零值来更好地避让。纵向上采样的是目标时刻 T 和目标速度 v₁ 的组合。常用的做法是T 取 {2, 4, 6, 8} 秒v₁ 取 {0, 参考速度的 0.5 倍, 参考速度, 参考速度的 1.2 倍}。这样粗算下来横向 5 个采样 × 纵向 16 个采样 80 条候选轨迹再扣除掉横向纵向耦合后不合理的组合实际评估大概几十条。这个数量级在 10 毫秒的控制周期里是可以接受的。采样密度的取舍是个经验活。太稀某些可行解根本不在采样集合里表现为明明有路却规划不出来太密计算量上涨不说还容易出现两条代价几乎相同的候选选中哪条基本靠噪声导致帧间抖动。我的经验是宁可采样稀一点用更细的代价函数和更严格的约束筛选。代价函数是连续的采样是离散的用连续的准则去从粗采样里挑比用粗准则从细采样里挑更稳。还有一个容易被忽视的点候选集合的生成必须保留上一帧的最优解作为候选之一。这在控制论上叫上一时刻解的一致性保持实际效果就是避免相邻两帧之间在两个代价接近的候选之间反复横跳。代价函数里加一个与上帧轨迹的偏差项也是同一个目的。4. 动态障碍物怎么塞进静态的 Frenet 世界4.1 ST 图把时间轴展开成一张地图纯几何的多项式采样有个天然缺陷它只考虑空间不考虑时间。一条横穿马路的行人轨迹在 (s, d) 平面上看是一条线你没法判断自车和他会不会在同一个时刻出现在同一个位置。解决办法是在 Frenet 坐标系上再叠一个时间维度也就是ST 图。横轴是时间 t纵轴是沿参考线的弧长 s。自车的轨迹在这张图上是一条从左下到右上的曲线障碍物的预测轨迹也投影成曲线再加上安全裕度膨胀就变成了一条带。这一下就把会不会撞变成了一个二维几何问题自车的 s(t) 曲线和障碍物的膨胀带相交就是冲突不相交就是安全。在 ST 图上做速度规划本质上是在一张障碍物带的地图上找一条从起点到终点的可行路径。ST 图的最大好处是把纵向速度规划和碰撞约束统一起来了。横向换道决策在 d 维度上做纵向的加减速在 s-t 维度上做两者分开但通过代价函数耦合。这就是为什么 Frenet 方案特别适合动态场景——它给了你两个正交的平面去分别处理两类问题。4.2 障碍物膨胀前向多留还是均匀多留障碍物从 (x, y) 转到 Frenet 的 (s, d)本质上是对它的每个预测轨迹点做一次坐标转换。但转换之后不能直接当矩形用必须膨胀。膨胀的策略直接影响驾驶风格这里有两个极端均匀膨胀在 s 和 d 方向都加同样的安全距离比如前后各 3 米、左右各 1.5 米。简单但对纵向的跟车场景不友好——前车匀速前进时你其实可以跟得近一点。前向膨胀根据相对速度决定前向膨胀量公式大概是 d_safe d_base v_rel × t_reaction。v_rel 是自车与障碍物的相对纵向速度t_reaction 是预判时间取 1.0 到 1.5 秒。这样静止障碍物前面只留基础距离迎面来车如果参考线方向一致但前车突然减速前面会留出更大的空间。我在实际项目里用的是一种折中纵向膨胀量按相对速度线性插值横向膨胀量按障碍物类型固定。行人、自行车这类横向可能突然移动的横向给 1.0 到 1.5 米车辆给 0.5 到 0.8 米就够因为它们横向运动受车道约束。注意膨胀量加得太大会导致本来有解的窄通道变得无解。我见过一个案例在一条 3.2 米宽的车道里两侧障碍物各膨胀 1.5 米直接把可行域压没了规划器报无可行轨迹。后来把横向膨胀改成按障碍物实际占用宽度 0.4 米余量问题解决。4.3 暴力采样不够用得上 DP QP如果障碍物不多、场景简单前面那套横向 5 × 纵向 16的采样是够用的。但动态障碍物一多问题就变了性质。想象一个场景前方有两辆车一快一慢右侧车道还有一辆车正在减速准备并入。你的纵向速度剖面不能是任何单调的东西——你得先减速给右侧并线车让出空间等它进来之后加速然后在前方慢车后面稳定跟车。这种减-加-减的速度剖面用固定的 s₁ 和 v₁ 采样是生成不出来的因为你在采样阶段就假设了速度是单调趋近目标的。工业界的标准解法是两级结构第一级用**动态规划DP**在 ST 图上做粗规划。把 ST 图离散成网格t 方向 0.1 秒一格、s 方向 0.5 米一格每个格子有一个代价代表自车在这个时刻处于这个位置的舒适性代价障碍物占据的格子代价无穷大。DP 从起点推到终点得到一个粗略的最优速度剖面——它能处理减-加-减这种任意形状因为 DP 不做单调性假设。第二级用**二次规划QP**对 DP 的结果做平滑。DP 出来的曲线是锯齿状的因为它是在离散网格上走的。QP 以 DP 结果为参考构造一个带约束的优化问题最小化 jerk 和偏离 DP 结果的代价约束是位置、速度、加速度、jerk 的上下限以及障碍物约束。这个两级的思路很有讲究。DP 负责在离散空间里做全局搜索能找到各种奇怪形状的解QP 负责在连续空间里做局部精修保证平滑和动力学可行。两者互补。直接上 QP会陷入局部最优遇到减-加-减这种非凸场景直接卡死只用 DP轨迹抖得没法执行。5. 代价函数什么才算最优5.1 三项基本代价每一项目标都不一样对每条候选轨迹最常用的代价函数可以拆成三块横向代价C_d k_j ∫ (d)² dt k_t T k_d (d₁)²第一项是横向加加速度jerk的平方积分控制乘坐舒适性。为什么是 jerk 而不是加速度因为人对加速度的绝对值其实不敏感飞机起飞时 0.3g 的加速度大家都受得了但对加速度的变化率极其敏感——这就是为什么急刹比匀速刹车难受。jerk 的平方积分是舒适性的经典度量。第二项是时间代价鼓励快一点完成动作。没有这一项规划器会倾向于用最长时间做最慢的动作因为它最平滑。第三项是终点偏移代价鼓励终点尽量贴近车道中心。没有这一项规划器可能停在离车道中心 1 米的地方——它不违法但很难看而且给下一次规划留了个烂摊子。纵向代价C_s k_j ∫ (s)² dt k_t T k_v (v₁ − v_ref)²结构一样只是最后一项换成了终点速度和参考速度的偏差。跟车场景下 v_ref 取前车速度巡航场景下取限速。总代价C_total k_lat C_d k_lon C_s再加上一系列硬约束和软惩罚碰撞硬约束直接删掉候选、曲率超限、横向加速度超限、超出车道边界、与上帧轨迹的不一致。5.2 权重怎么调从量纲开始别从感觉开始调权重最容易犯的错误是凭感觉试数。正确的起点是看量纲。jerk 平方积分的量纲是 m²/s⁵乘以时间 T。假设一段换道动作jerk 峰值 2 m/s³、持续时间 4 秒积分大概是 4 秒 × 平均 1 m²/s⁶ ≈ 4。而 (d₁)² 项如果终点偏移 0.5 米就是 0.25。两者差了一个量级多所以 k_j 和 k_d 的初始比例应该在这个量级附近而不是随手都填 1。我的做法是先固定一个基准场景比如 60 km/h 直道上换一条车道把所有候选的每一项代价值打出来看它们的分布范围然后把权重调到让各项对总代价的贡献在同一个数量级。这比盲调靠谱得多。再分享一个实操细节权重必须做归一化。不同场景下的速度、距离差好几个数量级如果权重是绝对值高速场景里 v 相关的项会压倒一切。常见做法是把每项代价除以一个参考值再乘权重参考值取该场景下典型工况的量级。代价项量纲典型量级建议初始权重∫d² dtm²/s⁵1 ~ 101.0∫s² dtm²/s⁵1 ~ 201.0Ts2 ~ 810 ~ 30(d₁)²m²0.01 ~ 1100 ~ 300(v₁ − v_ref)²m²/s²0 ~ 255 ~ 20表格里的数字只是起点不同车型、不同驾驶风格差异很大。豪华车的舒适性权重要大运动模式可以放宽 jerk 惩罚。5.3 别忘了校验动力学可行性代价函数挑出来的最优候选不一定是可执行的。最常见的问题是曲率超限。规划出来的轨迹在 Frenet 里看着很漂亮转成笛卡尔之后发现某一段的曲率半径小于车辆的最小转弯半径。这在高速场景下尤其容易发生因为高速时轨迹的横向偏移量在 s 方向被拉得很长转换到笛卡尔后曲率被放大。所以流程里必须有一个硬校验环节把候选轨迹从 Frenet 转回笛卡尔采样若干点计算每个点的曲率检查是否满足 κ ≤ a_lat_max / v² 和 κ ≤ κ_max车辆机械极限。同时检查航向角变化率确保方向盘转角速度不超限。不满足的直接淘汰不参与代价比较。这个校验还有一个作用它是发现 Frenet 转换 bug 的第一道防线。如果某条候选的曲率算出来是 10 1/m 这种离谱数值那基本可以确定是 (1 − κ_r d) 那里出了问题或者参考线的曲率数据本身就是错的。6. 实操中把人折磨疯的几个数值问题6.1 低速时 d 的发散——最容易踩的坑d ḋ / ṡ。当 ṡ 接近 0d 会瞬间发散。这个坑在停车场、拥堵蠕行、路口停车这些场景里必然出现。表现是车慢下来的时候横向规划开始剧烈抖动方向盘左右小幅摆动乘客体感很差。日志里能看到 d 出现几十甚至上百的数值。根源在于用弧长做自变量在低速时是病态的。车几乎不动s 几乎不增加但时间在流逝慢慢积累的横向偏移除以几乎为零的 Δs导数自然爆炸。业内有三类解法各有适用场景方案一速度阈值 时间参数化切换。当 ṡ 低于某个阈值比如 0.5 m/s时横向规划从对 s 求导切换到对时间求导直接规划 d(t) 而不是 d(s)。代价函数也相应地从 ∫d² ds 改成 ∫d² dt。切换时会有一个数值上的不连续需要在切换点做一段过渡或者保证两种参数化在切换点附近给出的轨迹足够接近。方案二给 ṡ 设下限。计算 d 时用 max(ṡ, ε)ε 取 0.3 到 0.5 m/s。简单粗暴但在真正的静止场景下会引入明显误差——车完全不动的时候你算出来的 d 其实是实际横向速度除以 0.5数值被压小了很多规划器会以为横向很平滑实际执行可能偏。方案三低速时降低横向采样密度并放大时间尺度。既然低速时横向动作本身就应该慢就把采样时间 T 拉长同时把横向的目标偏移限制在一个小区间比如 ±0.3 米。这个方案和多边形的低速专用规划器配合得最好。我在项目里用的是方案一加方案三的组合设计一个按速度分档的规划策略高于 3 m/s 用完全的 Frenet 弧长参数化1 到 3 m/s 之间用时间参数化低于 1 m/s 用一个专门的、时间尺度拉长的低速规划器。三档之间有重叠区间做混合避免切换点的突变。6.2 参考线切换时的坐标系跳变前面说过换参考线就是换坐标系。这在代码里的具体表现是上一帧算出来 s 100.3、d 0.8这一帧换了参考线之后变成 s 99.7、d 0.4。数值上跳了但物理上自车一动不动。如果你的规划器把与上帧轨迹的偏差作为代价项或者用上一帧的状态作为本帧的初始条件这个跳变会直接变成控制量的抖动。处理方式是在参考线切换时做一次状态的重新初始化和轨迹重新锚定新参考线下重新计算自车状态然后以新状态为起点重新生成轨迹。等价的技巧是保留旧参考线下的一段已生成轨迹在重叠区域上做一次坐标映射让新旧轨迹在重叠区平滑过渡。一个更工程化的做法是从源头上避免频繁切换参考线。参考线应该是一个滚动窗口只要窗口覆盖了当前和未来几秒的行驶需求就不需要换。只有当车辆接近窗口边界或者导航层明确指示了路径变更时才做一次切换。这样切换频率从每帧一次降到每分钟几次问题就好处理多了。6.3 参考线长度和窗口管理的细节参考线的长度取多少合适太短高速下一个规划周期走完就出界了太长投影搜索和坐标转换的计算量白给。我的经验公式是参考线长度 ≥ 规划时域 × 最大速度 × 1.5。规划时域 8 秒、最大速度 30 m/s那就是 360 米取 400 米比较稳。低速场景下可以缩短到 100 米。点间距的选取也有讲究。太密0.1 米内存和计算浪费太稀疏2 米投影精度不够弯道上的线性化误差会很大。0.5 米是个比较通用的折中。但要注意点间距必须均匀——如果参考线是从地图里抽出来的原始点间距可能是变化的必须先重采样成等间距。重采样的做法是按弧长累加每 0.5 米插一个新点用三次样条或者线性插值求位置切向用相邻点差分曲率用切向的差分除以弧长得到。这里有个细节曲率用差分算出来噪声很大必须做一次低通滤波。我一般用滑动窗口平均或者简单的指数平滑窗口长度 5 到 10 个点。曲率数据的噪声会直接传导到 (1 − κ_r d) 和路径曲率的计算上。6.4 轨迹拼接的连续性规划器每帧输出一条轨迹控制器每帧执行一部分。如果每帧的轨迹在拼接点处不连续——位置、速度、加速度有跳变——车辆就会抖。保证连续性的关键是用上一帧的实际执行状态作为本帧的初始条件。注意是实际执行而不是上一帧规划出来的。控制器可能因为执行器延迟、限幅等原因实际执行的和规划的并不完全一致。用规划值作为初始条件误差会累积。具体做法控制器每帧反馈实际的 (x, y, θ, v, a)规划器用它做坐标转换得到 (s₀, d₀, ṡ₀, ḋ₀, s̈₀, d̈₀)作为本帧多项式生成的起点。这样每帧的轨迹在 t0 处的状态就是车辆当前的真实状态拼接天然连续。7. 怎么确认轨迹生成真的没问题7.1 可视化把两套坐标系同时画出来调试这套东西可视化工具的重要性不亚于算法本身。我的习惯是同时开三个视图笛卡尔视图车辆、参考线、障碍物、规划轨迹、预测轨迹全都画在地上。这个是给眼睛看的用来快速判断这个规划是不是疯了。Frenet 视图横轴 s、纵轴 d画出参考线边界、障碍物的 Frenet 矩形、候选轨迹族、最优轨迹。这个是用来判断采样是否合理、代价是否选对了。ST 图横轴 t、纵轴 s画出自车速度剖面、障碍物的 ST 带、DP 路径、QP 平滑后的结果。这个是用来判断速度规划的逻辑对不对。三个视图联动点击某条候选轨迹另外两个视图同步高亮。这套工具搭起来大概要两三天但后续每次排查问题能省几个小时。7.2 值得长期监控的几个指标跑通之后需要一些量化指标来判断系统是不是真的稳定。我自己会盯这几项指标含义警戒线帧间轨迹偏差相邻两帧在重叠时段的轨迹最大距离 0.3 m 需要关注候选淘汰率因硬约束被淘汰的候选占比 90% 说明约束过紧或采样失当最优候选的次优差距最优与次优的代价差占最优值的比例 5% 说明选点不稳易抖动横向 jerk 峰值实际执行的横向 jerk 最大值 5 m/s³ 乘客会不适曲率利用率实际曲率 / 曲率上限长期 0.8 说明规划过于激进第三项最优候选的次优差距是很多人不关注的但它对抖动特别有诊断价值。如果每帧的最优候选和次优候选代价只差 2%那么任何一点数值噪声都可能让它们换位表现出来就是轨迹跳变。这时候要么细化代价函数让区分度更高要么在代价里加一致性项。7.3 必须专门测的边界场景清单常规场景跑通不代表系统可用下面这些场景一定要单独构造测试参考线接缝正好落在自车位置把参考线的拼接点人为地放到自车当前位置附近测试投影的连续性。极小曲率半径的参考线比如路口转弯处的参考线曲率可能达到 0.1 以上测试 (1 − κ_r d) 的保护逻辑。自车正好在参考线上d ≈ 0测试 d 的符号判定是否稳定会不会在正负之间反复跳。静止障碍物正好在参考线切向方向测试 ST 图上膨胀带的生成以及 DP 是否能找到绕行或等待的解。横向偏移极大人为把目标横向偏移设到 5 米测试投影失败和钳位逻辑。速度为零到起步的过渡测试低速参数化切换的平滑性。这六个场景里我自己踩过坑的是第一个和第二个。第一个的表现是在某个特定位置的轨迹抖动非常难复现最后是靠打印参考线的 θ_r 序列才定位到的。第二个是直接导致规划器抛异常反而好查。最后分享一个我在实际项目里的体会。这套 Frenet 框架真正的难点不在于多项式、不在于 DP/QP、也不在于代价函数的形式这些都有成熟的做法。难点在于参考线这个隐形依赖的稳定性。它由上游模块生成但它决定了你所有公式的有效性参考线的角度连续性决定了 d 的信噪比参考线的曲率上限决定了 (1 − κ_r d) 会不会爆参考线的 s 分布一致性决定了跨帧轨迹能不能对齐。我后来养成了一个习惯每次规划模块出现问题第一个打开的日志不是规划模块自己的而是参考线模块的。把 θ_r(s)、κ_r(s)、点间距这三条曲线画出来看一眼十有八九问题就在那里。