自动驾驶横纵向PID+MPC混合控制方案详解

1. 自动驾驶横纵向控制方案概述

在自动驾驶系统开发中，横纵向控制器的设计直接影响车辆行驶的安全性和舒适性。我最近基于Carsim和Simulink联合仿真平台，实现了一套采用PID+MPC混合控制策略的自动驾驶控制器。这套方案最显著的特点是：

• 纵向控制：采用双环PID结构，直接参考百度Apollo的成熟设计
• 横向控制：基于模型预测控制(MPC)算法，使用车辆二自由度模型作为预测模型
• 轨迹规划：采用五次多项式生成参考轨迹，兼顾平滑性和计算效率

实测在双移线工况下，横向位置误差可控制在0.2米以内，纵向速度误差保持在±3km/h范围内。这个精度虽然还达不到L4级自动驾驶的要求，但对于研究级应用已经足够验证算法可行性。

关键设计选择：油门和刹车共用同一个PID控制器，通过输出信号的正负自动切换控制模式。这种设计避免了传统双控制器方案中的模式切换抖动问题。

2. 纵向PID控制器实现细节

2.1 双环控制结构解析

Apollo风格的纵向控制器采用速度-加速度双闭环结构：

1. 外层速度环：根据目标速度与实际速度的偏差，计算出期望加速度
2. 内层加速度环：通过PID调节油门/刹车开度，使实际加速度跟踪期望值

这种分层设计的好处是将速度跟踪问题分解为两个相对独立的控制子问题，每个环节的参数调节更直观。实际调试时，建议先调内环再调外环。

2.2 PID实现代码剖析

在Simulink中，我使用S-Function实现了如下PID控制逻辑：

matlab复制function [throttle, brake] = fcn(target_accel, current_speed)
    persistent pid;
    if isempty(pid)
        pid = pidController('Kp',0.8,'Ki',0.05,'Kd',0.02); 
    end
    
    accel_error = target_accel - getActualAccel();
    control = step(pid, accel_error);
    
    if control > 0
        throttle = saturate(control, 0, 1);
        brake = 0;
    else
        throttle = 0;
        brake = saturate(-control, 0, 1);
    end
end

几个关键设计点：

1. 使用同一个PID对象处理油门和刹车，通过输出极性自动切换
2. 饱和函数确保输出在[0,1]范围内
3. 故意未做积分抗饱和，利用稳态时的自然平衡

2.3 参数整定经验

经过大量仿真测试，总结出以下参数调节规律：

实测发现当车速稳定在30km/h时，油门开度约18%即可维持匀速，这与多数经济型燃油车的实际数据吻合。

3. 横向MPC控制器设计

3.1 车辆动力学模型建立

MPC的核心是预测模型，这里采用经典的二自由度自行车模型：

code复制状态方程：
dx/dt = A·x + B·u

其中：
x = [横向位置偏差, 横向速度, 横摆角偏差, 横摆角速度]ᵀ
u = 前轮转角

状态矩阵A的表达式如下：

cpp复制A << 0, 1, 0, 0,
     0, -(Cf+Cr)/(m*vx), (Cf+Cr)/m, -(a*Cf - b*Cr)/(m*vx),
     0, 0, 0, 1,
     0, -(a*Cf - b*Cr)/(Iz*vx), (a*Cf - b*Cr)/Iz, -(a²*Cf + b²*Cr)/(Iz*vx);

模型中各参数含义：

• Cf/Cr：前/后轮侧偏刚度
• m：车辆质量
• Iz：绕Z轴转动惯量
• a/b：质心到前/后轴距离
• vx：纵向车速

3.2 MPC实现中的关键技术点

1. 低速处理：当vx→0时矩阵元素会发散，实际实现中设置vx<1m/s时冻结横向控制

2. 权重调节：经过大量测试，推荐权重配置：

   • 横向误差权重：500
   • 横摆角误差权重：200
   • 前轮转角权重：10
   • 转角变化率权重：5

3. 预测时域：一般取20-30步，对应3-5秒（取决于仿真步长）

3.3 曲率突变问题解决方案

五次多项式轨迹在转折点容易出现曲率突变，导致MPC计算的前轮转角剧烈变化。我们通过两种方式缓解：

1. 轨迹滤波：在规划与控制层之间加入低通滤波器，限制曲率变化率≤0.1m⁻¹/s
2. MPC约束：在优化问题中增加转角变化率约束，|Δδ|≤5°/step

实测表明，滤波会引入约0.5m的跟踪滞后，这是平滑性与响应速度的典型权衡。

4. 联合仿真实践技巧

4.1 Carsim-Simulink接口配置

联合仿真中最容易出问题的环节是接口配置，分享几个关键检查点：

1. 初始化顺序：必须在S-Function的Initialize子函数中先调用VS_Init()
2. 信号映射：确保Carsim输出与Simulink输入端口严格对应
3. 单位统一：特别注意角度单位（rad/deg）和坐标系定义的一致性

4.2 调试工具配置建议

为提高调试效率，推荐在模型中添加以下监测点：

1. 关键状态变量：横向误差、横摆角、车速
2. 控制输出：油门/刹车指令、方向盘转角
3. 中间计算结果：MPC优化代价、PID输出

可以使用Simulink的Dashboard模块创建可视化监控面板，实时观察各信号变化。

4.3 典型问题排查指南

5. 性能优化与改进方向

当前方案在理想条件下表现良好，但在噪声环境下仍有提升空间：

1. 抗干扰增强：

   • 在MPC中增加扰动观测器
   • 采用鲁棒MPC设计方法
   • 实现动态预瞄距离调整

2. 计算效率优化：

   • 将QP求解替换为更高效的算法
   • 尝试显式MPC实现
   • 减少预测时域长度

3. 参数自适应：

   • 根据车速自动调整PID参数
   • 基于路面附着系数修正轮胎模型

这套控制系统目前对输入轨迹质量要求较高，加入10cm噪声就会导致明显性能下降。下一步计划引入轨迹预测和运动补偿机制，提升在复杂场景下的鲁棒性。控制算法的开发永远都是在理想与现实的夹缝中寻找最优解，这或许就是它的魅力所在。