共计 1733 个字符，预计需要花费 5 分钟才能阅读完成。
背景与核心挑战
自动驾驶控制算法需要解决三个核心矛盾：
- 系统延迟：从传感器数据采集到执行器响应存在 100-300ms 延迟，传统反馈控制容易引发振荡
- 环境扰动：路面坡度变化、轮胎打滑等不可测干扰会显著影响控制精度
- 多目标优化：需同时满足跟踪精度、乘坐舒适性、能耗效率等相互冲突的指标
以 Apollo 7.0 实测数据为例，在潮湿路面 60km/ h 紧急变道场景中，单纯 PID 控制会导致横向误差超过 0.3m，而 MPC 能将其控制在 0.1m 内。
控制算法横向对比
| 指标 | PID | LQR | MPC |
|---|---|---|---|
| 响应速度 | 快（~10ms） | 中（~50ms） | 慢（100-300ms） |
| 抗干扰能力 | 弱 | 中 | 强 |
| 多约束处理 | 不支持 | 部分支持 | 完全支持 |
| 计算复杂度 | O(1) | O(n^3) | O(n^3·N) |
注：N 为 MPC 预测时域步长，Apollo 默认设置为 20 步（约 3 秒）
MPC 实现关键技术
车辆动力学建模
Apollo 采用经典的自行车模型进行简化：
// apollo/modules/control/common/mpc_controller.cc
void UpdateModelMatrix(const VehicleState &localization) {const double Lf = vehicle_param_.wheel_base(); // 轴距
const double delta = steer_angle_filtered_; // 前轮转角
// 状态矩阵 A (4x4)
matrix_a_(0, 2) = cos(phi_);
matrix_a_(1, 2) = sin(phi_);
matrix_a_(2, 3) = tan(delta) / Lf;
// 控制矩阵 B (4x2)
matrix_b_(2, 0) = v_ / (Lf * cos(delta) * cos(delta));
matrix_b_(3, 1) = 1.0; // 加速度输入
}OSQP 求解器配置
关键参数设置直接影响求解成功率：
// apollo/modules/control/lib/mpc_solver/qp_solver_osqp.cc
void OSQPSolver::Solve() {osqp_set_default_settings(&settings_);
settings_.max_iter = 500; // 最大迭代次数
settings_.eps_abs = 1e-5; // 绝对收敛阈值
settings_.eps_rel = 1e-5; // 相对收敛阈值
settings_.verbose = false; // 关闭调试输出
// 构造 QP 问题：min (1/2)x^T*P*x + q^T*x
// s.t. l <= A*x <= u
OSQPData data = {n, m, P.data(), q.data(), A.data(), l.data(), u.data()};
osqp_setup(&workspace_, &data, &settings_);
}工程实践技巧
权重矩阵经验值
Apollo 默认参数配置（单位归一化后）：
- 横向误差权重：1.0
- 航向角误差权重：0.5
- 转向角变化率权重：0.1
- 加速度变化率权重：0.05
提示：在高速场景（>80km/h）应适当降低横向误差权重，增加舒适性权重
实时性优化方案
- 热启动（Warm Start）：复用上一周期最优解作为初始猜测值，可减少 30% 迭代次数
- 时域压缩：将预测时域从 3 秒降至 1.5 秒（需同步提高规划频率）
- 稀疏矩阵优化：利用 OSQP 的 CSC 矩阵存储格式，减少内存拷贝开销
常见问题排查
求解失败处理流程
graph TD
A[MPC 求解失败] --> B{是否超时?}
B -->| 是 | C[启用备用 PID 控制器]
B -->| 否 | D[检查约束可行性]
D --> E[放松终端状态约束]
E --> F[降低预测时域]数值稳定性保障
- 避免小角度假设：当转向角 >30°时需采用完整非线性模型
- 矩阵条件数检查：在 UpdateModelMatrix()后添加断言
CHECK_LE(cond(A), 1e6) - 输入归一化：将状态变量统一转换到 [-1,1] 范围
开放性问题讨论
- 当规划模块与控制模块运行频率不同步时（如 20Hz vs 100Hz），如何设计插值策略？
- 在极端低附着路面（冰雪路面），是否应该动态切换控制算法？
- 如何验证 MPC 控制器的安全边界？是否需要引入形式化验证方法？
欢迎在评论区分享你的实战经验
正文完
