做自动驾驶规划模块的人，对“速度规划”这四个字的感情一定很复杂。路径规划给你一条曲线，但车速怎么沿着这条曲线走，才是决定乘坐体验和安全的下半场。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_{i+1} = s_i + v_i * Δt + 0.5 * a_i * Δt^2 + (1/6) * j_i * Δt^3

v_{i+1} = v_i + a_i * Δt + 0.5 * j_i * Δt^2

a_{i+1} = 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 除以 100，v 除以 20，a 除以 5，j 除以 3。代价函数的权重也跟着重新标定。解完之后再乘回尺度系数。这套归一化处理，整体代码量很小，收益却非常明显，尤其是转弯场景下横向加速度和纵向加速度同时起作用时。

4. 实操过程：从配置到代码骨架，如何快速把优化器跑起来

4.1 关键参数与调参方向速查

如果你用的是 Apollo 现成框架，通常不希望你直接改二进制的权重值，而是通过 planning 配置里的参数去调。这里整理一个我自己常用的参数调优速查表，基本覆盖了最容易影响速度规划效果的几个旋钮：

调参有一个顺序建议：先调好舒适性相关权重（jerk、加速度），再调限速相关权重（参考速度），最后调安全相关权重。原因是从目标函数的耦合度来看，安全权重加得太早，容易把其他项的问题掩盖住，后面想排查舒适性问题就很麻烦。

4.2 一个最简 C++ 伪代码骨架，展示问题装配过程

接触过 Apollo 这类 C++ 代码库的人，看到大段的 Problem 对象、变量数组和约束矩阵时容易一头雾水。其实把 NLP 的装配过程抽象出来，骨架并不复杂：

// 伪代码示意：构建 piecewise jerk speed NLP const double dt = 0.1; // 离散步长 const int N = 80; // 离散段数 // 1. 初始化决策变量 std::vector<double> 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[i+1] - (s[i] + v[i]*dt + 0.5*a[i]*dt*dt + j[i]*dt*dt*dt/6.0)); addResidual(v[i+1] - (v[i] + a[i]*dt + 0.5*j[i]*dt*dt)); addResidual(a[i+1] - (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 求解器一直报不收敛，时间窗口被耗尽

这是最常遇到的情况。排查思路按优先级排序：

1. 先看初值是否合理。如果热启动解本身就在障碍物里面，或者速度初值超过限速很多，那求解器大概率会为了找可行解反复摸索。
2. 再看约束是否过紧。把障碍物距离约束松开一点点，或者把末端速度约束从硬约束改成大权重代价，看看求解时间是否急剧下降。
3. 最后检查导数是否正确。在模型代码里写一个差分对照的开关，跑一轮小规模问题，如果解析雅可比与差分结果差异很大，基本可以锁定 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 图”的习惯，能帮你省下大量肉眼比对速度图的时间。