MPC与双PID协同的车辆轨迹跟踪控制实践

1. 项目背景与核心价值

轨迹跟踪控制在自动驾驶和高级驾驶辅助系统(ADAS)中扮演着关键角色。传统PID控制器在简单场景下表现尚可，但在复杂动态环境中往往力不从心。这个项目采用模型预测控制(MPC)框架，结合双PID结构，实现了车辆横向和纵向运动的协同控制。

我在实际工程中发现，纯MPC方案虽然理论优美，但存在计算负担大、实时性差的问题。而传统PID又难以处理多约束条件。这种混合架构恰好结合了两者优势：MPC负责全局优化和约束处理，PID则提供快速响应。实测表明，在保持控制精度的同时，系统响应速度提升了约40%。

2. 系统架构设计

2.1 整体控制框架

系统采用分层控制结构：

1. 上层MPC控制器：基于车辆动力学模型预测未来状态
2. 下层PID执行器：
   • 横向控制PID：调节前轮转角
   • 纵向控制PID：调节油门/制动

关键设计选择：MPC预测时域设为3秒(20个采样点)，这个时长既能覆盖典型操控反应时间，又不会引入过多计算延迟。

2.2 车辆动力学建模

采用经典的自行车模型作为预测模型：

code复制ẋ = v*cos(θ+β)
ẏ = v*sin(θ+β)
θ̇ = (v/l_r)*sin(β)
v̇ = a
β = arctan((l_r/(l_f+l_r))*tan(δ))

其中l_f/l_r为前后轴距，δ为前轮转角。这个简化模型在80km/h以下速度时误差小于5%。

3. MPC控制器实现细节

3.1 成本函数设计

成本函数包含四个关键项：

python复制J = w1*(y-y_ref)^2 + w2*(θ-θ_ref)^2 + w3*δ^2 + w4*a^2

权重系数通过灵敏度分析确定：

• 横向偏差权重w1=1.2
• 航向角权重w2=0.8
• 转向量权重w3=0.3
• 加速度权重w4=0.5

3.2 约束处理

考虑实际物理限制：

• 转向角约束：|δ| ≤ 30°
• 加速度约束：-3 m/s² ≤ a ≤ 2 m/s²
• 转向速率约束：|δ̇| ≤ 15°/s

在QP求解器中采用松弛变量处理软约束，避免无解情况。

4. 双PID控制器设计

4.1 横向PID参数整定

采用增量式PID避免积分饱和：
``