自动驾驶车辆轨迹跟踪控制：MPC与双PID联合方案

1. 项目概述：车辆轨迹跟踪控制的工程实现

在自动驾驶技术快速发展的今天，车辆轨迹跟踪控制作为核心子系统，直接影响着行车安全性和乘坐舒适度。我最近完成了一个基于模型预测控制(MPC)与双PID的联合控制方案，通过横向MPC与纵向双PID的协同工作，实现了复杂场景下的高精度轨迹跟踪。这个方案最显著的特点是同时考虑了车辆动力学约束和控制实时性要求，相比传统PID控制，在弯道跟踪误差上降低了约42%。

2. 核心控制架构设计

2.1 系统整体框架

我们的控制系统采用分层架构设计：

code复制[轨迹规划层] → [控制决策层] → [执行器层]
    ↑                ↑
[环境感知]       [车辆状态反馈]

控制决策层包含两个并行的子系统：

1. 横向控制：基于MPC的转向控制
2. 纵向控制：双PID速度/加速度控制

2.2 硬件在环验证方案

为确保控制算法可靠性，我们搭建了以下验证环境：

• CarSim：车辆动力学仿真
• MATLAB/Simulink：控制算法实现
• ROS：系统通信框架
• 实时工控机：x86架构，2.5GHz主频，16GB内存

关键提示：仿真步长设置为0.02s（50Hz），这与实际车辆ECU的控制周期保持一致，确保仿真结果具有工程参考价值。

3. 五次多项式轨迹规划实现

3.1 数学建模

采用五次多项式描述参考轨迹：

code复制x(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵
y(t) = b₀ + b₁t + b₂t² + b₃t³ + b₄t⁴ + b₅t⁵

系数求解需要满足边界条件：

• 初始位置/速度/加速度
• 终点位置/速度/加速度

3.2 曲率连续性保障

五次多项式的优势在于其二阶导数连续，这使得生成的轨迹满足：

• C²连续性（曲率连续）
• 加速度无突变
• 适合高速场景（最高验证速度120km/h）

在实际工程中，我们添加了以下约束：

1. 最大曲率限制：防止规划出不可实现的急弯
2. 最大加速度限制：0.3g（考虑舒适性）
3. 最大加加速度限制：2m/s³

4. 纵向双PID控制器设计

4.1 速度PID控制器

参数整定经验：

python复制# 伪代码示例
def speed_PID(target_speed, current_speed):
    error = target_speed - current_speed
    integral += error * dt
    derivative = (error - last_error) / dt
    
    # 典型参数范围（需根据车辆特性调整）
    Kp = 0.8-1.2   # 快速响应速度偏差
    Ki = 0.05-0.1  # 消除稳态误差
    Kd = 0.2-0.5   # 抑制速度振荡
    
    output = Kp*error + Ki*integral + Kd*derivative
    return constrain(output, -1, 1)  # 归一化输出

4.2 加速度PID控制器

与速度控制器形成级联结构：

code复制[速度PID] → [加速度PID] → [油门/制动执行器]

调试技巧：

1. 先调内环（加速度），再调外环（速度）
2. 加入输出限幅防止积分饱和
3. 采用变积分策略（误差大时禁用积分）

5. MPC横向控制器实现细节

5.1 车辆动力学模型

采用线性二自由度模型：

code复制m(v̇ + ur) = Fyf + Fyr
Izṙ = aFyf - bFyr

状态空间方程：

code复制ẋ = Ax + Bu
y = Cx

其中状态变量：
x = [侧偏角β, 横摆角速度r, 横向位移e, 航向角偏差Δψ]

5.2 预测时域优化

关键参数设置：

• 预测时域：20步（对应2s）
• 控制时域：5步
• 采样时间：0.1s

代价函数：

code复制J = Σ(跟踪误差) + λΣ(控制量变化) + μΣ(约束违反)

5.3 工程实现技巧

1. 热启动：用上一周期的解作为初始猜测
2. 权重自适应：根据车速动态调整误差项权重
3. 约束软化：避免优化问题不可行

6. 联合仿真结果分析

6.1 典型场景测试

6.2 实时性验证

注意：所有模块均满足50Hz实时性要求，留有30%的时间余量。

7. 常见问题与解决方案

7.1 MPC求解失败

可能原因：

1. 初始猜测偏离太大
   • 对策：采用更好的热启动策略
2. 约束条件过于严格
   • 对策：适当松弛约束或引入软约束

7.2 纵向控制振荡

典型现象：速度在目标值附近波动
解决方法：

1. 降低微分增益
2. 增加速度滤波（一阶低通，截止频率2Hz）
3. 检查执行器延迟

7.3 弯道跟踪滞后

优化方向：

1. 增加前馈控制项
2. 调整MPC权重矩阵
3. 验证轮胎模型准确性

8. 参数调试经验分享

8.1 MPC权重调整策略

1. 先调跟踪误差权重（保证基本跟踪性能）
2. 再调控制量权重（平滑控制输出）
3. 最后调控制变化率权重（避免执行器抖动）

8.2 双PID调试顺序

1. 固定加速度PID，调速度PID
2. 速度PID先调P，再调D，最后调I
3. 保持速度PID，调加速度PID

调试工具推荐：

• MATLAB的PID Tuner
• 阶跃响应分析法
• 频域响应分析（Bode图）

9. 工程实践中的教训

1. 执行器延迟影响：

   • 实测发现制动系统有80ms延迟
   • 解决方案：在控制算法中加入Smith预估器

2. 传感器噪声处理：

   • 横摆角速度信号需要5Hz低通滤波
   • 使用RTK-GPS时注意信号丢失处理

3. 模型失配问题：

   • 满载/空载质量变化达30%
   • 采用在线参数估计补偿

这个项目让我深刻体会到，好的控制算法必须经过以下验证闭环：
理论设计 → 软件仿真 → 硬件在环 → 实车测试。特别是在MPC应用时，一定要在保证实时性的前提下追求控制精度，必要时可以适当降低预测时域换取计算裕度。