自动驾驶控制算法解析：从b站老王案例看PID与模型预测控制的实战对比

共计 1975 个字符，预计需要花费 5 分钟才能阅读完成。

最近在复现 b 站老王的自动驾驶小车项目时，发现控制算法选型对路径跟踪效果影响巨大。本文将通过实测对比，拆解 PID 和 MPC 这两种常用的控制算法，分享一些工程落地时的实用技巧。

1. 为什么控制算法总在弯道翻车？

在测试双移线工况时，我的小车频繁出现两种典型问题：

• 超调震荡 ：PID 控制器在急弯处像醉汉一样左右摇摆，横向误差最大达到 0.5 米
• 延迟响应 ：车速提到 30km/ h 后，系统响应明显滞后，导致切弯时总是压线

查看 b 站老王的实测数据日志，发现同样条件下：

[PID] 最大横向误差: 0.48m | RMS: 0.21m
[MPC] 最大横向误差: 0.15m | RMS: 0.07m 

2. 控制算法原理对比

2.1 PID 控制的三板斧

PID 的数学表达很简单：

$$u(t) = K_p e(t) + K_i \int_0^t e(\tau)d\tau + K_d \frac{de(t)}{dt}$$

但实际调参时总会遇到：

• 积分项饱和（上坡时持续加速）
• 微分噪声放大（传感器抖动导致方向盘抽搐）

2.2 MPC 的降维打击

MPC 的核心思想是滚动优化：

$$\min_{u} \sum_{k=0}^{N-1} (x_k^T Q x_k + u_k^T R u_k) + x_N^T P x_N$$

subject to:
  x_{k+1} = A x_k + B u_k
  u_{min} \leq u_k \leq u_{max}

相比 PID，MPC 有两个先天优势：

1. 显式处理系统延迟（通过预测模型）
2. 天然支持多约束（方向盘转角速率限制等）

3. 代码实现关键点

3.1 工业级 PID 实现（C++20）

class AntiWindupPID {
public:
  void reset() {
    integral_ = 0.0;
    prev_error_ = 0.0;
  }

  double update(double error, double dt) {
    // 积分项抗饱和
    if (ki_ > 0.0) {
      integral_ += error * dt;
      integral_ = std::clamp(integral_, -i_max_/ki_, i_max_/ki_);
    }

    // 微分项滤波
    double derivative = (error - prev_error_) / dt;
    derivative = lowpass_filter_.update(derivative);

    prev_error_ = error;
    return kp_ * error + ki_ * integral_ + kd_ * derivative;
  }

private:
  double kp_{0.0}, ki_{0.0}, kd_{0.0};
  double integral_{0.0}, prev_error_{0.0};
  double i_max_{1.0}; // 积分限幅
  LowPassFilter lowpass_filter_{10.0}; // 截止频率 10Hz
};

3.2 MPC 求解核心（Eigen 库）

struct MPCConfig {
  int horizon = 10; // 预测时域
  double dt = 0.1;  // 时间步长
  Eigen::MatrixXd Q; // 状态权重
  Eigen::MatrixXd R; // 输入权重
};

class MPCController {
public:
  VectorXd solve(const VectorXd& x0, const MatrixXd& ref_path) {
    // 构建 QP 问题
    Eigen::MatrixXd H = /* 二次项矩阵 */;
    Eigen::VectorXd f = /* 一次项向量 */;

    // OSQP 求解器配置
    OsqpSolver solver;
    solver.settings()->warm_start = true;

    // 返回最优控制序列
    return solver.solve(H, f, A_constraint_, lb_, ub_).col(0);
  }
};

4. 工程部署避坑指南

在实车部署时踩过的坑：

• QP 求解超时 ：当预测时域 N >15 时，默认 OSQP 配置可能超时

• 解决方案：启用 warm start，降低求解精度 tol=1e-4

• 雅可比矩阵计算瓶颈 ：数值微分法消耗 15% CPU

• 改用手写解析雅可比，耗时降低到 1% 以内

ROS2 参数配置建议：

controller:
  mpc:
    horizon: 8      # 平衡实时性与预测能力
    max_iter: 100   # 迭代次数限制
    q_angle: 2.0    # 航向角权重 

5. 仿真验证结果

在 CARLA 的 Town07 地图测试双移线工况（i7-11800H @4.2GHz）：

开放性问题

MPC 的预测时域就像望远镜——看得越远理论效果越好，但计算成本呈指数增长。在自动驾驶这样的实时系统中，如何找到预测时域和计算延迟的最佳平衡点？或许可以尝试：

• 时变预测时域（近处精细，远处粗略）
• 异步优化（控制周期内未完成求解时使用上次结果）

欢迎在评论区分享你的实战经验！