自动驾驶横纵向混合控制架构与PID+MPC实现

1. 自动驾驶控制系统的核心架构

在自动驾驶领域，横纵向控制是车辆运动控制的基础模块。纵向控制负责车辆的前后运动（加速/减速），横向控制则决定车辆的左右运动（转向）。这套系统相当于人类驾驶员手脚的配合——右脚控制油门刹车，双手控制方向盘转向。

我参与的这套系统采用PID控制纵向运动，MPC控制横向运动，背后依据的是经典的二自由度车辆动力学模型。这种组合方案在业界被称为"混合控制架构"，既保证了纵向控制的快速响应，又兼顾了横向控制的预测优化能力。

提示：二自由度模型指的是车辆在平面运动中只有纵向和横向两个运动自由度，忽略垂向运动、悬架动态等复杂因素，是自动驾驶控制中最常用的简化模型。

1.1 系统整体工作流程

当自动驾驶系统接收到规划层发出的轨迹指令后，控制流程是这样的：

  1. 轨迹分解器将全局轨迹拆解为横向位置偏差和纵向速度指令
  2. 纵向PID控制器根据速度误差计算油门/刹车指令
  3. 横向MPC控制器预测未来数秒的车辆状态，优化方向盘转角
  4. 底层执行器将控制指令转化为实际动作
  5. 车辆状态通过传感器反馈形成闭环

这种架构的优势在于：

  • 纵向控制简单直接，PID足以应对常见的加速制动场景
  • 横向控制需要更高精度，MPC的前瞻性可以避免"画龙"现象
  • 二自由度模型计算量适中，适合实时控制需求

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

2.1 PID控制原理与参数整定

PID控制作为经典算法，由比例(P)、积分(I)、微分(D)三部分组成。在纵向控制中：

  • P项对应当前速度误差的快速响应
  • I项消除稳态误差（如长上坡时的速度维持）
  • D项抑制速度突变带来的振荡

具体实现时，我的经验公式是：

油门指令 = Kp×速度误差 + Ki×误差积分 + Kd×误差微分 刹车指令 = 当油门指令为负值时取绝对值

参数整定过程建议：

  1. 先设Ki=0，Kd=0，逐步增大Kp直到出现轻微振荡
  2. 加入Kd（通常为Kp的1/10~1/5）抑制振荡
  3. 最后加入小量Ki消除稳态误差

注意：电动车需要特别注意再生制动与机械制动的平滑过渡，这里建议设置5%的死区缓冲。

2.2 实际应用中的改进策略

基础PID在自动驾驶场景需要多项增强：

  1. 速度分段PID：低速（<30km/h）用大Kp保证响应，高速用小Kp确保平稳
  2. 坡度补偿：通过IMU检测坡度，在PID输出上叠加固定补偿量
  3. 前车距离保护：当跟车距离小于安全值时，强制覆盖PID输出为减速指令

实测数据表明，经过优化的PID控制器在80km/h以下速度段，速度跟踪误差可以控制在±0.5m/s以内，满足大多数场景需求。

3. 横向MPC控制器的设计实现

3.1 车辆动力学模型建立

基于二自由度模型，我们建立如下状态方程：

状态量 x = [横向位置y 横摆角ψ 横向速度vy 横摆角速度r] 输入量 u = 前轮转角δ 输出量 y = 横向位置y 状态方程： ẋ = A·x + B·u y = C·x

其中矩阵A、B、C的参数来自车辆特性：

  • 质量m、转动惯量Iz
  • 前后轴侧偏刚度Cf、Cr
  • 轴距L（前轴到质心距离a，后轴到质心距离b）

3.2 MPC优化问题构建

在每个控制周期（通常50-100ms），MPC执行以下步骤：

  1. 预测时域（通常3-5秒）内系统的状态演变

  2. 构建代价函数：

    J = Σ(位置误差² + 转向量² + 转向变化率²)

  3. 添加约束条件：

    • 最大转向角（通常±30度）
    • 转向速率限制（通常±15度/秒）
    • 轮胎侧偏角约束（避免失稳）

  4. 求解优化问题得到最优转向序列

  5. 只执行第一个控制量，下一周期重新优化

3.3 工程实现技巧

在实际编码中，有几个关键优化点：

  1. 模型离散化：采用前向欧拉法，步长与控制周期一致
  2. QP求解器选择：OSQP比IPOPT更适合实时系统
  3. 热启动：用上一周期的解作为本次初始猜测
  4. 权重自适应：高速时增大位置误差权重，低速时减小

测试数据显示，MPC控制器在60km/h速度下，横向位置误差可控制在±0.3m以内，明显优于纯追踪算法。

4. 系统集成与联合调试

4.1 横纵向协同控制策略

单纯的纵向或横向控制都不足以保证良好体验，必须考虑二者的耦合效应：

  1. 速度-转向解耦：高速时减小转向灵敏度
  2. 制动-转向协调：紧急制动时暂时冻结横向控制
  3. 过弯速度规划：根据曲率自动限制最高速度

我的实现方案是增加一个协调层，接收横纵向控制指令后，进行以下处理：

def coordinate_control(steer, accel): # 速度灵敏度调整 steer_gain = 1.0 / (1.0 + 0.05*abs(velocity)) steer *= steer_gain # 制动优先逻辑 if accel < -3.0: # 急刹车 steer = 0.8 * steer # 减小转向 return steer, accel

4.2 典型问题排查指南

在实际调试中，常见问题及解决方案：

现象可能原因解决方法
纵向速度振荡PID参数过于激进减小Kp，增加Kd
转向响应迟钝MPC预测时域过短从3秒增至5秒
高速过弯偏移模型参数不准确重新标定Cr/Cf
紧急制动跑偏横纵向耦合过强启用制动协调策略

4.3 实车测试注意事项

最后分享几点实车调试经验：

  1. 安全第一：初始测试时限制最高速度为20km/h
  2. 分段验证：先单独测试纵向，再单独测试横向
  3. 数据记录：保存所有传感器原始数据和控制指令
  4. 可视化工具：实时绘制轨迹跟踪效果图
  5. 参数渐变：每次只调整1-2个参数，小步迭代

这套系统经过3个月的迭代测试，最终在园区接驳车上实现了小于0.4m的轨迹跟踪精度，证明了混合控制架构的可行性。对于想尝试类似项目的开发者，建议先从仿真环境（如CARLA）开始验证核心算法，再逐步过渡到实车部署。