Apollo自动驾驶系统中的QP-Spline-ST速度优化器详解
【免费下载链接】apollo An open autonomous driving platform 
项目地址: https://gitcode.com/gh_mirrors/apo/apollo
前言
在自动驾驶系统中，路径规划只是第一步，如何让车辆沿着规划路径以最优的速度行驶同样至关重要。Apollo自动驾驶平台采用了一种基于二次规划(Quadratic Programming)和样条曲线(Spline)的ST速度优化方法，本文将深入解析这一核心算法。
一、ST图与速度优化问题
1.1 ST图概念
在完成路径规划后，Apollo将所有障碍物和自动驾驶车辆(ADV)转换到路径-时间(ST)图中。ST图的横轴表示时间(T)，纵轴表示沿路径的位置(S)，这种表示方法可以直观地展示车辆位置随时间的变化关系。
1.2 速度优化任务
速度优化的核心目标是在ST图上找到一条满足以下条件的轨迹：
- 无碰撞：避开所有障碍物
- 舒适性：加速度变化平缓
- 可行性：符合车辆动力学约束
二、QP-Spline-ST优化方法
2.1 基本思路
Apollo采用样条曲线表示速度剖面，并使用二次规划来寻找最优解。这种方法结合了样条曲线的平滑特性和二次规划的高效求解优势。
2.2 数学模型
优化问题的标准形式为：
minimize ½·xᵀ·H·x + fᵀ·x
s.t. LB ≤ x ≤ UB
A_eq·x = b_eq
A·x ≤ b
三、目标函数设计
3.1 样条分段
将ST轨迹分为n段，每段用五次多项式表示(默认，可配置)：
s = f_i(t) = a₀i + a₁i·t + a₂i·t² + a₃i·t³ + a₄i·t⁴ + a₅i·t⁵
3.2 平滑性代价(cost₁)
使轨迹平滑，最小化各阶导数的平方积分：
∑(w₁·∫(fᵢ')²ds + w₂·∫(fᵢ'')²ds + w₃·∫(fᵢ''')²ds)
3.3 巡航轨迹跟随代价(cost₂)
最小化与巡航ST轨迹(给定速度限制)的差异：
∑∑(fᵢ(tⱼ) - sⱼ)²
3.4 跟随轨迹代价(cost₃)
最小化与跟随ST轨迹的差异，形式与cost₂类似。
四、约束条件
4.1 初始点约束
确保轨迹从当前状态开始，包括：
- 初始位置
- 初始速度
- 初始加速度
这些约束转化为QP等式约束A_eq·x = b_eq。
4.2 运动方向约束
确保车辆保持前进方向。对采样点j和j-1：
- 同一样条段：fⱼ(tⱼ) > fⱼ₋₁(tⱼ₋₁)
- 不同样条段：fₖ(tⱼ) > fₗ(tⱼ₋₁)
4.3 连接平滑约束
确保相邻样条段在连接点处平滑：
- 位置连续：fₖ(tₖ) = fₖ₊₁(t₀)
- 速度连续：fₖ'(tₖ) = fₖ₊₁'(t₀)
- 加速度连续：fₖ''(tₖ) = fₖ₊₁''(t₀)
- 加加速度连续：fₖ'''(tₖ) = fₖ₊₁'''(t₀)
4.4 边界约束
沿路径均匀采样m个点，检查障碍物边界：
- 下边界：A·x ≤ b_lb
- 上边界：A·x ≤ -b_ub
4.5 速度边界约束
确保速度在允许范围内：
- 速度下限：f'(tⱼ) ≥ v_lb,ⱼ
- 速度上限：f'(tⱼ) ≤ v_ub,ⱼ
五、实现细节与调优建议
-
样条次数选择：默认使用五次多项式，可在配置中调整。更高次多项式更灵活但可能引入振荡。
-
采样点密度：增加采样点可以提高约束精度，但会增加计算量。
-
权重调整：平滑性权重(w₁,w₂,w₃)的调整会影响乘坐舒适性。
-
实时性考虑：QP问题的规模需要根据计算资源进行合理控制。
六、总结
Apollo的QP-Spline-ST速度优化器通过将速度规划问题转化为二次规划问题，结合样条曲线的平滑特性，实现了高效、舒适的速度规划。该方法考虑了各种实际约束条件，包括初始状态、障碍物避碰、速度限制等，是自动驾驶系统中速度规划的核心算法之一。
理解这一优化器的原理，有助于开发者更好地调参优化，也能为自动驾驶算法的改进提供思路。在实际应用中，还需要考虑车辆动力学特性、传感器噪声等因素，才能获得最佳的性能表现。
【免费下载链接】apollo An open autonomous driving platform 项目地址: https://gitcode.com/gh_mirrors/apo/apollo
创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
