做自动驾驶规划模块的人对“速度规划”这四个字的感情一定很复杂。路径规划给你一条曲线但车速怎么沿着这条曲线走才是决定乘坐体验和安全的下半场。Apollo 9.0 里的这套 piecewise jerk speed nonlinear optimizer Ⅱ就是速度规划环节里最硬核的一块它把速度曲线求解建模成一个分段加加速度piecewise jerk约束下的非线性规划问题。这篇文章不打算从头讲概念直接聊大家最关心的几件事——代价函数怎么设计、约束系统怎么搭、为什么必须用非线性求解器、以及代码跑起来之后参数该怎么调、不收敛时怎么排查。内容适合正在看 Apollo 规划源码的人、想在自己的无人车框架里实现类似速度规划器的工程师以及被“NLP 求解慢/发散/结果抖”折磨过的同学。我会按我自己优化这类问题的思路展开不堆公式标题把拆过的模型和踩过的坑尽量说透。1. 速度规划到底解决什么问题从 ST 图到 piecewise jerk 模型1.1 路径定下来之后速度为什么还要单独做规划在 Apollo 的规划栈里规划模块大致按“感知 → 预测 → 路由 → 决策 → 规划 → 控制”的链路工作。规划这一步通常又分成两层先解路径Path再解速度Speed。路径决定车往哪条线走速度决定车上每个时刻以多快沿着这条线走。两者不能完全解耦因为弯道上的最高车速与曲率有关动态障碍物又跟时间有关所以速度规划一直是单独拿出来重点处理的部分。速度规划的输出本质上是一条 s-t 曲线s 表示车辆沿参考线走过的纵向弧长t 表示时间。有了 s(t)就可以求导得到速度 v(t)、加速度 a(t) 和加加速度 j(t)。所有跟“舒适性”“安全性”有关的约束最后都会落实到这条曲线上。1.2 ST 图把动态问题变成二维几何问题处理动态障碍物最经典的手段就是 ST 图。横轴是时间纵轴是纵向位移。静态障碍物在 ST 图上表现为一段沿 s 方向的平行区域动态障碍物则表现为一块斜着的多边形区域——前车减速时它的末端在 ST 图上会向上弯对向车道来车看起来更陡。规划任务就是在 ST 图里找一条从当前状态出发、绕过所有障碍物区域、满足速度和加速度上下界的三次平滑曲线。Apollo 里早期的 DP 速度规划就是先在离散网格上粗搜一条可行走廊再用 QP 平滑。但粗搜和二次规划在处理连续的非线性约束曲率限速、障碍物距离度、舒适度指标时都需要做分段线性近似精度和光滑性都不够理想。于是后续迭代里引入了基于 piecewise jerk 的非线性优化器把整个问题直接建成 NLP 来做。1.3 为什么叫“piecewise jerk”简单说piecewise jerk 模型把速度规划的时间域离散成 N 个等长小段每一小段上的加加速度jerk被假设为常数。这样一来加速度在每一段内是线性变化的速度在每一段内是二次变化的位移在每一段内是三次变化的。所以每个小段 s(t) 都是一个三次多项式。如果你把相邻若干段连起来看位移不仅能保证连续还能保证速度、加速度连续这正是乘员能明显感知到的舒适性边界。选择 jerk 作为控制量而不是加速度是有讲究的。如果只对加速度做约束规划出来的速度曲线可以保证加减速过程是“平滑”的但加速度突变仍然会让人感到冲击。加上 jerk 约束后加加速度被限制在一个小范围内车辆起步、刹停的过程才真正柔和下来。这也是 piecewise jerk 类方法在工业界被广泛应用的原因。1.4 先回顾一下第一版非线性优化器做了什么既然是“Ⅱ”前面必然有一篇讲基础模型的。第一部分的重点一般是定义决策变量、写出运动学递推关系、搭出优化问题的整体骨架。决策变量通常包含每段的位移 s_i、速度 v_i、加速度 a_i以及作为控制输入的 jerk j_i。相邻变量之间满足s_{i1} s_i v_i * Δt 0.5 * a_i * Δt^2 (1/6) * j_i * Δt^3v_{i1} v_i a_i * Δt 0.5 * j_i * Δt^2a_{i1} a_i j_i * Δt这三个式子就是整个优化问题的核心等式约束后面所有推导都是围绕它展开。第二部要讨论的就是在这个模型之上如何把现实场景里的各种目标、约束、数值问题都塞进一个可求解的 NLP 框架里。2. 非线性优化器Ⅱ的核心代价函数与约束系统的设计2.1 代价函数不是拍脑袋定的每一项都对应一种“不满”速度规划的本质是在多个互相矛盾的目标里找平衡既想跑得快又怕刹不住既要贴线又担心太颠既想跟前车保持距离又不希望车速忽快忽慢。这些矛盾全部体现在代价函数里。一个典型的 piecewise jerk speed 代价函数会包含以下几类项对加速度 a^2 的惩罚限制大油门、急刹车惩罚大加速度。对加加速度 j^2 的惩罚限制加速度的突变提升平顺性。对参考速度偏差 (v - v_ref)^2 的惩罚让速度尽可能贴近期望巡航速度或限速避免司机觉得“这车怎么慢吞吞的”。对横向加速度的惩罚弯道处要考虑侧向力而横向加速度约等于 v^2 乘以曲率 κ(s)所以这项本质上是把路径曲率信息耦合进了速度规划。对障碍物距离的惩罚不能让车贴着障碍物边界走惩罚项给安全余量留出空间。末端代价针对停车场景期望最后一段速度接近 0、加速度接近 0让车辆能“稳稳停住”。把这么多项写进目标函数时每一项前面的权重系数决定了它的“话语权”。权重差一个数量级规划出来的速度曲线风格就完全不一样。这就是为什么很多团队对同一套源码调了几个月参数配出来的车感千差万别。2.2 为什么这个优化问题必须是“非线性”的很多初学者会问这些目标看起来有平方项约束看起来也不少用二次规划QP不是更快更成熟吗确实Apollo 早年版本里就有一个 QP-based 的 piecewise jerk speed optimizer。但 QP 只能处理线性约束和二次目标在速度规划这个场景里有几个东西是天然非线性的曲率限速约束。弯道允许的最大车速与曲率相关通常写成 v ≤ sqrt(a_lat_max / |κ(s)|)。这个约束包含开方不是线性约束。横向加速度目标项。横向加速度本身是 v^2 与 κ 的乘积是二次非线性的。障碍物距离惩罚。如果直接用分段线性约束来表示 ST 图中的障碍物上边界会让可行域出现大量折角求解出来的速度曲线也容易出现转折点。更好的做法是用平滑函数比如指数、对数障碍函数表达“离障碍物越远越安全”这几乎注定是非线性问题。所以非线性优化器Ⅱ的定位就是把这些真实约束直接用原始公式塞进优化模型靠 NLP 求解器去找一个满足全部条件的局部最优解。2.3 约束系统分层等式约束、变量边界、障碍物约束实际搭约束的时候我习惯把它们分成三层来梳理。第一层是等式约束。运动学递推三式必须严格满足它们在 NLP 里以残差形式出现比如把等式改写成 g_i(x) 0 后交给求解器。第二层是变量边界。位移 s 有上下界——下界来自不可倒退和动态障碍物下边界上界来自停车线、静态障碍物和跟随目标速度 v 有上下界——下界一般是 0上界来自道路限速与曲率限速的较小者加速度和加加速度的上下界通常由车辆性能和舒适性标定决定。第三层是障碍物相关的约束。对于每个动态障碍物需要把它的 ST 投影变成随时间变化的上边界和距离边界。最直接的做法是构造一组分段线性边界约束非线性优化器里更倾向用平滑函数去逼近障碍物的“可通行区域边界”然后把这个平滑函数引入到目标函数或不等约束中。2.4 末端状态约束的细节很容易被忽略很多团队第一次做速度规划时重点都放在中间过程平滑却忽略了终点状态。结果就是车在接近终点或者停车线时速度曲线不收敛最后几米突然冲一下或者刹不住。原因很简单如果优化目标里没有端点的速度和加速度约束那么 N 段轨迹的最后一小段状态只是自由变量它可以被目标函数里的其他项“欺负”得很狼狈。Apollo 这类规划器在处理停车场景时通常会对末端 s、v、a 设置硬约束或者很强的末端代价让末点速度 v_N 接近 0、加速度 a_N 接近 0。正常巡航场景则会对末端速度设置期望值保证与下一帧规划结果衔接连续。这个细节在复现算法时一定要加上否则离线仿真看着没事一上实车就是各种“点头”。3. 求解器选型与数值实现从理论到代码的最后一公里3.1 决策变量与问题规模小身材里的稀疏性接进来一个实际的点。假设时间窗口 T 取 8 秒离散步长 Δt 取 0.1 秒那么 N 80。每一段有 s_i、v_i、a_i 三个状态变量再加上每一段的控制变量 j_i总共大约有 4 个变量乘以 80 段也就是 320 个决策变量。再加上 240 个左右运动学等式约束以及各种上下界约束。这个规模在现代 NLP 求解器面前不算大但如果把雅可比矩阵当成稠密矩阵来写计算量和内存消耗都会成倍上涨。正确的做法是利用稀疏结构大多数约束只涉及相邻两三个时间节点雅可比矩阵呈带状分布。用稀疏矩阵接口来装配 Jacobian 和 Hessian求解效率会快一个量级以上。3.2 解析雅可比永远是首选能不用差分就不用差分我在实际调试中遇到过几次“解决问题的时候发现求解器很慢”排查到最后发现是代码里用了有限差分去估算导数。有限差分在变量规模小的时候看不出问题但到了 300 多个变量时每次迭代都要多次扰动求解浪费时间而且差分步长选不好会引入数值噪声导致优化结果抖来抖去。对这个模型解析导数并不难写。状态递推式对 s_i、v_i、a_i、j_i 的导数就那么几个常数项代价函数对 a 和 j 的求导也很直接唯一麻烦的是横向加速度项它会同时依赖 v_i 和 κ_i写链式法则时容易漏项。我建议每写完一个导数函数先做一次数值差分对拍确认误差在 1e-6 量级再往下走。提示别小看这一步。数值对照是检验解析雅可比最快的办法很多“轻量级 bug”比如下标差一、漏乘 Δt几乎都是靠对拍抓出来的。3.3 热启动解决非凸问题局部最优的关键一招NLP 本质上是个非凸优化找到全局最优是非常困难的。速度规划这类问题更实际的目标是“求一个可行且足够好的局部最优解”。要让求解器稳定找到这样一个解初值非常重要。Apollo 的实现里通常会用上一步的规划结果或者一个粗糙的 DP / QP 解来作为热启动。热启动的好处有两层一是大幅减少迭代次数让求解能在 50 毫秒量级内完成二是让优化落在上一拍解的附近避免两帧之间速度曲线发生跳变。我自己踩过的坑是某些场景下热启动解已经接近约束边界比如贴着障碍物边界NLP 从这种初始点出发很容易被障碍物惩罚项推到另一个分歧解里。这时候可以先把障碍物距离惩罚做大一点让初值“远离”约束边界再逐步恢复原权重。3.4 数值规范化让量纲不同的问题不再病态速度规划里同时存在 s、v、a、j。数值上 s 动辄几十上百v 十几二十a 只有几j 是零点几到几。如果直接把这些变量丢给求解器矩阵条件数很容易变差导致收敛缓慢或者压根不收敛。我的做法是在进入求解器前把每个变量除以一个特征尺度比如 s 除以 100v 除以 20a 除以 5j 除以 3。代价函数的权重也跟着重新标定。解完之后再乘回尺度系数。这套归一化处理整体代码量很小收益却非常明显尤其是转弯场景下横向加速度和纵向加速度同时起作用时。4. 实操过程从配置到代码骨架如何快速把优化器跑起来4.1 关键参数与调参方向速查如果你用的是 Apollo 现成框架通常不希望你直接改二进制的权重值而是通过 planning 配置里的参数去调。这里整理一个我自己常用的参数调优速查表基本覆盖了最容易影响速度规划效果的几个旋钮参数方向对应目标项手感表现调参数值的影响jerk 权重调大加加速度惩罚速度曲线更柔起步/刹停更线性曲线变钝但可能过分保守加速度权重调大加速度惩罚大油门/急刹减少跟车灵敏度下降参考速度权重调大贴近限速车速更接近期望速度容易忽略舒适性横向加速度权重调大弯道车速入弯速度更早降下来出弯提速变慢障碍物距离权重调大安全距离跟车距离变大通过性变差可行域更大平滑约束灵活度提升更容易出现局部振荡调参有一个顺序建议先调好舒适性相关权重jerk、加速度再调限速相关权重参考速度最后调安全相关权重。原因是从目标函数的耦合度来看安全权重加得太早容易把其他项的问题掩盖住后面想排查舒适性问题就很麻烦。4.2 一个最简 C 伪代码骨架展示问题装配过程接触过 Apollo 这类 C 代码库的人看到大段的 Problem 对象、变量数组和约束矩阵时容易一头雾水。其实把 NLP 的装配过程抽象出来骨架并不复杂// 伪代码示意构建 piecewise jerk speed NLP const double dt 0.1; // 离散步长 const int N 80; // 离散段数 // 1. 初始化决策变量 std::vectordouble s(N 1), v(N 1), a(N 1), j(N); // 2. 设定状态初值热启动 s[0] current_s; v[0] current_v; a[0] current_a; // 3. 构建代价函数项 for (int i 0; i N; i) { cost w_ref * (v[i] - v_ref[i]) * (v[i] - v_ref[i]); cost w_acc * a[i] * a[i]; cost w_lat * kappa[i] * v[i] * v[i] * v[i] * v[i]; } for (int i 0; i N; i) { cost w_jerk * j[i] * j[i]; } // 4. 添加运动学等式约束写成残差形式 for (int i 0; i N; i) { addResidual(s[i1] - (s[i] v[i]*dt 0.5*a[i]*dt*dt j[i]*dt*dt*dt/6.0)); addResidual(v[i1] - (v[i] a[i]*dt 0.5*j[i]*dt*dt)); addResidual(a[i1] - (a[i] j[i]*dt)); } // 5. 添加变量边界 for (int i 0; i N; i) { addLowerBound(s[i], st_s_min[i]); addUpperBound(s[i], st_s_max[i]); addLowerBound(v[i], 0.0); addUpperBound(v[i], std::min(limit_speed[i], curvature_limit_speed[i])); addLowerBound(a[i], a_min); addUpperBound(a[i], a_max); } // 6. 设置末端约束 addLowerBound(v[N], 0.0); addUpperBound(v[N], 0.0); // 停车场景示例 // 7. 交给 NLP 求解器求解 solver.solve(cost, constraints);这段代码虽然只是骨架但已经包含了求解一个速度曲线所需的最小要素。很多团队在工程落地时会再加一层对每个时间点把外部传入的障碍物 ST 约束转换成一个分段线性函数存到数组 st_s_max 里。这一步做实了整个问题才真正“接”上了感知和预测。4.3 一个典型的场景实测弯道遇静止障碍物跑通代码后我建议用一个简单场景来验证整体效果车辆沿弯道行驶前方同一车道内有一个静止障碍物比如一台抛锚的货车。这个场景能同时暴露曲率限速、障碍物避让、末端停车三个方面的耦合问题。从结果上你一般会看到离障碍物还有一段距离时速度先开始缓慢下降而不是到跟前才急刹。这是加加速度约束在起作用。进入弯道之前速度会比直道限速更低因为横向加速度项的惩罚开始压过目标速度项。在障碍物位置附近s-t 曲线会留出明显的安全余量不会贴着障碍物边界走。接近停车目标点时末段速度和加速度同时收敛到 0整条曲线看起来像“自然滑停”。如果实测中看到速度曲线在弯道段出现抖动多半是横向加速度权重和曲率图分辨率不匹配或者离散步长太大把曲率变化剧烈的地方采样漏了。可以试试把 Δt 从 0.1 缩到 0.05代价是段数翻倍求解时间会上升但曲线的顺滑度通常能明显变好。5. 常见问题与排查技巧实录5.1 问题一NLP 求解器一直报不收敛时间窗口被耗尽这是最常遇到的情况。排查思路按优先级排序先看初值是否合理。如果热启动解本身就在障碍物里面或者速度初值超过限速很多那求解器大概率会为了找可行解反复摸索。再看约束是否过紧。把障碍物距离约束松开一点点或者把末端速度约束从硬约束改成大权重代价看看求解时间是否急剧下降。最后检查导数是否正确。在模型代码里写一个差分对照的开关跑一轮小规模问题如果解析雅可比与差分结果差异很大基本可以锁定 bug 在导数。注意非线性优化不收敛时不要上来就调求解器参数。先降问题难度再逐步加约束逐个排查效率高得多。这个顺序我用了很多次屡试不爽。5.2 问题二速度曲线出现明显的高频振荡这种现象的典型特征是 v-t 图上有肉眼可见的纹波尤其是起步、停车和通过曲率突变点时。原因主要有三个jerk 权重太小对加加速度的惩罚力度不够让速度曲线可以肆意地在相邻时间点来回弹跳。解决办法是调大 jerk 权重。约束不光滑障碍物边界是分段线性表达时折角会引入不光滑导致目标函数的梯度在交界面附近剧烈变化。解决办法是用平滑函数近似边界。初值本身带振荡上一步 DP 解太粗直接拿它热启动NLP 需要大量迭代去“磨平”初值。解决办法是对初值先做一次轻量级平滑或者增加迭代上限。我给一个经验值方向jerk 权重的量级通常要比加速度权重大 2 到 5 倍才能压得住振荡趋势。具体数值受不同车型、不同整车质量影响还是需要结合实车/仿真标定。5.3 问题三实时性不达标求解耗时超过 100 毫秒Apollo 这类系统里规划周期一般只有 100ms 甚至更短留给 NLP 的时间最多 50 到 60ms。如果超时常见处理路径是缩小优化时域。把 8 秒窗口砍到 6 秒或者把 Δt 从 0.05 放大到 0.1段数 N 减半。热启动做扎实。同一场景连续多帧规划解之间的差异很小热启动可以把迭代次数压到个位数。对难解场景做“多级降级”。先让快速求解器跑一个完整解如果时间预算不够就用上一次规划结果结合局部修正而不是继续死磕 NLP。预处理曲率表、限速表。在进入优化器之前把 s 相关的静态信息算好避免在目标函数里频繁查表。实时性问题的根源通常在“建模太重”而不是求解器太慢。把不必要的大 N 和过紧的约束放掉往往比升级硬件更直接。5.4 问题四规划结果与控制模块对不上车实际表现比曲线“愣”规划输出的是未来若干秒的速度曲线控制模块去跟踪时会有滞后尤其在急转弯或者强制动场景。这时我看到很多团队第一反应是去调 PID但真正的问题可能出在规划末端状态不连续。每一帧规划开始时初始状态必须是上一帧规划在“当前时刻”的状态值而不是直接用定位反馈的即时值。如果这两者差得多相当于让优化器从全新起点重新规划每一帧都会推出一段不同形状的曲线控制自然追不上。Apollo 这类系统非常强调上一个周期的输出与下个周期的输入对齐查这个问题时先看变量衔接处是否有跳变往往能找到真凶。6. 后续扩展与一点个人体会非线性速度优化器的能力边界不止于当前这套配置。如果你继续往深了做会发现它和路径规划是强耦合的当前方法是把路径当作已知输入所以横向加速度项只是“引用”曲率而不是去决策路径形状。更进一步的方案会把路径采样和速度优化放在同一层迭代求解但工程复杂度会成倍上涨。我个人的体会是piecewise jerk speed nonlinear optimizer 这类算法真正难的不是把数学公式写成代码而是在真实感知噪声、控制偏差、限速变化频繁切换的场景里保持解的稳定性和连续性。针对这一点持续维护一个场景回归集特别重要——把单车道跟车、多车博弈、弯道静止障碍物、连续红绿灯等场景录制成离线测试包每改一次权重或者约束逻辑就全量回放一遍。这套流程跑顺之后你再回去看源代码里那些权重参数就能真正理解每一处数值背后的行车性格。最后分享一个小技巧调试时优先看加加速度曲线而不是速度曲线。加速度和加加速度是对舒适性最敏感的指标两帧之间速度曲线看着差不多但加加速度曲线可能已经出现了肉眼可见的毛刺。养成“最终判读看 a-t 和 j-t 图”的习惯能帮你省下大量肉眼比对速度图的时间。