开车的朋友大概都有过这种体验明明前面路是空的方向盘却总在微调一会儿往左偏一点一会儿往右偏一点坐车的人被晃得直皱眉。把这个场景搬到自动驾驶的轨迹规划模块里问题会更尖锐——车不仅要走得稳还得在几秒钟之内算出一条既不撞人、又不压线、还得让乘客舒服的路线。我做了几年路径规划相关的工作踩过的坑基本都集中在坐标系选择和动态障碍处理这两块。Frenet 坐标系下动态街道场景的最优轨迹生成说直白一点就是把“我该往哪走”这件事翻译成“沿着道路往前多少米、横向偏出多少米”这两个数字然后在这个二维空间里搜索一条代价最小的曲线同时把周围会动的车、人、自行车都当成随时间变化的空间占用区域躲开它们。这个思路适用的场景非常具体城市道路、有明确车道线或可提取中心线的结构化路段、中低速行驶大致 0~60km/h、周围存在动态交通参与者。如果你是做自动驾驶规控的工程师、机器人移动底盘方向的学生或者正在把一套扫地机式的规划器往室外场景迁移下面这些内容应该能直接拿去用。我会把数学推导里最容易翻车的地方、代码里最耗时的部分、以及那些只有真上过车才知道的调参经验都摊开讲。1. 为什么偏偏是 Frenet 坐标系从笛卡尔坐标的三大尴尬说起1.1 笛卡尔坐标下的轨迹规划到底卡在哪在笛卡尔坐标系里描述车辆我们习惯写成 (x, y, θ, v, a)。这套表达最直观但如果直接拿它做优化变量问题马上就来了。第一条尴尬是参考线约束没法自然表达。车辆必须沿着道路走这是一个沿路方向的强约束而在笛卡尔坐标里“沿着道路”只能用一系列位置点或者复杂的曲线不等式来近似优化器处理起来非常别扭。第二条尴尬是横向和纵向的运动耦合在一起你在 x 方向上调整一个值y 方向的可行性也跟着变曲率约束、道路边界约束全都拧在一块求解复杂度直接上一个台阶。第三条尴尬是障碍物的表达。一个行驶中的前车在笛卡尔坐标里是一个随时间移动的多边形加上一段预测轨迹你很难用一个简洁的代数形式把它写成约束。我做第一个版本规划器的时候就是用笛卡尔坐标硬做结果发现每加一个约束求解时间就往上涨最后在一个十字路口场景里直接超时。后来换成 Frenet 坐标同一台机器上求解时间掉了将近一半而且代码可读性好了非常多。这不是说 Frenet 万能而是它把问题放到了“自然的坐标系”里。1.2 Frenet 坐标系和参考线的关系一句话讲透网上关于“frenet 坐标系和参考线的关系”的讨论很多我自己的理解是参考线提供了一把尺子Frenet 坐标系是这把尺子上的刻度。具体来说参考线是一串按行驶方向排好序的离散点每个点包含 (x, y, θ_r, κ_r, s)其中 s 是从起点算起的弧长θ_r 是切向角κ_r 是曲率。有了这条线空间中任意一点都可以用两个量描述s也就是这一点在参考线上投影位置的弧长l也就是这一点到参考线的横向偏移向左为正、向右为负正负约定要统一否则后面全是 bug。也就是说Frenet 坐标把二维平面“拉直”成了一条带子纵向是沿着路往前延伸横向是垂直于路的方向。这个变换带来的最大好处是解耦。道路边界通常可以写成 l ≤ l_max 和 l ≥ l_min 的简单不等式而且 l_max、l_min 只随 s 变化不随其他东西变。车道保持就是让 l 尽量接近 0 或某个车道中心变道就是让 l 从当前值平滑过渡到目标车道中心。这些在 Frenet 空间里都是最自然的表达。同理纵向的速度控制、跟车距离控制全都在 s 方向上单独处理。1.3 什么场景该用、什么场景别硬上Frenet 坐标系有一个前提存在一条合理的参考线。城市结构化道路、高速公路、封闭园区道路这些场景车道线清晰参考线提取很稳定用起来非常舒服。但如果是在无标线的停车场、野外越野、完全开放的空地参考线本身就很难提取或者提取出来的线频繁跳变这时候硬用 Frenet 反而会引入噪声。我的经验是参考线的横向抖动超过 0.3 米、或者曲率符号频繁翻转的路段要先做参考线平滑和重采样再进规划否则规划器会一直输出左右摆动的轨迹。还有一个容易忽略的点参考线是“静态”的但我们的规划是分周期滚动的。每个规划周期通常 100ms都要重新提取一次参考线如果两次提取结果差异很大轨迹就会跳。所以工程上一般会把参考线做一次缓存和平滑只在偏离超过阈值时才更新这个细节后面还会展开讲。2. 核心数学细节坐标转换、参数化与曲线构造2.1 参考线的离散化与弧长参数化参考线的质量决定了后面一切。原始输入可能是高精地图的车道中心线也可能是感知出来的车道线拟合结果点间隔可能不均匀。第一步必须做重采样按固定间隔我常用 0.5 米插值出新的点列并保证 s 单调递增这样后面做最近点查找和插值才稳。重采样之后要算每点的三个量切向角 θ_r、曲率 κ_r、弧长 s。θ_r 用相邻点差分θ_r[i] atan2(y[i1] - y[i-1], x[i1] - x[i-1])用中心差分比前向差分稳定得多。κ_r 用切向角的变化率除以弧长κ_r[i] (θ_r[i1] - θ_r[i-1]) / (s[i1] - s[i-1])。这里要注意角度要解卷绕unwrap否则在 ±π 附近会出现巨大的假曲率我第一版就栽在这导致车辆在接近直线段时突然“发抖”。s 就是逐点累加欧氏距离。注意曲率一定要做一次低通滤波或滑动平均。原始差分出来的曲率噪声很大直接拿去算横向加速度会导致轨迹抖动。2.2 笛卡尔与 Frenet 互转公式、近似和真实工程做法从笛卡尔转到 Frenet核心是找最近点。常见做法是先按粗略的 s 定位比如用 GPS 或者上一周期的结果做初值然后在附近几个点上找距离最小的点再在相邻两点之间做投影。投影的方式是把待转换点到线段两端点的连线算垂足位置判断垂足是否落在线段内落在里面就取垂足否则取端点。找到投影点之后位置分量很简单l (x - x_r) * (-sin θ_r) (y - y_r) * cos θ_r也就是点到参考线的向量在法向上的投影。s s_r 投影长度。麻烦的是速度和加速度的转换。设车辆速度为 v、航向角为 θ参考线切向角为 θ_r横向偏移为 l曲率为 κ定义航向偏差 θ_e θ - θ_r那么有关系ṡ v * cos θ_e / (1 - κ * l)l̇ v * sin θ_e这两个式子很重要因为它解释了为什么高速过弯时横向偏移会“放大”纵向速度。当 κl 接近 1 的时候分母趋近 0ṡ 会爆炸。物理含义是当你的横向偏移等于曲率半径时实际上你已经绕到了曲率中心坐标系退化了。工程上必须给 1 - κl 设一个下限比如 0.2低于这个值直接判定该点无效丢弃这个候选。我见过不止一个项目在这里没做保护结果在急弯处规划器输出了一个 NaN整条轨迹作废。加速度转换会更长完整解析式里包含曲率导数 κ 的修正项。我的实际做法是如果曲率变化平缓相邻点曲率差很小可以用忽略 κ 的简化式如果要精细我更倾向于不手推解析式而是在 Frenet 空间里直接对 s 和 l 的序列做数值差分得到 ṡ、l̇、s̈、l̈然后做一次滑动平均或卡尔曼滤波去噪。这样做的好处是代码短、不容易出错代价是差分会放大噪声。实测下来在规划周期 100ms、参考线间隔 0.5 米的条件下数值差分加五点平滑的效果足够用横向加速度的误差大概在 0.1 m/s² 量级比手推公式写错了要好得多。反方向转换Frenet 转笛卡尔相对简单x x_r - l * sin θ_ry y_r l * cos θ_r做轨迹输出的时候一定要用这个式子把 (s, l) 序列还原成 (x, y) 序列然后再做碰撞检测和可视化。我建议在这里加一个自检把还原后的点再转回 Frenet看 s、l 的误差是否在 1e-3 以内超过就报警这能帮你快速发现 θ_r 符号或索引错位的 bug。2.3 横向纵向解耦后为什么用多项式而不是样条解耦之后横向轨迹通常写成 l(s) 的函数纵向写成 s(t) 的函数。选多项式而不是贝塞尔或 B 样条主要原因是边界条件可以精确满足。五次多项式有六个系数正好可以同时约束起点和终点的位置、一阶导、二阶导。对于横向运动起点是当前 l、l̇、l̈终点是目标 l_target、0、0五点或六点边界条件刚好凑齐。横向五次多项式的一般形式l(s) a0 a1 s a2 s² a3 s³ a4 s⁴ a5 s⁵给定 s 从 0 到 Δs起点 (l0, l0, l0)终点 (l1, l1, l1)可以解出唯一的系数向量。这个解在数学上就是横向最小 jerk 问题的最优解代价函数 ∫ (l)² ds 最小所以采出来的轨迹天然平滑。这也是为什么这个框架被叫做“最优轨迹生成”——在给定边界条件下它确实是最优的只是最优性建立在横向和纵向可以分开处理这个假设上。纵向通常用四次多项式 s(t)因为速度的边界条件比横向少一个一般不需要约束 s 的二阶导四次就够。但如果你希望加加速度连续jerk 连续就用五次。巡航、跟车、超车、停车这几种模式本质上就是给终点状态赋不同的值然后用同一套多项式求解器算出来。提示采样时不要只采终点位置要采终点状态的组合。只采位置会得到一堆形状相似、但速度不匹配的轨迹最后评价函数只能靠硬凑权重来区分很难调。3. 动态街道场景建模把会动的东西变成可计算的约束3.1 动静态障碍物在 Frenet 空间里的投影静态障碍物好办把它投影到 (s, l) 平面就是一个矩形或一组离散点规划时候直接检查候选轨迹是否进入这个区域就行。真正麻烦的是动态障碍物因为它们的位置随时间变化。主流做法是把动态障碍物的预测轨迹投影到 s-t 平面形成“时空占用矩形”。具体来说对某个障碍物它的预测轨迹是一系列 (t, x, y, v, θ) 的点把每个点转成 (t, s, l)然后在 s-t 平面上对于每个时刻 t障碍物在 s 方向占据的区间是 [s_center - L/2 - margin, s_center L/2 margin]其中 L 是障碍物沿参考线方向的长度。把所有时刻的区间连起来就得到一个随时间变化的 s 区间带。规划出来的轨迹是一条 s(t) 曲线只要它在任意时刻都不落在某个障碍物的 s 区间内就说明纵向没有冲突。横向同理可以在 l-s 平面上投影。这样做的好处是把三维问题x, y, t拆成两个二维问题分别检查计算量大幅下降。代价是丢了圆形障碍物在斜向的相对关系所以最后一定要用真正的多边形碰撞检测做一次复核二维投影只用于快速剪枝。3.2 动态障碍物的不确定性怎么处理预测永远不准。一个行人可能站着不动也可能突然迈步前车可能匀速也可能急刹。工程上常见的有三种处理方式第一种是膨胀法。直接把障碍物的尺寸在预测基础上膨胀一圈比如横向加 0.5 米、纵向加 1.0 米把不确定性吃掉。简单粗暴但会损失通行空间窄路会车的时候经常导致规划器找不到可行解。第二种是多模态假设。给每个障碍物生成多条可能的预测轨迹直行、左偏、右偏、减速、加速然后对每一种组合做一次规划取最好的。这样通行能力强但组合数会爆炸。实践中一般只对最危险的 1~2 个障碍物做多模态其余还是单模态膨胀。第三种是概率约束。用高斯过程或者卡尔曼滤波给出预测的协方差把碰撞概率控制在某个阈值以内。这个理论上漂亮但工程落地时协方差很难标定得准最后往往还是会退化成定值膨胀。我自己的做法是混合对行人、自行车这类高机动性目标用较大的横向膨胀加上多模态对四轮车用较小的膨胀加单模态但把跟车距离调大一些。实测下来这个组合在城区道路上比较平衡既不会频繁卡死也不会太激进。3.3 约束怎么写成优化问题的一部分把上面这些东西组织成约束大致是这样的一个结构道路边界约束l_min(s) ≤ l(s) ≤ l_max(s)逐点检查。曲率约束|κ| ≤ κ_max由横向加速度反推κ_max a_lat_max / v²。注意 v 在变所以曲率约束其实是速度相关的。碰撞约束候选轨迹的时空占用与障碍物时空占用不相交。动力学约束横向加速度、纵向加速度、jerk 都在车辆能力范围内。运动学约束对于非完整约束车辆轨迹的曲率要与航向变化一致这个在横向五次多项式里天然满足因为 l(s) 的二阶导和曲率有解析关系。其中曲率到横向加速度的换算要特别注意符号。横向加速度 a_lat v² * κ其中 κ 是轨迹曲率不是参考线曲率。轨迹曲率 κ_traj 与 l(s) 的关系在参考线曲率较小时可以近似为 κ_traj ≈ l但当参考线本身弯的时候需要加上参考线曲率项。我一般用 κ_traj ≈ κ_r l / (1 l²)^1.5 这个形式l 很小时第二项就退化成 l。注意横向加速度上限别直接取 3.0 m/s²。乘客舒适性阈值通常在 1.5~2.0 m/s²超过 2.5 m/s² 大部分人就会明显感到被甩。紧急避障可以放宽但要用单独的代价惩罚项标记出来方便后续复盘。4. 最优轨迹生成实操流程从采样到落地4.1 采样-评价框架怎么搭这套框架的骨架其实很朴素生成一批候选轨迹逐条算代价取最小代价的那条再做一次平滑输出。难点全在“生成”和“评价”两个环节的细节上。纵向采样我一般这样设计。先根据当前状态和场景决定一个目标速度 v_target比如跟车时取前车速度、巡航时取限速、接近红绿灯时取减速曲线。然后围绕 v_target 上下各采 2~3 个值形成速度候选集。同时采几个不同的到达时间 t_arrival因为同样的终点位置配不同时间得到的速度曲线形状完全不同。我的经验是速度采 ±20% 分三档、时间采 ±15% 分三档比较合适再细就只是增加计算量收益很小。横向采样用终点状态的方式更高效。不是把 l 的每个值都试一遍而是采一组 (l_target, l_target, l_target)。常见的几类横向意图l_targetl_targetl_target适用场景保持当前车道当前 l 或车道中心00直行、跟车向左变道左侧车道中心00超车、避让向右变道右侧车道中心00让行、靠边小幅避让当前 l ± 0.300绕过井盖、锥桶保持现状l0l0l0紧急情况下的兜底这样每类意图下只要采 1~3 个 Δs总共也就十几条横向轨迹。纵向十几条乘以横向十几条两百条左右的候选项在一般的车载算力上做碰撞检测和评价100ms 周期内足够跑完。4.2 代价函数怎么设计才不会互相打架代价函数是这套框架里最玄学的部分。我的建议是分层设计避免所有项平铺在一起然后互相拉扯。第一层是硬约束不满足就直接淘汰比如越界、碰撞、超过加速度极限。这一层用布尔判断不进入代价。第二层是安全性代价包括与障碍物的距离、与前车的时距。距离项用反比例或者指数衰减保证离得越近代价涨得越快形成一个软墙。我常用 J_obs w_obs * exp(-d / d0)d0 取 2~3 米。第三层是舒适性代价主要是横向加速度平方积分、纵向加速度平方积分、jerk 平方积分三项。这三项的量纲不一样需要归一化。我的做法是用各自的典型最大值做分母把每一项压到 0~1 之间再加权。第四层是效率代价包括速度偏差和时间。J_v w_v * (v - v_target)²J_t w_t * t_arrival。权重标定我推荐从一个大到小的顺序调先只留硬约束和碰撞代价确认能安全跑再加舒适性把横向加速度压下来最后加效率。一上来就把所有项全开你根本分不清是哪一项导致的行为异常。我给一组起步值参考w_obs 10, w_lat_acc 1.0, w_lon_acc 0.5, w_jerk 0.8, w_l 0.3, w_v 0.5, w_t 0.1。这是量级参考实际要按你的归一化方式缩放。提示代价函数里保留一个“调试项”比如把每条候选轨迹的各项代价都记录到日志里。上线前分析行为异常时能直接把日志拉出来看是哪一项在主导比在车上盲调强太多。4.3 碰撞检测与轨迹平滑的最后一道关候选轨迹做碰撞检测一定要考虑时间维度。把自车按时间步长比如 0.1 秒展开成一系列位姿每个位姿用一个矩形或者三个圆来近似然后检查每一帧是否与障碍物的预测位姿重叠。用三个圆近似车体的做法很常见前圆、中圆、后圆半径取车宽的一半加安全裕度。这样做的好处是碰撞检测退化成圆与圆的距离比较速度极快。缺点是车头车尾的角落会有“切角”误差所以安全裕度要给足一点一般 0.2~0.3 米。矩形的话可以用分离轴定理SAT精度高但慢一些。我的经验是先用圆做粗筛对通过粗筛的轨迹再用 SAT 精算能省下大量时间。碰撞检测通过之后还有一步平滑。多项式采样的轨迹虽然平滑但不同周期之间选中的轨迹可能在连接处有跳变表现为方向盘的小幅抖动。常见的处理是在输出前做一次时间上的插值平滑比如用上一周期的轨迹和新轨迹做加权融合融合系数取 0.2~0.3。但要注意平滑不能破坏碰撞检测的结论所以融合之后的轨迹必须重新做一次碰撞检测这一步绝对不能省。我见过为了省时间跳过后检导致的擦碰事故教训很直接。5. 常见问题与排查技巧实录5.1 典型 Bug 速查表这套框架实现下来出问题的地方高度集中在几个位置。我把踩过的典型问题整理成表出问题时可以对着查。现象最可能的原因排查方法轨迹周期性左右抖动参考线每周期重新提取点序或起点不一致打印相邻周期的参考线起点 s 和首点坐标急弯处轨迹作废或 NaN1 - κ*l 分母接近 0 未做保护检查 κ*l 的最大值加上限截断车辆在直线段“发抖”曲率差分时角度未解卷绕检查 θ_r 序列是否有 ±π 跳变规划器找不到可行解障碍物膨胀过大或时间采样太密打印候选淘汰原因统计看是碰撞淘汰还是边界淘汰跟车距离忽远忽近纵向代价里速度项权重过大减小 w_v增大时间项权重变道过程横向过冲横向终点 l 约束缺失确认终点边界条件是否设了二阶导为零输出轨迹与可视化不一致Frenet 转笛卡尔的 θ_r 索引错位用自检函数把 (s,l) 转回来比对计算超时候选数量过多或碰撞检测未做粗筛统计各阶段耗时先加圆粗筛这张表里的每一条我基本都真实遇到过。最坑的是第一条和最后一条一个导致行为诡异但难复现一个导致偶发超时都很难查。5.2 曲率与横向偏移的那几个数值陷阱回到前面提到的分母问题这里展开讲。1 - κ*l 这个因子在整个框架里出现频率极高横向速度、纵向速度、加速度转换都要用。它出问题的方式有两种一种是直接等于零或负数导致方向反转另一种是接近零但不为零导致数值巨大。我的处理方式是在参考线构建阶段就算出每个点的 κ 和 s 方向上的最大允许横向偏移取 l_limit 0.9 / max(|κ|, 1e-3)。规划时候选轨迹的 l 值如果超过这个限制直接淘汰。这个限制比道路边界往往更紧所以能提前发现坐标系退化区域。还有一个陷阱是曲率的符号约定。参考线曲率如果定义成“左转为正”那么横向偏移也必须是“左正右负”否则轨迹曲率算出来符号全反。我在两个项目里见过不同的约定混用一次结果是车辆在弯道里往外冲。所以接手任何一套代码第一件事就是找这两个约定的定义写一行注释钉死在文件头上。5.3 参数标定与验证该怎么下手参数标定别一上来就车调先在离线回放里做。把一段真实的路测数据包含感知结果、定位结果录下来然后让规划器离线跑把输出轨迹和实际行驶轨迹叠在一起看。这样能快速筛掉大部分明显不合理的参数组合。离线验证我一般看几个指标横向加速度的 95 分位值、纵向加速度的 95 分位值、jerk 的最大值、最小障碍物距离、规划失败率、单周期计算耗时。前三个反映舒适性第四个反映安全性后两个反映可用性。这几个指标一起看基本能判断一组参数能不能用。上车之后先做低速场景20km/h 以内确认基本行为正确再逐步提速。城区的挑战主要来自两点一是路口内没有车道线参考线需要靠推理生成二是行人和非机动车的预测非常不准。我的做法是在路口区域把横向膨胀临时放大 30%同时把速度上限压低宁可慢一点也别冒险。注意每次改参数一定要记录改之前之后的几个关键指标。凭感觉调参最后会陷入“改了一个问题冒出三个问题”的循环有数据对照才走得出来。6. 从跑通到好用几个个人体会第一点体会是关于代码结构的。这套框架里坐标转换、参考线管理、采样器、评价器、碰撞检测这五个模块一定要彻底分开。我早期版本把它们耦合在一起结果每次调参都要翻遍整个文件。拆开之后每个模块单独写单元测试坐标转换用几组手工算好的数据验证碰撞检测用构造的矩形对验证出问题时定位快得多。第二点体会是关于“最优”这个词的。数学意义上的最优轨迹在真实场景里往往不是最舒服的那条。因为评价函数里的权重是我们主观设的而乘客的感受又很难量化。我的做法是保留一个 Pareto 前沿的概念不追求单条最优而是在安全性代价接近的候选里选舒适性最好的那条。这样做出来的车行为风格会更接近人类司机不会有那种“精确但生硬”的感觉。第三点体会是关于失败兜底的。再好的规划器都会有找不到可行解的时候这时候必须有一条确定安全的兜底轨迹通常是“保持当前横向位置、以可控减速度减速到停”。这条轨迹不参与评价直接由规划器外部注入。千万不要让规划器在无解时输出上一周期的轨迹继续跑因为上一周期的轨迹是针对上一时刻的障碍物算的可能已经不安全了。最后聊一个后续可以做扩展的方向把这条采样-评价的流水线换成基于优化的方法比如把采样得到的次优解作为二次规划的热启动用 QP 在连续空间里精修。这样能得到更平滑的结果同时因为有采样解兜底求解失败的频率也会低很多。我自己在小范围试过横向加速度的波动能再降一档代价是计算时间增加需要看具体平台的算力预算。 
长河君 · 2026/9/29 9:00:14 · 阅读 1.2k 自动驾驶轨迹规划：Frenet坐标系动态场景最优轨迹生成
开车的朋友大概都有过这种体验明明前面路是空的方向盘却总在微调一会儿往左偏一点一会儿往右偏一点坐车的人被晃得直皱眉。把这个场景搬到自动驾驶的轨迹规划模块里问题会更尖锐——车不仅要走得稳还得在几秒钟之内算出一条既不撞人、又不压线、还得让乘客舒服的路线。我做了几年路径规划相关的工作踩过的坑基本都集中在坐标系选择和动态障碍处理这两块。Frenet 坐标系下动态街道场景的最优轨迹生成说直白一点就是把“我该往哪走”这件事翻译成“沿着道路往前多少米、横向偏出多少米”这两个数字然后在这个二维空间里搜索一条代价最小的曲线同时把周围会动的车、人、自行车都当成随时间变化的空间占用区域躲开它们。这个思路适用的场景非常具体城市道路、有明确车道线或可提取中心线的结构化路段、中低速行驶大致 0~60km/h、周围存在动态交通参与者。如果你是做自动驾驶规控的工程师、机器人移动底盘方向的学生或者正在把一套扫地机式的规划器往室外场景迁移下面这些内容应该能直接拿去用。我会把数学推导里最容易翻车的地方、代码里最耗时的部分、以及那些只有真上过车才知道的调参经验都摊开讲。1. 为什么偏偏是 Frenet 坐标系从笛卡尔坐标的三大尴尬说起1.1 笛卡尔坐标下的轨迹规划到底卡在哪在笛卡尔坐标系里描述车辆我们习惯写成 (x, y, θ, v, a)。这套表达最直观但如果直接拿它做优化变量问题马上就来了。第一条尴尬是参考线约束没法自然表达。车辆必须沿着道路走这是一个沿路方向的强约束而在笛卡尔坐标里“沿着道路”只能用一系列位置点或者复杂的曲线不等式来近似优化器处理起来非常别扭。第二条尴尬是横向和纵向的运动耦合在一起你在 x 方向上调整一个值y 方向的可行性也跟着变曲率约束、道路边界约束全都拧在一块求解复杂度直接上一个台阶。第三条尴尬是障碍物的表达。一个行驶中的前车在笛卡尔坐标里是一个随时间移动的多边形加上一段预测轨迹你很难用一个简洁的代数形式把它写成约束。我做第一个版本规划器的时候就是用笛卡尔坐标硬做结果发现每加一个约束求解时间就往上涨最后在一个十字路口场景里直接超时。后来换成 Frenet 坐标同一台机器上求解时间掉了将近一半而且代码可读性好了非常多。这不是说 Frenet 万能而是它把问题放到了“自然的坐标系”里。1.2 Frenet 坐标系和参考线的关系一句话讲透网上关于“frenet 坐标系和参考线的关系”的讨论很多我自己的理解是参考线提供了一把尺子Frenet 坐标系是这把尺子上的刻度。具体来说参考线是一串按行驶方向排好序的离散点每个点包含 (x, y, θ_r, κ_r, s)其中 s 是从起点算起的弧长θ_r 是切向角κ_r 是曲率。有了这条线空间中任意一点都可以用两个量描述s也就是这一点在参考线上投影位置的弧长l也就是这一点到参考线的横向偏移向左为正、向右为负正负约定要统一否则后面全是 bug。也就是说Frenet 坐标把二维平面“拉直”成了一条带子纵向是沿着路往前延伸横向是垂直于路的方向。这个变换带来的最大好处是解耦。道路边界通常可以写成 l ≤ l_max 和 l ≥ l_min 的简单不等式而且 l_max、l_min 只随 s 变化不随其他东西变。车道保持就是让 l 尽量接近 0 或某个车道中心变道就是让 l 从当前值平滑过渡到目标车道中心。这些在 Frenet 空间里都是最自然的表达。同理纵向的速度控制、跟车距离控制全都在 s 方向上单独处理。1.3 什么场景该用、什么场景别硬上Frenet 坐标系有一个前提存在一条合理的参考线。城市结构化道路、高速公路、封闭园区道路这些场景车道线清晰参考线提取很稳定用起来非常舒服。但如果是在无标线的停车场、野外越野、完全开放的空地参考线本身就很难提取或者提取出来的线频繁跳变这时候硬用 Frenet 反而会引入噪声。我的经验是参考线的横向抖动超过 0.3 米、或者曲率符号频繁翻转的路段要先做参考线平滑和重采样再进规划否则规划器会一直输出左右摆动的轨迹。还有一个容易忽略的点参考线是“静态”的但我们的规划是分周期滚动的。每个规划周期通常 100ms都要重新提取一次参考线如果两次提取结果差异很大轨迹就会跳。所以工程上一般会把参考线做一次缓存和平滑只在偏离超过阈值时才更新这个细节后面还会展开讲。2. 核心数学细节坐标转换、参数化与曲线构造2.1 参考线的离散化与弧长参数化参考线的质量决定了后面一切。原始输入可能是高精地图的车道中心线也可能是感知出来的车道线拟合结果点间隔可能不均匀。第一步必须做重采样按固定间隔我常用 0.5 米插值出新的点列并保证 s 单调递增这样后面做最近点查找和插值才稳。重采样之后要算每点的三个量切向角 θ_r、曲率 κ_r、弧长 s。θ_r 用相邻点差分θ_r[i] atan2(y[i1] - y[i-1], x[i1] - x[i-1])用中心差分比前向差分稳定得多。κ_r 用切向角的变化率除以弧长κ_r[i] (θ_r[i1] - θ_r[i-1]) / (s[i1] - s[i-1])。这里要注意角度要解卷绕unwrap否则在 ±π 附近会出现巨大的假曲率我第一版就栽在这导致车辆在接近直线段时突然“发抖”。s 就是逐点累加欧氏距离。注意曲率一定要做一次低通滤波或滑动平均。原始差分出来的曲率噪声很大直接拿去算横向加速度会导致轨迹抖动。2.2 笛卡尔与 Frenet 互转公式、近似和真实工程做法从笛卡尔转到 Frenet核心是找最近点。常见做法是先按粗略的 s 定位比如用 GPS 或者上一周期的结果做初值然后在附近几个点上找距离最小的点再在相邻两点之间做投影。投影的方式是把待转换点到线段两端点的连线算垂足位置判断垂足是否落在线段内落在里面就取垂足否则取端点。找到投影点之后位置分量很简单l (x - x_r) * (-sin θ_r) (y - y_r) * cos θ_r也就是点到参考线的向量在法向上的投影。s s_r 投影长度。麻烦的是速度和加速度的转换。设车辆速度为 v、航向角为 θ参考线切向角为 θ_r横向偏移为 l曲率为 κ定义航向偏差 θ_e θ - θ_r那么有关系ṡ v * cos θ_e / (1 - κ * l)l̇ v * sin θ_e这两个式子很重要因为它解释了为什么高速过弯时横向偏移会“放大”纵向速度。当 κl 接近 1 的时候分母趋近 0ṡ 会爆炸。物理含义是当你的横向偏移等于曲率半径时实际上你已经绕到了曲率中心坐标系退化了。工程上必须给 1 - κl 设一个下限比如 0.2低于这个值直接判定该点无效丢弃这个候选。我见过不止一个项目在这里没做保护结果在急弯处规划器输出了一个 NaN整条轨迹作废。加速度转换会更长完整解析式里包含曲率导数 κ 的修正项。我的实际做法是如果曲率变化平缓相邻点曲率差很小可以用忽略 κ 的简化式如果要精细我更倾向于不手推解析式而是在 Frenet 空间里直接对 s 和 l 的序列做数值差分得到 ṡ、l̇、s̈、l̈然后做一次滑动平均或卡尔曼滤波去噪。这样做的好处是代码短、不容易出错代价是差分会放大噪声。实测下来在规划周期 100ms、参考线间隔 0.5 米的条件下数值差分加五点平滑的效果足够用横向加速度的误差大概在 0.1 m/s² 量级比手推公式写错了要好得多。反方向转换Frenet 转笛卡尔相对简单x x_r - l * sin θ_ry y_r l * cos θ_r做轨迹输出的时候一定要用这个式子把 (s, l) 序列还原成 (x, y) 序列然后再做碰撞检测和可视化。我建议在这里加一个自检把还原后的点再转回 Frenet看 s、l 的误差是否在 1e-3 以内超过就报警这能帮你快速发现 θ_r 符号或索引错位的 bug。2.3 横向纵向解耦后为什么用多项式而不是样条解耦之后横向轨迹通常写成 l(s) 的函数纵向写成 s(t) 的函数。选多项式而不是贝塞尔或 B 样条主要原因是边界条件可以精确满足。五次多项式有六个系数正好可以同时约束起点和终点的位置、一阶导、二阶导。对于横向运动起点是当前 l、l̇、l̈终点是目标 l_target、0、0五点或六点边界条件刚好凑齐。横向五次多项式的一般形式l(s) a0 a1 s a2 s² a3 s³ a4 s⁴ a5 s⁵给定 s 从 0 到 Δs起点 (l0, l0, l0)终点 (l1, l1, l1)可以解出唯一的系数向量。这个解在数学上就是横向最小 jerk 问题的最优解代价函数 ∫ (l)² ds 最小所以采出来的轨迹天然平滑。这也是为什么这个框架被叫做“最优轨迹生成”——在给定边界条件下它确实是最优的只是最优性建立在横向和纵向可以分开处理这个假设上。纵向通常用四次多项式 s(t)因为速度的边界条件比横向少一个一般不需要约束 s 的二阶导四次就够。但如果你希望加加速度连续jerk 连续就用五次。巡航、跟车、超车、停车这几种模式本质上就是给终点状态赋不同的值然后用同一套多项式求解器算出来。提示采样时不要只采终点位置要采终点状态的组合。只采位置会得到一堆形状相似、但速度不匹配的轨迹最后评价函数只能靠硬凑权重来区分很难调。3. 动态街道场景建模把会动的东西变成可计算的约束3.1 动静态障碍物在 Frenet 空间里的投影静态障碍物好办把它投影到 (s, l) 平面就是一个矩形或一组离散点规划时候直接检查候选轨迹是否进入这个区域就行。真正麻烦的是动态障碍物因为它们的位置随时间变化。主流做法是把动态障碍物的预测轨迹投影到 s-t 平面形成“时空占用矩形”。具体来说对某个障碍物它的预测轨迹是一系列 (t, x, y, v, θ) 的点把每个点转成 (t, s, l)然后在 s-t 平面上对于每个时刻 t障碍物在 s 方向占据的区间是 [s_center - L/2 - margin, s_center L/2 margin]其中 L 是障碍物沿参考线方向的长度。把所有时刻的区间连起来就得到一个随时间变化的 s 区间带。规划出来的轨迹是一条 s(t) 曲线只要它在任意时刻都不落在某个障碍物的 s 区间内就说明纵向没有冲突。横向同理可以在 l-s 平面上投影。这样做的好处是把三维问题x, y, t拆成两个二维问题分别检查计算量大幅下降。代价是丢了圆形障碍物在斜向的相对关系所以最后一定要用真正的多边形碰撞检测做一次复核二维投影只用于快速剪枝。3.2 动态障碍物的不确定性怎么处理预测永远不准。一个行人可能站着不动也可能突然迈步前车可能匀速也可能急刹。工程上常见的有三种处理方式第一种是膨胀法。直接把障碍物的尺寸在预测基础上膨胀一圈比如横向加 0.5 米、纵向加 1.0 米把不确定性吃掉。简单粗暴但会损失通行空间窄路会车的时候经常导致规划器找不到可行解。第二种是多模态假设。给每个障碍物生成多条可能的预测轨迹直行、左偏、右偏、减速、加速然后对每一种组合做一次规划取最好的。这样通行能力强但组合数会爆炸。实践中一般只对最危险的 1~2 个障碍物做多模态其余还是单模态膨胀。第三种是概率约束。用高斯过程或者卡尔曼滤波给出预测的协方差把碰撞概率控制在某个阈值以内。这个理论上漂亮但工程落地时协方差很难标定得准最后往往还是会退化成定值膨胀。我自己的做法是混合对行人、自行车这类高机动性目标用较大的横向膨胀加上多模态对四轮车用较小的膨胀加单模态但把跟车距离调大一些。实测下来这个组合在城区道路上比较平衡既不会频繁卡死也不会太激进。3.3 约束怎么写成优化问题的一部分把上面这些东西组织成约束大致是这样的一个结构道路边界约束l_min(s) ≤ l(s) ≤ l_max(s)逐点检查。曲率约束|κ| ≤ κ_max由横向加速度反推κ_max a_lat_max / v²。注意 v 在变所以曲率约束其实是速度相关的。碰撞约束候选轨迹的时空占用与障碍物时空占用不相交。动力学约束横向加速度、纵向加速度、jerk 都在车辆能力范围内。运动学约束对于非完整约束车辆轨迹的曲率要与航向变化一致这个在横向五次多项式里天然满足因为 l(s) 的二阶导和曲率有解析关系。其中曲率到横向加速度的换算要特别注意符号。横向加速度 a_lat v² * κ其中 κ 是轨迹曲率不是参考线曲率。轨迹曲率 κ_traj 与 l(s) 的关系在参考线曲率较小时可以近似为 κ_traj ≈ l但当参考线本身弯的时候需要加上参考线曲率项。我一般用 κ_traj ≈ κ_r l / (1 l²)^1.5 这个形式l 很小时第二项就退化成 l。注意横向加速度上限别直接取 3.0 m/s²。乘客舒适性阈值通常在 1.5~2.0 m/s²超过 2.5 m/s² 大部分人就会明显感到被甩。紧急避障可以放宽但要用单独的代价惩罚项标记出来方便后续复盘。4. 最优轨迹生成实操流程从采样到落地4.1 采样-评价框架怎么搭这套框架的骨架其实很朴素生成一批候选轨迹逐条算代价取最小代价的那条再做一次平滑输出。难点全在“生成”和“评价”两个环节的细节上。纵向采样我一般这样设计。先根据当前状态和场景决定一个目标速度 v_target比如跟车时取前车速度、巡航时取限速、接近红绿灯时取减速曲线。然后围绕 v_target 上下各采 2~3 个值形成速度候选集。同时采几个不同的到达时间 t_arrival因为同样的终点位置配不同时间得到的速度曲线形状完全不同。我的经验是速度采 ±20% 分三档、时间采 ±15% 分三档比较合适再细就只是增加计算量收益很小。横向采样用终点状态的方式更高效。不是把 l 的每个值都试一遍而是采一组 (l_target, l_target, l_target)。常见的几类横向意图l_targetl_targetl_target适用场景保持当前车道当前 l 或车道中心00直行、跟车向左变道左侧车道中心00超车、避让向右变道右侧车道中心00让行、靠边小幅避让当前 l ± 0.300绕过井盖、锥桶保持现状l0l0l0紧急情况下的兜底这样每类意图下只要采 1~3 个 Δs总共也就十几条横向轨迹。纵向十几条乘以横向十几条两百条左右的候选项在一般的车载算力上做碰撞检测和评价100ms 周期内足够跑完。4.2 代价函数怎么设计才不会互相打架代价函数是这套框架里最玄学的部分。我的建议是分层设计避免所有项平铺在一起然后互相拉扯。第一层是硬约束不满足就直接淘汰比如越界、碰撞、超过加速度极限。这一层用布尔判断不进入代价。第二层是安全性代价包括与障碍物的距离、与前车的时距。距离项用反比例或者指数衰减保证离得越近代价涨得越快形成一个软墙。我常用 J_obs w_obs * exp(-d / d0)d0 取 2~3 米。第三层是舒适性代价主要是横向加速度平方积分、纵向加速度平方积分、jerk 平方积分三项。这三项的量纲不一样需要归一化。我的做法是用各自的典型最大值做分母把每一项压到 0~1 之间再加权。第四层是效率代价包括速度偏差和时间。J_v w_v * (v - v_target)²J_t w_t * t_arrival。权重标定我推荐从一个大到小的顺序调先只留硬约束和碰撞代价确认能安全跑再加舒适性把横向加速度压下来最后加效率。一上来就把所有项全开你根本分不清是哪一项导致的行为异常。我给一组起步值参考w_obs 10, w_lat_acc 1.0, w_lon_acc 0.5, w_jerk 0.8, w_l 0.3, w_v 0.5, w_t 0.1。这是量级参考实际要按你的归一化方式缩放。提示代价函数里保留一个“调试项”比如把每条候选轨迹的各项代价都记录到日志里。上线前分析行为异常时能直接把日志拉出来看是哪一项在主导比在车上盲调强太多。4.3 碰撞检测与轨迹平滑的最后一道关候选轨迹做碰撞检测一定要考虑时间维度。把自车按时间步长比如 0.1 秒展开成一系列位姿每个位姿用一个矩形或者三个圆来近似然后检查每一帧是否与障碍物的预测位姿重叠。用三个圆近似车体的做法很常见前圆、中圆、后圆半径取车宽的一半加安全裕度。这样做的好处是碰撞检测退化成圆与圆的距离比较速度极快。缺点是车头车尾的角落会有“切角”误差所以安全裕度要给足一点一般 0.2~0.3 米。矩形的话可以用分离轴定理SAT精度高但慢一些。我的经验是先用圆做粗筛对通过粗筛的轨迹再用 SAT 精算能省下大量时间。碰撞检测通过之后还有一步平滑。多项式采样的轨迹虽然平滑但不同周期之间选中的轨迹可能在连接处有跳变表现为方向盘的小幅抖动。常见的处理是在输出前做一次时间上的插值平滑比如用上一周期的轨迹和新轨迹做加权融合融合系数取 0.2~0.3。但要注意平滑不能破坏碰撞检测的结论所以融合之后的轨迹必须重新做一次碰撞检测这一步绝对不能省。我见过为了省时间跳过后检导致的擦碰事故教训很直接。5. 常见问题与排查技巧实录5.1 典型 Bug 速查表这套框架实现下来出问题的地方高度集中在几个位置。我把踩过的典型问题整理成表出问题时可以对着查。现象最可能的原因排查方法轨迹周期性左右抖动参考线每周期重新提取点序或起点不一致打印相邻周期的参考线起点 s 和首点坐标急弯处轨迹作废或 NaN1 - κ*l 分母接近 0 未做保护检查 κ*l 的最大值加上限截断车辆在直线段“发抖”曲率差分时角度未解卷绕检查 θ_r 序列是否有 ±π 跳变规划器找不到可行解障碍物膨胀过大或时间采样太密打印候选淘汰原因统计看是碰撞淘汰还是边界淘汰跟车距离忽远忽近纵向代价里速度项权重过大减小 w_v增大时间项权重变道过程横向过冲横向终点 l 约束缺失确认终点边界条件是否设了二阶导为零输出轨迹与可视化不一致Frenet 转笛卡尔的 θ_r 索引错位用自检函数把 (s,l) 转回来比对计算超时候选数量过多或碰撞检测未做粗筛统计各阶段耗时先加圆粗筛这张表里的每一条我基本都真实遇到过。最坑的是第一条和最后一条一个导致行为诡异但难复现一个导致偶发超时都很难查。5.2 曲率与横向偏移的那几个数值陷阱回到前面提到的分母问题这里展开讲。1 - κ*l 这个因子在整个框架里出现频率极高横向速度、纵向速度、加速度转换都要用。它出问题的方式有两种一种是直接等于零或负数导致方向反转另一种是接近零但不为零导致数值巨大。我的处理方式是在参考线构建阶段就算出每个点的 κ 和 s 方向上的最大允许横向偏移取 l_limit 0.9 / max(|κ|, 1e-3)。规划时候选轨迹的 l 值如果超过这个限制直接淘汰。这个限制比道路边界往往更紧所以能提前发现坐标系退化区域。还有一个陷阱是曲率的符号约定。参考线曲率如果定义成“左转为正”那么横向偏移也必须是“左正右负”否则轨迹曲率算出来符号全反。我在两个项目里见过不同的约定混用一次结果是车辆在弯道里往外冲。所以接手任何一套代码第一件事就是找这两个约定的定义写一行注释钉死在文件头上。5.3 参数标定与验证该怎么下手参数标定别一上来就车调先在离线回放里做。把一段真实的路测数据包含感知结果、定位结果录下来然后让规划器离线跑把输出轨迹和实际行驶轨迹叠在一起看。这样能快速筛掉大部分明显不合理的参数组合。离线验证我一般看几个指标横向加速度的 95 分位值、纵向加速度的 95 分位值、jerk 的最大值、最小障碍物距离、规划失败率、单周期计算耗时。前三个反映舒适性第四个反映安全性后两个反映可用性。这几个指标一起看基本能判断一组参数能不能用。上车之后先做低速场景20km/h 以内确认基本行为正确再逐步提速。城区的挑战主要来自两点一是路口内没有车道线参考线需要靠推理生成二是行人和非机动车的预测非常不准。我的做法是在路口区域把横向膨胀临时放大 30%同时把速度上限压低宁可慢一点也别冒险。注意每次改参数一定要记录改之前之后的几个关键指标。凭感觉调参最后会陷入“改了一个问题冒出三个问题”的循环有数据对照才走得出来。6. 从跑通到好用几个个人体会第一点体会是关于代码结构的。这套框架里坐标转换、参考线管理、采样器、评价器、碰撞检测这五个模块一定要彻底分开。我早期版本把它们耦合在一起结果每次调参都要翻遍整个文件。拆开之后每个模块单独写单元测试坐标转换用几组手工算好的数据验证碰撞检测用构造的矩形对验证出问题时定位快得多。第二点体会是关于“最优”这个词的。数学意义上的最优轨迹在真实场景里往往不是最舒服的那条。因为评价函数里的权重是我们主观设的而乘客的感受又很难量化。我的做法是保留一个 Pareto 前沿的概念不追求单条最优而是在安全性代价接近的候选里选舒适性最好的那条。这样做出来的车行为风格会更接近人类司机不会有那种“精确但生硬”的感觉。第三点体会是关于失败兜底的。再好的规划器都会有找不到可行解的时候这时候必须有一条确定安全的兜底轨迹通常是“保持当前横向位置、以可控减速度减速到停”。这条轨迹不参与评价直接由规划器外部注入。千万不要让规划器在无解时输出上一周期的轨迹继续跑因为上一周期的轨迹是针对上一时刻的障碍物算的可能已经不安全了。最后聊一个后续可以做扩展的方向把这条采样-评价的流水线换成基于优化的方法比如把采样得到的次优解作为二次规划的热启动用 QP 在连续空间里精修。这样能得到更平滑的结果同时因为有采样解兜底求解失败的频率也会低很多。我自己在小范围试过横向加速度的波动能再降一档代价是计算时间增加需要看具体平台的算力预算。