保姆级教程：手把手教你用Matlab/Simulink复现无人驾驶MPC轨迹跟踪（附完整代码）

从零实现无人驾驶MPC轨迹跟踪：Simulink实战指南与代码解析

当第一次打开《无人驾驶车辆模型预测控制》第四章时，满屏的矩阵推导和状态空间方程确实容易让人望而生畏。作为在自动驾驶领域深耕多年的工程师，我完全理解这种理论到实践的鸿沟——这正是三年前我接手第一个轨迹跟踪项目时的真实写照。本文将分享如何用最直观的方式在Simulink中搭建完整MPC控制系统，包含7个关键操作步骤和3类典型报错解决方案，即使没有深厚控制理论背景也能快速上手。

1. 环境配置与基础准备

1.1 软件环境搭建

推荐使用MATLAB R2021a及以上版本与Carsim 2019的组合，这是经过多个项目验证的稳定搭配。安装时需特别注意：

• MATLAB附加工具包：必须安装Control System Toolbox和Optimization Toolbox
• Carsim接口配置：在Carsim安装目录下找到Solver_Interfaces文件夹，将carsim_block.mdl复制到MATLAB工作路径
• 版本兼容性检查：执行以下命令验证环境是否正常：

  >> ver % 查看工具箱版本 >> carsimlib % 测试Carsim模块加载

1.2 车辆参数设置

在Carsim中新建工程时，重点配置以下参数（以常见乘用车为例）：

提示：这些参数会直接影响MPC控制效果，建议先在Carsim的"Parameters"模块中预设好模板。

2. Simulink模型架构设计

2.1 核心模块连接

建立如图所示的信号流架构：

[CarSim Vehicle Model] → [Sensor Outputs] → [MPC Controller] → [Actuator Commands] → [CarSim Inputs]

具体操作步骤：

1. 从Carsim界面点击"Send to Simulink"
2. 在Simulink库浏览器添加以下模块：
   • MPC Controller（自定义S-Function）
   • XY Graph（轨迹可视化）
   • To Workspace（数据记录）

2.2 接口信号配置

关键接口信号对应关系：

% Carsim输出 → MPC输入 inputs = { 'X0', 'Position_X'; % 车辆X坐标 'Y0', 'Position_Y'; % 车辆Y坐标 'Yaw', 'Heading_Angle' % 航向角 }; % MPC输出 → Carsim输入 outputs = { 'Steer_CMD', 'IMP_STEER_SW'; % 转向指令 'Speed_CMD', 'IMP_SPEED' % 速度指令 };

3. MPC控制器代码精解

3.1 核心算法结构

MPC控制器采用S-Function实现，主要包含四个函数：

function [sys,x0,str,ts] = MpcController(t,x,u,flag) switch flag case 0 % 初始化 [sys,x0,str,ts] = mdlInitializeSizes; case 2 % 状态更新 sys = mdlUpdates(t,x,u); case 3 % 输出计算 sys = mdlOutputs(t,x,u); otherwise sys = []; end end

3.2 预测模型实现

车辆运动学模型离散化处理：

% 状态矩阵更新 A = [1 0 -v*sin(θ)*dt; 0 1 v*cos(θ)*dt; 0 0 1 ]; B = [cos(θ)*dt 0; sin(θ)*dt 0; tan(δ)*dt/L v*dt/(L*cos(δ)^2)];

3.3 优化问题构建

二次规划标准形式转换：

H = [THETA'*Q*THETA + R, zeros(Nu*Nc,1); zeros(1,Nu*Nc), Row]; % 松弛因子 f = [2*error'*Q*THETA, 0]; % 线性项

4. 典型调试问题解决方案

4.1 轨迹发散问题

现象：车辆轨迹逐渐偏离参考路径
排查步骤：

1. 检查预测时域Np和控制时域Nc的比值（建议2:1）
2. 验证权重矩阵Q和R的取值：

   Q = diag([100, 100, 50]); % 位置权重 > 航向角权重 R = diag([1, 10]); % 转向权重 > 速度权重

3. 确认车辆参数与实际一致（特别是轴距L）

4.2 实时性不足

现象：仿真步长超过0.1秒
优化方案：

1. 减少预测步长（可尝试从60调整到30）
2. 使用预编译的QP求解器：

   options = optimoptions('quadprog','Algorithm','active-set');

3. 启用MATLAB加速模式：

   coder.extrinsic('optimoptions'); % 声明外部函数

4.3 Carsim-Simulink通信异常

错误提示："Unable to resolve Simulink signal"
解决方法：

1. 在Carsim的Interface选项卡中重新生成映射文件
2. 检查采样时间同步：

   ts = [0.05 0]; % 必须与Carsim仿真步长一致

3. 清理临时文件后重启软件

5. 进阶调参技巧

5.1 不同场景参数优化

根据路径曲率动态调整参数：

5.2 多目标协调策略

在mdlOutputs函数中添加约束优先级处理：

if norm([X_err, Y_err]) > 2 % 当位置误差较大时 R(2,2) = R(2,2)*0.5; % 放松转向约束 end

6. 可视化与性能分析

6.1 轨迹对比绘图

改进版的绘图代码包含误差分析：

figure; subplot(2,1,1); plot(ref_x, ref_y, 'b--', actual_x, actual_y, 'r'); legend('参考轨迹','实际轨迹'); subplot(2,1,2); error = sqrt((ref_x-actual_x).^2 + (ref_y-actual_y).^2); plot(time, error); xlabel('时间(s)'); ylabel('跟踪误差(m)');

6.2 控制量监测

关键控制量的时域特性分析：

figure; stairs(time, steer_angle); hold on; stairs(time(1:end-1), diff(steer_angle)/dt); legend('转向角','转向速率');

7. 工程实践建议

在实际项目中，有三个方面需要特别注意：

1. 模型失配处理：当车辆负载变化时，建议增加自适应模块在线更新质量参数
2. 执行器延迟补偿：在MPC输出后添加一阶滞后环节模拟转向系统延迟
3. 紧急制动融合：当横向误差超过阈值时触发安全接管机制

记得保存完整的参数配置脚本，例如mpc_config.m，这对团队协作和项目交接至关重要。最近在调试一个复杂S弯道场景时，正是通过系统化的参数日志快速定位到了采样时间不匹配的问题。