自动驾驶速度规划：从QP到NLP的优化演进

1. 自动驾驶速度规划中的非线性优化挑战

在自动驾驶系统的决策规划模块中，速度规划算法扮演着关键角色。传统基于二次规划（QP）的PiecewiseJerkSpeedOptimizer虽然计算效率高，但在处理复杂场景时存在明显局限性。我在实际工程实践中发现，当车辆需要通过急弯或突然遇到限速区域时，QP方案往往会产生不符合预期的速度曲线。

这里存在一个典型的"时空错配"问题：假设某路段在空间区间[S1,S2]需要限速（比如曲率较大需降速），QP算法会根据初始动态规划生成的ST曲线（黑色）确定对应的时间窗口[T1,T2]，然后对这个时间窗口内的所有点施加速度约束。但经过QP优化后，新的速度曲线（黄色）会导致原空间区间对应的时间窗口发生偏移，最终实际约束的可能是错误的时空区域。

2. 从二次规划到非线性规划的演进

2.1 传统QP方法的固有缺陷

在Apollo 8.0及之前版本中使用的QP速度规划器存在两个核心问题：

1. 曲率惩罚失准：当参考线曲率较大时，QP优化后的速度曲线可能无法准确对应高曲率区域，导致车辆实际通过弯道时速度过高。我曾测试过一个案例：在曲率0.2m⁻¹的弯道上，QP方案给出的速度曲线比预期高了15%，这直接影响了横向控制稳定性。

2. 约束窗口漂移：如图1所示，限速区间[S1,S2]经过QP优化后，其对应的时间窗口会从[T1,T2]漂移到[T1',T2']。实测数据显示，在80km/h初始速度下，这种漂移可能导致约0.5秒的时间误差，相当于11米的距离偏差。

2.2 非线性规划的理论优势

非线性规划（NLP）通过以下方式解决上述问题：

• 空间域直接优化：直接在空间域（s方向）定义约束条件，避免时间窗口漂移问题。我们的实验表明，这种方法能将位置误差控制在0.3米以内。

• 高阶连续性保证：采用jerk（加加速度）作为优化变量，可以保证加速度曲线的C²连续性。在实际道路测试中，NLP方案的平均jerk值比QP方案降低了40%，显著提升了乘坐舒适性。

3. 非线性速度规划的实现框架

3.1 预处理流程

3.1.1 曲率曲线平滑

使用五次样条插值对原始曲率进行平滑处理。关键参数包括：

• 平滑权重α=0.5
• 曲率变化率约束|κ'|≤0.1 m