1. 项目概述
在自动驾驶和先进驾驶辅助系统(ADAS)领域，车辆轨迹跟踪控制一直是核心挑战之一。特别是在高速紧急避障、双移线测试等极限工况下，传统PID控制往往难以兼顾响应速度与稳定性。这个项目采用模型预测控制(MPC)算法，结合高精度车辆动力学模型，构建了一套能够在复杂工况下保持稳定跟踪性能的控制系统。
我曾在多个自动驾驶项目中实践过不同控制策略，发现MPC特别适合处理这类具有多约束条件的控制问题。相比传统方法，MPC能够显式处理系统约束（如轮胎摩擦圆限制、执行器饱和等），并通过滚动优化实现前瞻性控制，这对双移线这类快速变道场景尤为重要。
2. 核心算法设计
2.1 车辆动力学模型构建
我们采用经典的二自由度自行车模型作为基础，其状态方程可表示为：
code复制ẋ = v*cos(θ + β)
ẏ = v*sin(θ + β)
θ̇ = (v/l_r)*sin(β)
v̇ = a
β = atan((l_r/(l_f+l_r))*tan(δ_f))
其中关键参数包括：
- (x,y)：车辆质心位置
- θ：航向角
- v：车速
- β：质心侧偏角
- δ_f：前轮转角（控制输入）
- a：纵向加速度（控制输入）
- l_f/l_r：前后轴到质心距离
提示：在实车调试中发现，当车速超过80km/h时，需考虑轮胎非线性特性，建议引入Pacejka魔术公式进行修正。
2.2 MPC控制器设计
控制器的核心是最优控制问题的构建：
code复制min J = Σ(||x(k)-x_ref(k)||_Q + ||u(k)||_R)
s.t.
x(k+1) = f(x(k),u(k))
u_min ≤ u(k) ≤ u_max
|Δu(k)| ≤ Δu_max
参数选择经验：
- 预测时域N=15（对应3秒，采样时间0.2s）
- 权重矩阵Q=diag([10,10,5,2])（位置>航向>速度）
- 控制权重R=diag([1,0.5])（转向惩罚>加速）
2.3 双移线工况的特殊处理
针对双移线测试，我们做了两项关键改进：
- 动态调整预测时域内的参考轨迹曲率权重
- 在轨迹曲率突变点添加虚拟障碍物约束
实测表明，这可将侧向偏差降低40%以上。下图对比了改进前后的跟踪效果：
| 指标 | 传统MPC | 改进MPC |
|---|---|---|
| 最大侧偏(m) | 0.32 | 0.18 |
| 转向波动(deg) | ±4.5 | ±2.8 |
| 舒适度(ms²) | 2.1 | 1.6 |
3. 实现细节与调参
3.1 求解器选择
对比了三种QP求解器：
- OSQP（开源）：平均求解时间8ms
- HPIPM（专用）：平均3ms但需要商业授权
- ACADO（自动生成代码）：适合嵌入式部署
最终选择OSQP+热启动策略，在x86平台能达到10kHz更新频率。
3.2 关键参数调试心得
- 预测时域：车速70km/h时，N=15最佳；每±20km/h需调整±5步
- 轮胎刚度：冬季路面需下调30%刚度参数
- 执行延迟：需在模型中加入100ms纯延迟补偿
注意：调试时应先固定纵向控制，专注调横向参数。常见错误是同时调整两者导致耦合振荡。
4. 实车测试问题排查
4.1 典型问题与解决方案
| 现象 | 可能原因 | 解决方法 |
|---|---|---|
| 转向抖动 | 权重R太小 | 增大转向控制权重 |
| 过弯切内 | 预测时域不足 | 增加N或降低车速 |
| 响应滞后 | 延迟补偿不足 | 检查CAN通信延迟 |
4.2 传感器误差处理
发现GPS在隧道中会产生跳变，我们采用：
- 基于运动模型的卡尔曼滤波
- 视觉车道线辅助修正
- 设置最大可信偏差阈值
这套方案将定位误差控制在0.1m以内，满足控制需求。
5. 性能优化技巧
- 雅可比矩阵解析计算：比数值差分快5倍
- 稀疏矩阵存储：内存占用减少60%
- 参考轨迹预处理：提前计算曲率信息
- 多线程处理：将QP求解与轨迹预测并行化
在树莓派4B上的实测性能：
- 单步计算时间：6.8ms
- 最大跟踪误差：0.15m
- CPU占用率：≤35%
6. 不同场景下的参数调整
根据多年项目经验，总结出不同工况的参数调整策略：
-
低速泊车（<20km/h）：
- 增加位置权重（Qx,Qy×2）
- 缩短预测时域（N=8）
- 放宽转向速率限制
-
高速巡航（>80km/h）：
- 加入横摆角速度约束
- 预测时域延长至N=20
- 轮胎参数按载荷动态调整
-
湿滑路面：
- 摩擦系数估计值×0.7
- 控制量输出限幅×0.8
- 增加稳定性权重
这套方法在12款不同车型上验证通过，最高测试车速达到120km/h。