去年在城区一个路口调试右转场景车辆进入弯道前反复出现横向抖动数据回放里横向偏差曲线像锯齿一样。顺着链路排查了很久最后锁定到参考线上地图采集出来的路径自带几个几厘米的折角参考线没做平滑就直接送进了规划模块Frenet坐标展开后这几个微小的折点被放大成了控制抖动。后来把参考线平滑模块换成FemPosDeviation加SQP这套组合问题才算根治。这篇文章就把这套方案的建模思路、求解器选型和工程落地细节完整过一遍适合正在做规划、控制或者地图后处理的朋友参考。1. 参考线不平滑受害的不只是规划曲线本身1.1 规划一上来就要在参考线上“拉坐标”自动驾驶里绝大多数规划和控制算法都工作在Frenet坐标系下以参考线为s轴沿路径方向以参考线的法线方向为d轴横向偏移。车辆当前在哪、目标在哪、障碍物横向距离多少全部要投影到这套坐标系上才能计算。参考线的几何质量直接决定了这一整套投影计算的质量。问题在于Frenet坐标变换不是简单把点投到一条折线上就完了。规划算法需要参考线的切线方向、法线方向甚至需要参考线的曲率信息。一旦参考线上有折角这个位置的切向量就会发生突变投影点和横向距离也随之跳动。后果就是S-L图里的障碍物边界出现毛刺规划器误以为横向环境在剧烈变化于是频繁调整轨迹。1.2 三条典型的参考线劣化症状我在实车数据里总结过三条规律凡是参考线质量不行的时候基本都会出现下面这些症状中的一条或几条规划输出曲率尖峰参考线折角处优化器为了让轨迹贴合参考线会在对应位置强行转一个很小的弯输出的曲率指令出现尖峰车辆横向加速度瞬间拉高。横向偏差d值抖动投影算法在折角附近可能找错最近点同一个物理位置在不同帧下投影出来的d值跳来跳去下游做横向控制时方向盘来回修正。曲率超限折角处的局部曲率远大于车辆最小转弯半径对应的曲率上限。控制模块使多大劲都跟不上最后只能看着跟踪误差越来越大。这三条里第一条最坑。因为规划轨迹本身看着平滑问题出在参考线如果不看参考线原始数据很容易误判成控制参数问题。1.3 为什么不能把“看着还行”的折线直接喂给下游很多刚入行的同学会问地图出来的参考线看起来也就是几个毫米级的折角至于这么紧张吗至于。参考线是规划问题的基准轨迹优化算法通常把参考线作为软约束或者硬约束。如果参考线自带锯齿下游的轨迹优化结果只有两条路要么为了贴合参考线把锯齿也复刻进轨迹里要么在平滑性和参考线贴合度之间艰难取舍牺牲其中之一。无论哪种最终体验都是差的。参考线的质量就是规划质量的天花板上游不修下游再怎么调权重也只是拆东墙补西墙。所以参考线平滑不是锦上添花而是规划链路上必须做的一步。它要把离散的、带噪声的、可能含有坏点的原始路径变成一条既贴近原路径、又能满足车辆运动学限制的平滑曲线。FemPosDeviation加SQP就是一套被大量工程验证过的解决方案。2. FemPosDeviation把参考线当成一根有刚度的金属丝2.1 弹性绳与金属丝的离散建模FemPosDeviation这个算法名字看着吓人建模思想其实很朴素把整条参考线想象成一根有一定刚度的金属丝原始参考点就是这根金属丝需要穿过的“约束环”我们希望把金属丝弯成一条贴近这些环的平滑曲线。金属丝弯曲得越剧烈内部的弯曲应变能就越大优化的目标就是让这个应变能尽量小。离散化之后金属丝的弯曲可以用相邻三点的二阶差分来近似。给定三个连续点p_{i-1}、p_i、p_{i1}构造向量d_i p_{i-1} p_{i1} - 2p_i当这三点共线并且间距均匀时d_i等于零向量弯曲越厉害d_i的模长越大。把所有点的d_i模长平方加起来就是整条曲线的“弯曲能量”J_smooth Σ ( (x_{i-1} x_{i1} - 2x_i)^2 (y_{i-1} y_{i1} - 2y_i)^2 )这正是离散二阶导数的形式和样条曲线里的曲率近似是一脉相承的。用二次代价函数表达最妙的地方在于它天然可导、二次可微、Hessian是常数矩阵给后面的求解器省了大量麻烦。2.2 目标函数里三个角色的权重分配光有弯曲能量还不够。如果只最小化平滑项算法会把曲线无限拉直完全脱离原始路径。所以目标函数里必须有“位置回拉”项让平滑后的点不要偏离原始参考点太远公式是J_ref Σ ( (x_i - x_ref_i)^2 (y_i - y_ref_i)^2 )除了这两项工程上还会加一个点距均匀项J_len Σ ( (x_i - x_{i-1})^2 (y_i - y_{i-1})^2 )这一项的作用是防止平滑过程中点间距被拉得忽密忽疏。参考线后续要做曲率估计、速度规划、插值重采样点距不均匀会让这些环节全部出问题。把三项组合起来就得到完整的目标函数min J w_smooth * J_smooth w_ref * J_ref w_len * J_len三个权重决定了曲线行为w_smooth越大越平滑w_ref越大越贴近原始路径w_len越大点距越均匀。它们之间是相互制约的关系调参的乐趣基本都在这里。2.3 两类硬约束边界框与曲率上限目标函数负责“尽量平滑”硬约束负责“不许越界”。FemPosDeviation里有两类关键约束。边界框约束每个平滑点必须落在原始参考点附近的一个矩形框内用公式表示就是|x_i - x_ref_i| ≤ bound_x|y_i - y_ref_i| ≤ bound_ybound就是允许的最大横向偏离量。这个约束意义重大它保证了平滑后的参考线不会离原始道路中心太远。如果没有它w_ref一不小心设小了曲线就可能飘到路外面去。曲率约束这是FemPosDeviation加SQP这套方案相对纯QP方案的最大区别。车辆有最小转弯半径参考线的曲率必须小于对应上限。离散点上近似曲率可以用相邻三点v1 p_i - p_{i-1}和v2 p_{i1} - p_i的夹角来表达κ ≈ 2 * sin(θ/2) / L其中θ是v1和v2的夹角L是相邻点平均距离。曲率上限κ_max一给θ就有上限。而这个约束写成三点坐标形式后是二次不等式且整体上不保证凸性这就是后面为什么要上SQP的根本原因。2.4 FEM这个缩写到底指什么很多资料把FemPosDeviation解释成“有限元位置偏差”初看很拗口。其实这里的FEM思想就是有限元里的“最小势能原理”一个弹性结构在外力作用下达到平衡时系统总势能最小。我们的弹性金属丝在“约束环”之间弯曲弯曲应变能最小化就是势能最小化的离散版本。“位置偏差”则体现在目标函数里那一项w_ref * J_ref上——平滑后的曲线位置相对原始参考线的偏差要尽可能小。组合起来算法做的事情就是在位置偏差尽量小的前提下让弯曲能量最小化。理解到这个层面后面看代码里的目标函数组装就不会迷路。3. SQP迭代曲率约束逼着我们把QP升级3.1 为什么纯QP方案无法表达曲率约束如果只看目标函数平滑问题其实是一个标准的二次规划QP目标函数是二次的边界框约束是线性的OSQP这类求解器毫秒级就能解完。事实上Apollo早期的参考线平滑器就是这么干的纯QP方案速度快实现简单。但纯QP方案有个硬伤它无法处理曲率约束。曲率约束在坐标形式下是相邻三点坐标的二次不等式这种约束在全局范围内不保证凸不能直接塞进QP的线性约束框架里。于是就会出现一种尴尬情况QP解出来的“最优平滑曲线”在某些急弯处曲率超过了车辆极限下游照样要硬着头皮解决“跟得上”的问题。要打破这个天花板必须从QP升级到非线性优化。而SQP序列二次规划就是非线性优化里最讲究工程实用性的一种方法。3.2 SQP子问题怎么构造线性化、二次近似与可信域SQP的思路直白得有些粗暴既然非线性约束难处理那就在当前迭代点把它做一阶泰勒展开变成线性约束。每次迭代求解一个带线性约束的QP子问题子问题的解指导下一步移动。反复迭代最终逼近原问题的解。具体流程是这样的取初值x0可以是上一次的平滑结果也可以先用纯QP做一版预平滑结果。在当前迭代点x_k处对曲率约束c(x)做一阶泰勒展开c(x_k) ∇c(x_k)^T (x - x_k) ≤ 0展开后是x的线性不等式。构造QP子问题目标函数用原目标函数因为原目标本身是二次的Hessian是常数这个子问题和原问题共享同一个目标约束换成线性化后的曲率约束加上原本的线性约束边界框、首末点固定。求解QP子问题得到增量Δx。更新x_{k1} x_k α * Δx其中α是步长系数。检查收敛条件不满足就回到第2步。这中间有一个细节直接关乎成败线性化只在当前点附近有效如果Δx太大线性化的约束和真实非线性约束之间的误差会急剧增大迭代就可能振荡甚至发散。工程上的应对手段是加“可信域”——给Δx的范数设定上限或者直接限制本次迭代里每个点最多移动多少米。我实际调试时习惯把单步最大移动距离压到0.1到0.2米迭代3到5次既能保证稳定速度也够快。3.3 迭代控制乘子更新、收敛判据与最大迭代次数QP子问题的另一个产出物是拉格朗日乘子。SQP里这些乘子不是算完就扔的它们会作为下一次迭代构造子问题时的乘子初值这也是“序列”二字的精髓之一——每一步都在向最优解逼近同时保留上一步的约束敏感度信息。收敛判据我一般看两个量曲率约束的违规量即当前点代入非线性约束后的最大值低于阈值比如曲率超限小于0.5%就算过。当前迭代相对上一步的目标函数下降量小到一定程度说明已经到平台期再迭代也是浪费时间。实车上不可能每次都等到完全收敛。我更习惯直接限制最大迭代次数Apollo里的默认配置一般也就3到5次。这样做有个好处每次平滑耗时可预估不会出现某个场景突然多跑几十次迭代导致帧耗时超限。代价是解可能不是原问题的最优解但只要约束违规量在可接受范围内对于参考线平滑这个场景已经完全够用了。3.4 Apollo里SQP与IPOPT、OSQP的定位演变Apollo参考线平滑这块的求解器演进本身就说明了SQP方案的价值。早期的离散点平滑器就是纯QP用OSQP求解快是真快但没有曲率约束。后来引入FemPosDeviation建模最开始用IPOPT这类通用非线性求解器直接解QCQP。IPOPT能处理曲率约束但有两个问题一是单次求解耗时波动大复杂路段可能几十毫秒到上百毫秒二是依赖重调试和部署成本高。再往后方案演变成自定义SQP框架加OSQP作为底层QP子问题求解器把非线性的曲率约束拆成几步线性化子问题既保住了约束能力又把耗时压回到毫秒级。这套组合的本质是用SQP的迭代框架去处理非线性用OSQP的高效稀疏QP求解去处理迭代中每一步的线性子问题。各取所长工程上非常优雅。4. 从公式到代码平滑器上车的工程细节4.1 变量编排与稀疏矩阵组装实现的第一步是把优化变量编排清楚。假设参考线有N个离散点每个点的x和y都是优化变量总变量数是2N状态向量z可以排成z [x0, y0, x1, y1, ..., x_{N-1}, y_{N-1}]ᵀ这个编排方式决定了矩阵的稀疏结构。目标函数的Hessian矩阵H是一次性组装的常数矩阵平滑项贡献块三对角结构每个点p_i只和p_{i-1}、p_{i1}耦合位置回拉项只在对角线上有值点距均匀项贡献相邻点之间的2x2块。整个H是一个块稀疏矩阵非常适合用稀疏矩阵格式存储和计算。线性约束部分分两类。边界框约束是简单的单位行第i个点的x约束只涉及变量2i这一列。曲率约束经过线性化之后会变成涉及六个变量三个点的x、y的窄带行。每次SQP迭代这一部分约束矩阵的系数要重新计算更新其他部分保持不变。伪代码层面大体是这种感觉# 伪代码FemPosDeviation-SQP骨架 z0 init_points() # 初值原始参考点或上一帧平滑结果 for k in range(max_iter): A_curv, lb_curv, ub_curv linearize_curvature_constraints(z_k) A_total assemble_constraint_matrix(A_bound, A_curv) l_total assemble_constraint_bounds(l_bound, lb_curv) result solve_qp(H, g, A_total, l_total, u_total, z_k) delta result.x - z_k step clip(delta, max_step) z_k_next z_k alpha * step if check_convergence(z_k_next, result): break z_k z_k_next这个流程里最影响性能的是linearize_curvature_constraints因为每次迭代都要重新算所有点的曲率约束雅可比。实测下来N在200到600个点这个量级一次迭代里这部分的开销非常小整体耗时基本由QP子问题求解占主导。4.2 坐标平移与数值尺度处理这一节的经验是用大量调试点换来的。自动驾驶的地图坐标通常是UTM坐标系数值动辄几十万米。这个数值尺度直接丢给优化求解器会引发严重的数值问题。Hessian里平滑项权重如果给的又是1e10这种级别H矩阵的元素轻易就到1e20OSQP内部计算条件数差得离谱轻则收敛慢重则直接报数值错误。我的处理办法是在平滑器入口做坐标预处理取参考线首点为原点把所有点平移到局部坐标系下必要时还可以整体缩放让坐标值落在米级甚至更小的量级。平滑完成后再把结果加回平移量输出到全局坐标系。这一步对数值稳定性带来的提升比调任何权重都明显。另一个常见问题是边界框和权重的量级不匹配。如果坐标已经缩到米级bound给0.2米、位置偏差权重给1e5平滑项权重给1e9左右系统行为就比较正常但如果你跳过了坐标预处理还是用这套参数那结果必然是一团糟。所以遇到求解器报错先检查坐标尺度再检查权重顺序不要搞反。4.3 三个权重的实际调参流程权重的调法是有套路的不能上来就瞎调。我习惯按下面这个顺序走先定bound。bound的下限要大于地图误差和定位误差之和。地图有0.1米的误差定位有0.1米的误差bound小于0.3米就会让平滑器过拟合噪声怎么调都不顺。固定w_smooth和w_len只调w_ref。先让平滑后的点最大偏移量接近但不超过bound值这样保证曲线不会飘出允许范围。逐步增大w_smooth观察两个指标最大曲率和弯曲能量下降曲线。一般会出现一个拐点过了这个点曲率下降变得非常缓慢取拐点附近的权重即可。加上曲率约束检查约束被激活的位置和数量。如果几乎所有点都压在曲率约束上说明w_smooth偏小曲率约束成了主导如果没有任何约束激活说明w_smooth已经够大可以适当降一降来减少位置损失。最后检查帧耗时和迭代次数如果迭代次数总顶到上限适当降低w_ref或者放宽bound。这套流程看起来朴素但我试过很多次比直接从网上抄一串权重再盲调高效得多。实测下来权重之间的相对量级关系通常差个几万到几百万倍但这种比值一定建立在坐标预处理得当的前提上离开了坐标预处理谈权重就是耍流氓。4.4 求解失败时的降级逻辑参考线平滑在完整规划链路里的地位决定了它必须是一个“宁可效果差一点不能把规划搞停”的模块。所以异常处理策略必须提前设计好。我见过三种典型的失败场景QP子问题报原始不可行常见原因是bound太紧加上曲率约束线性化后和边界约束打架。第一步先尝试放宽bound按1.1倍、1.2倍逐级放大放到1.5倍还不行就放弃曲率约束退化成纯QP。SQP迭代不收敛表现为约束违规量迟迟降不下去或者振荡。先把单步步长砍半再不行就换初值用纯QP的解作为SQP的起点。根本性失败比如参考线输入本身包含大量NaN点。这种情况下任何优化都无解直接返回原始参考线至少能让规划模块继续运行而不是输出空结果导致整个规划停摆。这条降级链路的背后是一个原则参考线平滑是质量增强模块不是安全关键模块。它的失败可以被下游感知但不能让下游直接宕机。5. 调试过程踩过的坑与横向对比5.1 故障一QP子问题报原始不可行这是我自己实现SQP时遇到的第一个拦路虎。现象很明确OSQP的求解状态返回PRIMAL_INFEASIBLE整个参考线平滑输出为空。排查路径是这样的先把曲率约束从SQP迭代里摘掉单独用QP解一遍发现能正常出解说明问题出在线性化的曲率约束上。再把曲率约束相关权重放大到极端值发现某些弯道处的线性化约束和边界框约束确实存在不可行区曲率约束要求三点不能弯得太急边界框又限制每个点不能离原始点太远两者一叠加可行域直接被压缩没了。解决办法有两层。第一层在SQP迭代里给线性化的曲率约束加松弛变量约束变得软一点优先保证子问题可解。第二层如果连续多次迭代仍然不可行就按降级策略逐步放宽bound直到恢复可行。从此之后PRIMAL_INFEASIBLE这类问题再也没让我在半夜爬起来过。5.2 故障二SQP迭代发散平滑点来回震荡另一个教训来自初值选择。最早我直接用原始参考点做SQP的初值结果在一些急弯处第一次迭代的线性化步长就跨出了有效范围第二步直接反弹到反方向之后几个迭代来回震荡曲率约束违规量越来越大。后来我把初值策略改成了两步走先用纯QP解一版不带曲率约束的平滑结果再用它作为SQP的初值。这个初值本身已经比较平滑曲率约束的违规量不大SQP只在小范围内修整稳定性一下子提升了一个档次。与此同时我对每步迭代的Δx加了0.15米的限幅相当于给线性化一个可信域双管齐下震荡问题基本绝迹。如果你在实车上观察到了SQP发散我的建议是先别动权重先检查初值和单步步长限制。这两个因素对稳定性的影响远大于权重微调。5.3 和其他平滑方案的实测差异我把几种常见方案放在同一组真实道路数据下做过对比结果很有参考价值平滑方案能显式约束曲率单次耗时量级稳定性工程依赖离散点QP否毫秒级高轻样条拟合后重采样否毫秒级高轻IPOPT直接解QCQP是几十到几百毫秒一般重SQP加OSQP是毫秒级较好中纯QP方案在大多数平缓路段上其实够用但遇到回头弯、匝道这类曲率变化剧烈的场景没有曲率约束的代价就会暴露出来。样条方案胜在简单但把曲率约束加进样条优化里处理起来并不比SQP轻松。IPOPT的效果上限最高但耗时的波动性让它很难在实车上稳定运行。SQP加OSQP最终胜出靠的不是某个单项指标而是“约束能力、速度、稳定性”三者的综合平衡。这个对比结果也解释了为什么Apollo后来把默认平滑方案切成SQP工程上选型从来不是选一个数学上最漂亮的方法而是选一个在限定算力上和限定时间内最靠谱的方案。FemPosDeviation加SQP恰好是这个位置上最靠谱的组合之一。最后再分享一个实际体会这套方案能不能跑好一半在建模一半在数值工程的细节。如果你正在自己实现别急着调权重先把坐标预处理、初值选取、失败降级这三件事做扎实。我后来复盘过很多次发现所有难缠的求解问题根因几乎都不在算法本身而在这些看起来不起眼的工程细节上。把这些细节吃透比拿到一版能跑的配置重要得多。