CarSim环境下基于MPC的自动驾驶车辆轨迹跟踪控制：原理与实现

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

为什么选择 MPC 进行轨迹跟踪？

传统 PID 控制在车辆轨迹跟踪中存在三个显著问题：

• 无法显式处理多输入多输出系统的耦合特性
• 难以融入车辆动力学约束（如轮胎摩擦圆）
• 对非线性系统的适应能力有限

而模型预测控制 (MPC) 通过滚动优化和反馈校正机制，可以很好地解决这些问题。我们通过 CarSim 仿真对比三种控制器在双移线工况下的表现：

车辆建模关键步骤

1. 动力学模型线性化

采用自行车模型作为预测模型，在平衡点附近进行线性化：

\begin{aligned}
\dot{x} &= v\cos(\psi) - v\sin(\psi)\beta \\
\dot{y} &= v\sin(\psi) + v\cos(\psi)\beta \\
\dot{\psi} &= r \\
\dot{v} &= \frac{F_{x}}{m} \\
\dot{r} &= \frac{N}{I_z}
\end{aligned}

其中 β 为滑移角，通过小角度假设简化后得到状态空间方程：

\dot{\mathbf{x}} = A\mathbf{x} + B\mathbf{u}

2. 离散化处理

采用零阶保持法离散化，采样时间 Δt=0.05s：

Ad = expm(A*dt);
Bd = (Ad - eye(size(Ad)))*inv(A)*B;

CarSim-Simulink 联合仿真配置

1. 在 CarSim 中设置车辆参数：
2. 选择 Sedan_2017 车型
3. 设置初始速度 60km/h

4. 激活 4DOF 动力学模式

5. Simulink 接口配置：

6. 添加 CarSim S-Function 模块
7. 配置输入输出信号映射表：

[VS_COMMAND, 1]   -> 方向盘转角
[VS_COMMAND, 2]   -> 油门开度
[VS_SIGNAL, 1]    <- 横向偏差
[VS_SIGNAL, 2]    <- 航向角偏差

MPC 核心代码实现

function [u, cost] = mpc_controller(x0, ref_traj)
    % 定义权重矩阵
    Q = diag([10, 5, 2, 1]);  % 状态权重
    R = 0.1*eye(2);           % 控制权重

    % 构建预测方程
    for k = 1:N
        A_all((k-1)*nx+1:k*nx,:) = Ad^k;
        for j = 1:k
            B_all((k-1)*nx+1:k*nx,(j-1)*nu+1:j*nu) = Ad^(k-j)*Bd;
        end
    end

    % 二次规划求解
    H = B_all'*Q*B_all + R;
    f = x0'*A_all'*Q*B_all - ref_traj'*Q*B_all;
    u = quadprog(H, f, [], [], [], [], lb, ub);
end

参数调优实战技巧

权重矩阵调整原则

• 先调 Q 矩阵确保跟踪精度：
• 横向误差权重 > 航向误差权重
• 速度误差权重最后调整
• 再调 R 矩阵平滑控制：
• 从 0.1 开始逐步增加
• 观察方向盘波动幅值

时域参数选择

推荐经验公式：

T_p = \frac{v}{2a_{max}}

其中 v 为当前车速，a_max 为最大预期减速度。对于 60km/ h 工况：

• 预测时域 Tp=1.5s → N=30 步
• 控制时域 Tc=0.5s → Nu=10 步

工程避坑指南

1. 执行器饱和处理：

2. 在 QP 求解中加入约束条件：

   lb = [-25; -0.5];  % 方向盘限幅±25deg
   ub = [25; 1];      % 油门限幅 0 -1

3. 实时性优化：

4. 使用热启动 (warm-start) 技术
5. 采用显式 MPC 方法

6. 启用 MATLAB Coder 生成 C 代码

7. 接口同步问题：

8. 设置 CarSim 仿真步长 =MPC 采样周期
9. 启用 RT 模式保证时序一致

仿真结果分析

在积雪路面工况下（μ=0.3），MPC 控制器表现出：
– 横向误差 RMS 值降低 37%
– 方向盘调节频率减少 42%
– 计算耗时平均 18ms

开放讨论

在实际部署中，我们面临计算延迟与跟踪精度的权衡：

• 增加预测时域提升精度 → 计算量指数增长
• 减少优化变量加快求解 → 可能失稳

您在实践中采用过哪些平衡策略？欢迎在评论区分享经验。

延伸阅读

1. Rajamani R. Vehicle dynamics and control[M]. Springer, 2011.
2. Katriniok A, et al. LTV-MPC approach for vehicle path tracking[J]. IFAC, 2016.