Apollo 9.0分段Jerk速度非线性优化器：建模、求解与调参实战

做自动驾驶规划这两年多我一直有个判断路径规划的问题大多“想得明白但写起来烦”速度规划则恰恰相反——“看起来简单跑起来全是坑”。尤其在 Apollo 9.0 平台里planning 模块中最容易被低估、也最经得起折腾的就是那个负责把一条已定路径变成可执行速度曲线的 piecewise jerk speed nonlinear optimizer分段 jerk 速度非线性优化器。我为什么单独把它拎出来写因为最近这段时间我在做 speed planning 的工程落地碰到的几个最诡异的线上问题最后全都定位到这个优化器身上一是障碍物 ST 约束被硬生生卡死导致频繁无意义减速二是目标函数权重失衡加减速来回震荡乘客体感非常差三是某些 cut-in 场景下旧版 QP 求解器直接报无解。Apollo 9.0 里带“非线性”三个字的这个版本就是冲着这些问题来的。这篇文章会把它的建模原理、求解链路、调参手段、以及那些不写在文档里的坑一次性讲透适合正在读 Apollo planning 源码、或者有意自己搭速度规划模块的工程师参考。1. 速度规划为什么绕不开“非线性”先看上一代 QP 版本卡在哪1.1 速度规划在这条链路里的角色先对齐一下基本流程。任何一个规划模块通常都是先解决“往哪走”再解决“怎么走”上游路径优化器给出一条平滑的几何曲线只包含位置信息速度规划的任务就是在这条曲线上找到一段随时间变化的关系即 s(t)、v(t)、a(t)最后拼成带时间戳的轨迹交给控制模块。整个过程里ST 图是核心中间产物——把障碍物投影到以时间为横轴、纵向距离为纵轴的图里速度规划本质上是在这张图上找一条合理的 s(t) 曲线。这一步的产出对整车体验的影响往往比路径规划更大。横向路径不平滑乘客顶多觉得“有点晃”但纵向速度曲线只要有一点抖动人体对纵向加速度的感知会立刻被放大——前仰后合、推背顿挫全是这里来的。所以速度规划一直是规划模块里对数值精度和实时性要求最苛刻的部分。1.2 上一代 QP 版本三个让人头疼的毛病Apollo 在非线性版本之前默认的 speed optimizer 走的是二次规划路线时间轴离散成若干段状态量是距离、速度、加速度jerk 作为控制量障碍物在 ST 图上用上下边界框出来写成 s_min(t) ≤ s(t) ≤ s_max(t) 的线性约束再配 slack 变量防止硬性无解。这套框架在教科书上无懈可击但工程上跑起来我总结出了三个典型问题。第一盒子约束天生保守。ST 图里的障碍物往往不是一个干净矩形而是带轨迹预测不确定度的区域。QP 版本为了安全通常取完整包络于是“前方一辆车在变道但实际离我挺远”也会被压成一个很紧的可行走廊。表现出来就是车提前一两百米开始减速后车疯狂按喇叭乘客也不知道你在躲什么。第二slack 和速度跟踪项打架。为了避免无解每个障碍物约束都配了 slack权重却不能设太大否则约束形同虚设设小了一旦约束被顶到边界优化器宁可产生巨大减速度也不愿破约束。我见过速度曲线在一个周期内从 72 掉到 35 km/h 再拉回 70 的情况就是 slack 权重与 v_ref 跟踪权重互相拉扯的结果。第三无解场景处理太粗糙。旁车不打灯直接切到车道这类场景ST 图上两个障碍物的可行区间在时间轴上互相挤压QP 的可行域直接为空。代码只能返回失败整个规划帧拿不到新曲线只能沿用上一帧甚至触发保守刹停。这在高速场景尤其危险因为安全留给你的反应时间本来就不多。1.3 非线性版本到底改变了什么以及“Ⅱ”迭代了什么非线性版本的思路换了个角度把“必须落在障碍物边界一侧”的硬逻辑变成“离障碍物边界越近代价越高”的软惩罚。优化器自己权衡“贴边加速通过”和“减速让行”哪个综合代价更低而不是被布尔条件一票否决。这带来两个直接好处可行域几乎不会为空因为任何时刻都能通过增大惩罚代价找到一条偏保守但存在的解速度曲线也不会因为单个 ST 盒子的抖动而剧烈变化因为惩罚函数是光滑的。另外非线性问题天然能表达更多耦合关系——弯道里限速和横向加速度的乘积关系、坡道上重力分量对加速度的抵消这些东西写进 QP 里要么线性化失真要么根本写不进去。至于标题里的“Ⅱ”以我对 Apollo 9.0 规划的跟踪理解这标记了该优化器的第二代实现。第一代更像是把原本的 QP 框架硬塞给 NLP 求解器的过渡品固定时间网格、障碍物惩罚简单粗暴第二代则真正按 NLP 思路重新设计了代价与约束结构引入非均匀时间离散、更精细的惩罚函数和更稳健的初始化策略。后面章节我会把这些差异具体展开。2. piecewise jerk 建模全解从分段常 jerk 假设到代价函数推导2.1 为什么是“分段常 jerk”先说一个反直觉的点速度规划里真正被优化的对象其实不是速度而是 jerk——加速度的变化率。乘客晕不晕车主要看 jerk 是否一直在大幅跳变。工程里常用的舒适性指标是纵向 jerk 不超过 2~4 m/s³超过这个范围人体很快会产生不适感。“分段常 jerk”就是把控制量简化成把时间轴切成 N 段每一段内 jerk 保持恒定。这样做最大的好处是第 i 段到第 i1 段的状态转移可以用闭式公式直接写出来优化变量只有每段的起始状态和该段的 jerk 值问题规模可控。标准递推式是s_{i1} s_i v_i·Δt ½·a_i·Δt² (1/6)·j_i·Δt³v_{i1} v_i a_i·Δt ½·j_i·Δt²a_{i1} a_i j_i·Δt其中 s 是纵向距离v 是速度a 是加速度j 是 jerkΔt 是这一段的时长。N 个时间段状态变量约 3N 个再加 N 个 jerk 控制量整体是 4N 量级的小规模问题对现代求解器来说负担并不大。2.2 代价函数每一项都在表述一种“驾驶性格”优化器的性格完全由代价函数决定。我把它的核心拆成四部分。第一部分是参考速度跟踪项w_v·Σ(v_i − v_ref,i)²。v_ref 来自限速、交通流速度、决策模块给的目标速度。这一项权重越大车越“急着”贴限速走权重越小车越“肉”会优先照顾其他目标。高速巡航和城市低速跟车场景对这项权重的需求完全不同。第二部分是加速度惩罚w_a·Σa_i²。它抑制大起大落让车尽量保持匀速或平缓加减速。注意这里惩罚的是加速度本身所以长时间缓加速和短时间急加速之间优化器会倾向于前者。第三部分是 jerk 惩罚w_j·Σj_i²。这是舒适性的直接体现。jerk 权重高时优化器宁可早一点开始减速也不愿意在最后关头猛踩一脚。实话说这一项才是速度曲线“柔软度”的真正来源。第四部分是障碍物软惩罚也是非线性版本的精髓。对每个时间点 iST 图给出安全区间 [s_low,i, s_high,i]然后定义一个门槛惩罚函数落在安全区间内时惩罚近乎为零一旦逼近甚至越过边界惩罚迅速上升。一种常见写法是单侧二次惩罚加缓冲区 Δp(s_i) ½·w_obs·max(0, s_low,i Δ − s_i)² ½·w_obs·max(0, s_i − (s_high,i − Δ))²这里有个工程细节必须注意max(0, ·) 会在触发点造成一阶不光滑梯度在那里跳变求解器容易在边界附近来回振荡。稳妥做法是把转折处做一个微小圆弧或线性过渡保证函数一阶光滑甚至二阶光滑。这一点后面调参部分还会再提。最后还有一类末端代价比如红灯场景要求某个时间点后速度归零或者要求在规划末段达到目标速度。我建议能写成软约束就写在代价里比写死约束更稳。硬约束在求解器数值误差下容易出现“差一点点不满足”然后整解被判失败的情况。2.3 约束体系哪些必须硬哪些可以软我的原则很简单物理极限必须硬安全与舒适可以软。硬约束包括状态转移方程本身、速度上下限、加速度上下限由轮胎附着和纵向控制能力决定、jerk 上下限、初始状态等式约束——当前位置、速度、加速度必须等于车辆实际值。末端如果明确要求停车比如红灯或前方静止障碍物终点速度等于零也要写成强约束或用足够大的末端权重逼近。软的方面则有两类。一类是障碍物边界用软惩罚代替硬约束前面已反复说明。第二类是舒适性边界比如“正常行驶加速度不超过 2 m/s²”。我会把它做成惩罚而非硬约束因为紧急避险时突破它是合理的硬约束会直接导致无解。还有一个经常被忽略的非线性约束弯道限速的耦合关系。弯道上速度必须满足 v_i² ≤ a_lat_max / κ(s_i)其中 κ 是路径在 s_i 处的曲率。κ 是 s 的函数而 s_i 本身是决策变量所以这个约束本质上是非线性的。QP 版本只能预先按路径位置算好一张限速表当常量用转弯前把速度压得很死NLP 版本可以在迭代过程中根据当前 s_i 修正曲率取值限速曲线与真实位置严格对应。这也是非线性版本在弯道场景体验更好的原因之一。3. 非线性问题怎么解Apollo 9.0 求解链路与工程细节3.1 求解器选型内点法还是序列二次规划NLP 问题规模虽小但选错求解器照样会跑不完一个规划周期。工程实现里主流是两条路一条用 IPOPT 这类内点法通用求解器一条是自研序列二次规划SQP。Apollo 9.0 的规划模块具备多种求解器调度能力工程上通常以外接 IPOPT 或等价实现为主。内点法对“变量少、约束结构稀疏且固定”的问题非常友好每次迭代只需要解析目标函数值、梯度以及约束雅可比矩阵。但内点法有一个大坑对数值尺度极其敏感。s 是几十米量级v 是十几 m/sa 是几个 m/s²jerk 则可能小到 0.01 量级。如果不做变量缩放Hessian 条件数会非常难看轻则收敛慢重则根本不收敛。我建议在做这类优化器时先把变量归一化或者在调用求解器时显式设置变量 scale。很多规划代码里 solver 配置那栏有一堆 tolerance 参数调来调去根因往往就是尺度没处理干净。梯度也要尽量给解析式。分段常 jerk 的状态转移是多项式求导是简单到不能再简单的事手写 Jacobian 完全可行。数值微分虽然在开发期省事但在线运行时每次多算几十次函数求值耗时会明显增加而且精度误差会在 KKT 判定时反咬你一口。3.2 warm start能不能收敛一半看初始猜测NLP 求解的是局部最优初始点往哪给结果就落在哪片山谷里。速度规划常见的三种初始化方式一是直接用上一帧的解平移二是先用动态规划DP在 ST 图上跑一个粗糙解插值到 NLP 的网格上三是用匀速外推。我强烈推荐做 DP warm start。原因很简单DP 虽粗糙但天然“可行”——它避开了障碍物、满足基本限速。把这些信息喂给 NLP 后求解器基本只需在可行点附近做打磨把加速度和 jerk 修平滑。直接拿上一帧解做 warm start低速场景还好一旦上一帧是紧急刹车状态平移过来的初值会让本帧解偏向刹车产生“刹一下、停一下、再刹一下”的怪曲线。匀速外推更糟糕障碍物前的初始点完全不满足约束求解器光是把点拉回可行域就要耗掉大半迭代预算。这里要特别说一句第二代实现里 warm start 的改进是肉眼可见的它把 DP 结果按非均匀网格重新采样近端插值更密远端稀疏让求解器在关键决策区域更快起步。这一点在实际测试中显著降低了迭代次数。3.3 收敛判据与失败回退一个规划帧只有 100 毫秒实时规划对耗时零容忍一个 planning cycle 通常只有 100 毫秒级别速度优化器必须在这个时间内交卷。工程上要设几道保险最大迭代次数要卡死比如 100~200 次超时就停求解器状态码要单独判断不是 SUCCESS 一律视为失败失败后不能把速度曲线直接丢掉否则控制端瞬间失去目标。我的回退链分三级先尝试沿用当前迭代点因为迭代到后期的点哪怕没收敛往往也八九不离十不行就回退到上一周期成功的速度曲线再不行才用保守的紧急刹停 profile但这是最低优先级选项。实测中真正走到第三步的场景极少大部分失败在第一步就被接住了。还有一个隐蔽的坑判断“成功”要看 KKT 条件残差是否真正满足而不是只看求解器返回值。我踩过一次求解器每次返回“最优”但速度曲线末端每隔几个周期就抖一下。排查很久才发现是约束 Jacobian 里某个动态约束对 s 的偏导写错了导致拉格朗日乘子假收敛。这类问题在仿真里基本看不出来实车只要跑到那个速度区间就会周期性顿挫。强烈建议把 KKT 残差、约束违例量这些诊断信息全部落盘做长里程数据回放后再上线。4. 调参与排障让非线性优化器在真实道路上跑稳4.1 权重标定从默认值出发的三步法调权重没有银弹但我有一套固定的起步流程保证不会从零开始乱试。第一步先固定加速度和 jerk 的相对关系。让 w_a 约等于 w_j 的 0.1 到 0.2 倍也就是允许适度加速度但强烈抑制 jerk 突变。这个比例决定了“平顺优先”的底色。第二步只调参考速度权重 w_v观察高速巡航和跟车距离是否满足要求。w_v 太小车跟在慢车后面懒得超w_v 太大遇障刹车过晚乘客被吓到。第三步再调障碍物软惩罚的形状参数比如惩罚函数上升的陡峭程度以及从多近开始起作用。我常用的量级参考如下但注意不同实现里 s/v/a 的量纲处理不一样直接抄数字没意义关键看相对比例参数量级参考影响方向w_v1 ~ 10速度跟踪紧迫感、跟车距离w_a100 ~ 1000加减速激进程度w_j1000 ~ 10000速度曲线平滑度、舒适性障碍物惩罚斜率视函数形式贴边通过 vs 保守避让举一个实际例子低速跟车场景我把 w_v 从 5 降到 2同时把障碍物惩罚的起始距离从 1.5 米扩到 3 米车辆在拥堵路段的“点头”频率立刻下降。代价是跟车距离稍微拉大容易被加塞但乘客舒适度明显提升。4.2 时间分辨率dt 的选择与分段设计速度规划的预测时域通常取 8~10 秒但到底切多少段、每段多长直接关系响应速度。dt 太大优化器对 cut-in 这类突发障碍反应迟钝因为第一个决策点离当前太远dt 太小变量个数膨胀求解时间不可控。工程上我推荐基础网格用 0.5~1 秒近端前 1~2 秒加密到 0.2~0.4 秒。别小看这一段加密它决定了紧急制动能不能在第一个周期内把减速度拉起来。举个例子8 秒时域全用 0.5 秒网格是 17 段改成近端 0.2 秒 × 1 秒、中段 0.5 秒 × 3 秒、远端 1.0 秒 × 4 秒总段数 13 段左右变量反而少了关键区域的决策精度却高了。NLP 版本比 QP 版本更愿意接受非均匀网格这个红利值得吃满。4.3 真实道路上的四个边界 case 与排查套路分享四个我在测试中真实撞过的场景每一个都值得记进自己的排查手册。第一个是绿灯倒计时。这个场景的坑在于决策模块给了“可通行”语义但 ST 图里又有汇聚的车流边界两次规划之间语义切换速度曲线会瞬间从“保持 50”变成“急停”。排查时要先看决策层语义是否滞后半拍再看终点速度约束是不是从“自由”突然变成 0。我的处理是给倒计时场景单独设置末端速度软约束用权重过渡代替硬切换。第二个是旁车 cut-in。非线性软惩罚大多时候表现惊艳但如果惩罚函数设计得太陡旁车一进来代价在边界附近出现陡峭梯度优化器给出的速度剖面会有明显的“下探”再“回拉”体感就是一脚急刹然后松开。解决思路是限制惩罚函数的最大斜率这等价于给避让行为设置纵向减速度上限。第三个是限速突变比如隧道入口从 80 限到 60。v_ref 如果直接给跳变优化器为了满足速度跟踪项可能在一个 segment 内强行产生巨大负 jerk。根子在上游但优化器这边可以兜底给 v_ref 序列做滑动平均同时把 jerk 硬上限设到舒适值以下让它无论如何都突破不了物理极限。第四个是高架上坡。加速度上限必须考虑重力沿坡道的分量平地上能刹住的减速度在坡上要额外抵消一个 g·sinθ。如果不把这项写进约束优化器算出的速度剖面在坡道会“超能力”实车表现为速度比规划值越掉越多控制端反复补油门出现追赶振荡。这个约束是典型的非线性红利QP 版本很难优雅处理。每个 case 的排查我都有固定动作先把 s/v/a/jerk 四个 profile 导出 CSV画出来看曲线形态再查约束违例量看哪些约束在图上最紧的位置被激活最后核对求解器状态和迭代轨迹。这套动作走下来80% 的速度异常能在十分钟内定位到是上游还是优化器本身的问题。5. 与上下游模块的配合一次完整的速度规划实战复盘5.1 上游输入的质量决定优化器一半的命运很多人遇到 NLP 优化结果抽风时第一反应是调参但我发现相当比例的“优化器抽风”根源在上游。路径曲率连续性是最典型的例子如果路径优化器输出一条 κ 不连续的曲线那么 v² ≤ a_lat/κ 这个曲率限速会在拐点处突变速度优化器被迫在极短时间内从 70 刹到 40这本身就是一个大 jerk。所以排查速度问题时先检查路径曲率曲线的连续性再谈其他。参考速度也一样。决策模块给的目标速度如果是步进式的比如前方路段限速从 80 在某处突变为 60喂给优化器的 v_ref 就是一个阶跃信号。我的习惯是在优化器前面加一个速度参考平滑层用一阶惯性或滑动窗口把跳变抹平。这比在代价函数里硬调权重省事得多而且对下游控制更友好。5.2 输出对接控制端的三个细节速度优化器交出的是一组时间轴上的 s/v/a 序列但控制模块真正需要的是更高采样率、且三阶可导的速度曲线。实际工程要做三件事一是把离散序列插值成高分辨率 profile。这一点在分段常 jerk 假设下非常省事任意时刻 τ 在第 i 段内的位置可以直接用闭式公式算s(τ) s_i v_i·τ ½·a_i·τ² (1/6)·j_i·τ³不需要重新拟合也不会引入插值误差。二是裁剪时域到真实可执行范围保证末端速度不为负、且对静止目标准确归零。三是和控制周期对齐避免多项式插值在控制 tick 之间引入相位差。我踩过一个很典型的坑只把 s 和 v 发给控制a 由控制端自己差分来算结果高频噪声被放大车里听到持续的细碎顿挫。后来把优化器里的 a 和 jerk 一并下发控制端直接用于前馈问题立刻消失。输出接口别省a 和 jerk 的精度对下游控制不是锦上添花是刚需。5.3 复盘一次高架汇入场景的对比最后讲一次让我印象深刻的实测对比。某条高架汇入口主路车流密集我们在同一段测试道路上分别跑了 QP 旧版和 nonlinear 新版。场景是左前方一辆慢车压着车道右侧汇入车流缓缓并入视线里几百米外路牌写着匝道结束。旧版 QP 给出的速度曲线在进入汇入段时先降一截等汇入车流离开可行域后猛加速再因为几百米后的限速又降——三段“平台—急刹—急加速”的阶梯体感很差。非线性版本则不同它把“贴边跟住慢车”和“为汇入车让点空间”放进同一个代价函数权衡输出曲线先建立在一个温和减速度上贴着可行域边缘稳定通过随后缓慢回到目标速度。两台车跑下来乘客对横向表现评价没差别纵向评价差距巨大。那次之后我把一条经验定成了团队内部原则速度优化器里的安全边界能软则软尽量不要用硬约束去做避让决策硬约束留给物理极限就够了。这条原则听起来简单真正实践起来需要针对每个约束做反复的仿真与实车 A-B 测试。我的个人体会是这个优化器真正值钱的地方不在“非线性”这个标签本身而在于它给了规划工程师一个连续调优的旋钮把很多从前只能靠规则硬编码的驾驶行为变成了目标函数里几个有物理意义的权重系数。想用好它先忘掉默认参数表回到“驾驶性格”四个字上想清楚每一项代价在表达什么然后一条路一条路地跑数据、回放、微调。