二次规划在自动驾驶轨迹优化中的工程实践：从Apollo框架到落地细节
当一辆自动驾驶汽车在城市道路上平稳地避开障碍物、完成变道时，背后是数学优化算法在实时计算最优轨迹。这其中，二次规划（Quadratic Programming, QP）扮演着核心角色——它能在毫秒级时间内，将复杂的物理约束和舒适性要求转化为可求解的数学问题。
1. 轨迹优化为何需要二次规划
自动驾驶的轨迹规划本质上是一个多目标优化问题：既要保证安全性（避开障碍物、遵守交通规则），又要考虑舒适性（加减速平缓、转向柔和），同时还需满足车辆动力学约束。这些需求最终都转化为数学上的代价函数和约束条件。
典型优化目标包括：
- 路径偏离惩罚：
(x-x_ref)^T Q (x-x_ref)（Q为权重矩阵） - 加速度平滑项：
u^T R u（u为控制输入） - 紧急制动规避项：基于距离的指数型代价函数
而约束条件则更为复杂：
# 车辆动力学约束示例（简化的自行车模型） def vehicle_dynamics(x, u): beta = atan(lr/(lf+lr) * tan(u[1])) # 滑移角 dx = np.zeros(4) dx[0] = x[3]*cos(x[2]+beta) # x方向速度 dx[1] = x[3]*sin(x[2]+beta) # y方向速度 dx[2] = x[3]/lr*sin(beta) # 横摆角速度 dx[3] = u[0] # 加速度 return dxQP的优势在于：
- 计算效率：相比一般非线性规划，凸QP问题有多项式时间解法
- 数值稳定性：成熟的求解器（如OSQP、qpOASES）能处理病态问题
- 模块化设计：便于与预测模块、控制模块集成
2. Apollo框架中的QP实践
百度Apollo的EM Planner采用分层优化架构，其中QP用于局部轨迹优化阶段。其核心流程如下：
2.1 问题建模
- 参考线离散化：将连续路径离散为50-100个点（间距0.5-1m）
- Frenet坐标系转换：将全局坐标转为(s,l)坐标，简化横向优化
- 五次多项式参数化：纵向s(t)和横向l(s)都用五次多项式表示
权重矩阵设计技巧：
Q = np.diag([1.0, 0.5, 0.1, 0.01]) # 位置>速度>加速度>加加速度 R = 0.1 * np.eye(2) # 控制输入权重2.2 约束构建
Apollo中典型的约束类型：
| 约束类型 | 数学形式 | 物理意义 |
|---|---|---|
| 边界约束 | l_min ≤ l ≤ l_max | 避免碰撞 |
| 动态约束 | x_{k+1} = A x_k + B u_k | 车辆动力学 |
| 曲率约束 | ` | d²l/ds² |
| 舒适约束 | ` | a |
矩阵构建示例（OSQP格式）：
P = sparse.csc_matrix(Q) # 二次项矩阵 q = -Q.dot(x_ref) # 线性项向量 A = sparse.vstack([A_eq, A_ineq]) # 约束矩阵 l = np.hstack([b_eq, -np.inf*np.ones(m_ineq)]) u = np.hstack([b_eq, b_ineq])2.3 求解加速技巧
- 热启动：用上一帧的解初始化当前求解
- 主动集识别：提前排除非活跃约束
- 矩阵稀疏性利用：使用CSC格式存储稀疏矩阵
- 并行计算：对多候选轨迹并行优化
实测数据：在Intel i7-1185G7上，Apollo的QP求解器能在5ms内完成100维问题的求解，满足实时性要求（控制周期通常为10-20ms）
3. 工程实践中的挑战与解决方案
3.1 数值稳定性问题
当约束条件接近冲突时，QP可能无解。Apollo采用的策略：
- 松弛变量法：对硬约束添加松弛项
minimize \ f(x) + ρ^T ε s.t. \ g(x) ≤ ε, ε ≥ 0- 优先级约束：安全约束优先于舒适约束
3.2 实时性保障
计算耗时分布（基于Apollo 7.0实测）：
| 模块 | 耗时占比 | 优化手段 |
|---|---|---|
| 矩阵构建 | 35% | 内存预分配、模板元编程 |
| 求解器 | 60% | 代数运算加速（AVX2指令集） |
| 结果校验 | 5% | 异步处理 |
3.3 参数调优经验
- 权重调整：先用对数尺度粗调（如[0.1, 1, 10]），再微调
- 约束松弛量：初始设为物理极限的10%（如ε_max=0.3m）
- 迭代次数：通常3-5次迭代即可达到1e-4精度
典型参数表：
| 参数 | 取值区间 | 影响敏感度 |
|---|---|---|
| 位置误差权重 | [0.5, 2.0] | 高 |
| 速度误差权重 | [0.1, 0.5] | 中 |
| 加速度权重 | [0.01, 0.1] | 低 |
| 松弛变量惩罚 | [1e3, 1e5] | 极高 |
4. 进阶优化方向
4.1 模型预测控制(MPC)集成
将QP嵌入MPC框架，实现滚动时域优化：
for k in range(horizon): # 构建当前时刻QP问题 P, q, A, l, u = build_qp(x_ref[k], obstacles) # 求解并应用第一步控制 u_opt = solve_qp(P, q, A, l, u) apply_control(u_opt[0]) # 状态更新 x = simulate(x, u_opt[0])4.2 数据驱动参数优化
通过强化学习自动调整QP权重：
- 定义奖励函数：
R = w_1·safety + w_2·comfort + w_3·efficiency - 使用PPO算法优化权重参数
- 在线自适应调整（如雨雪天增大安全权重）
4.3 硬件加速方案
- GPU加速：使用CUDA实现矩阵运算并行化
- FPGA部署：将QP求解器固化到硬件
- 专用芯片：如Tesla的DOJO加速器
在实际项目中，我们曾遇到过一个典型案例：某L4级自动驾驶车辆在通过施工区域时频繁急刹。分析发现是QP中曲率约束过于保守，通过动态调整曲率约束阈值（基于路面附着系数估计），使通过率从72%提升到89%，同时保持安全标准。
