MPC车辆路径跟踪：Simulink建模、参数整定与代码生成实践

简介：面向从事自动驾驶、智能车辆及先进控制研究的工程师和研究生，这份资源提供基于模型预测控制（MPC）的车辆路径跟踪MATLAB/Simulink完整实现。内容围绕MPC控制器设计流程，覆盖车辆动力学建模、状态空间转换、预测模型构建、成本函数与约束设计、在线优化求解及控制器实时更新等关键环节，并包含公路巡航与八字形轨迹两个典型仿真场景，便于对照学习。压缩包共25个文件、大小1.29MB，主要包含2个Simulink模型（.slx）、2个MATLAB脚本（.m）、1个Excel数据表格（.xlsx）、1个.mat数据文件及多张结果图像（png/jpg）。图像展示了轨迹跟踪误差、预测时域影响、参数变化等仿真结果，可作为调试与参数整定的直观参考。资源已有1176人学习下载，适合希望结合代码与仿真模型理解MPC原理并搭建车辆控制验证平台的读者，通过运行文件可观察预测时域、约束条件及控制器参数对跟踪精度的影响，从理论走向工程应用。

1. MPC 车辆路径跟踪：先分清“公式能跑”和“车能跑”

车辆路径跟踪这件事，PID 和 Stanley 方法在低速、小曲率场景下已经够用，但一旦进入高速变道、湿滑路面或大曲率弯道，纯反馈控制器就会暴露一个本质问题：它看不到未来。MPC（模型预测控制）之所以成为自动驾驶横向控制的主流方案，不是因为它“用了优化”，而是它在每个控制周期内显式地预测未来一段时域内的车辆状态，并把转向指令作为优化变量求解，天然能把“跟踪误差最小”和“执行器约束、稳定性约束”放进同一个优化问题里。换句话说，MPC 解决的不是“现在偏了多少”，而是“接下来 1 秒内车辆会怎么偏、怎么把它拉回来”。

这篇文章只讨论一条落地路径：在 MATLAB/Simulink 环境里，用车辆运动学或动力学模型做预测模型，使用模型预测控制进行车辆路径跟踪，覆盖建模、MPC 控制器搭建、参数整定和代码生成验证。MATLAB 与 Simulink 是 MPC 原型验证最常用的组合，前者负责离线调参与求解器对比，后者负责搭建闭环仿真环境、信号观测和自动代码生成。文中所有代码和模型结构都基于 MATLAB R2023b 及之后版本编写，如果你用的是 R2020a 之前的版本，部分求解器接口会有差异。适合人群：正在做智能车竞赛横向控制、ADAS 功能开发或刚接触预测控制的工程师和研究生。下文默认你已有基础 Simulink 操作能力，不会从“如何打开 Simulink”开始讲。

2. 预测模型：MPC 控制器的“内模”决定了控制上限

2.1 从车辆运动学模型到状态空间表达

MPC 的预测能力完全取决于内模精度。内模太粗糙，预测轨迹偏离实际，控制器在高速时会出现持续振荡；内模太精细，比如引入轮胎魔术公式和非线性轮胎侧偏特性，优化问题变成非线性规划（NLP），求解时间在普通工控机上可能超过 50ms，实时性崩溃。工程上最常见的折衷方案是：低速（< 36 km/h）用运动学自行车模型，高速用线性时变动力学模型。

运动学自行车模型的连续时间状态方程如下（前轮驱动、后轮轴中心为参考点）：

dx/dt = v * cos(psi + beta) dy/dt = v * sin(psi + beta) dpsi/dt = v * cos(beta) * tan(delta_f) / L beta = atan( tan(delta_f) * l_r / L )

写成分段仿射状态空间形式时，取状态 x = [y, psi, vy, r]，其中 y 为横向位置误差，psi 为航向角误差，vy 为侧向速度，r 为横摆角速度；控制输入 u = delta_f（前轮转角），干扰输入 d = v（纵向车速）。对这个系统在参考轨迹点做 Taylor 展开并离散化，得到标准线性时变模型：

x(k+1) = A(k) * x(k) + B(k) * u(k) + B_d(k) * d(k)

2.1.1 Simulink 中建立预测模型的推荐方式

常见做法是在 MATLAB 脚本里定义车辆参数，生成状态空间矩阵，然后通过ss对象或c2d离散化后传给 MPC 控制器。车辆参数建议统一放在一个结构体veh中，便于批量修改：

veh.m = 1575; % 车辆质量 [kg] veh.Iz = 2875; % 绕 z 轴转动惯量 [kg*m^2] veh.lf = 1.2; % 质心到前轴距离 [m] veh.lr = 1.6; % 质心到后轴距离 [m] veh.Cf = 19000; % 前轮侧偏刚度 [N/rad] veh.Cr = 33000; % 后轮侧偏刚度 [N/rad] veh.L = veh.lf + veh.lr; % 横向动力学状态方程（线性时变，在 vx 工作点线性化） % 状态: x = [ey, epsi, vy, r]' % 输入: u = [delta_f]' A_cont = [0, vx, 1, 0; 0, 0, 0, 1; 0, 0, -(veh.Cf+veh.Cr)/(veh.m*vx), (veh.lr*veh.Cr-veh.lf*veh.Cf)/(veh.m*vx)-vx; 0, 0, (veh.lr*veh.Cr-veh.lf*veh.Cf)/(veh.Iz*vx), -(veh.lf^2*veh.Cf+veh.lr^2*veh.Cr)/(veh.Iz*vx)]; B_cont = [0; 0; veh.Cf/veh.m; veh.lf*veh.Cf/veh.Iz]; % 离散化，采样时间 Ts = 20ms Ts = 0.02; sys_d = c2d(ss(A_cont, B_cont, eye(4), zeros(4,1)), Ts, 'zoh'); A = sys_d.A; B = sys_d.B;

这段代码的关键点在于：A_cont 的第三、四行中所有除以 vx 的项，在低速（vx < 2 m/s）时会变得非常大，导致离散化后系统矩阵条件数恶化。所以实际使用时要在前面加一个判断，vx 低于 2 m/s 时直接切换到运动学模型矩阵，避免数值问题。状态矩阵定义顺序要跟后续 MPC 代价函数的加权矩阵 Q 的维度保持一致，否则在 Simulink 里拼 Bus 信号时最容易出现“索引对不上”的错误。如果使用 MATLAB 的 Model Predictive Control Toolbox 中的mpc对象，需要把上述状态空间模型直接传给setmpc的Model.Plant字段。

2.1.2 代价函数、约束与 QP 问题的标准表达

MPC 在每个控制周期求解的最优化问题可以写作：

min sum_{i=1}^{Np} ( x(k+i) - x_ref(k+i) )' * Q * ( x(k+i) - x_ref(k+i) ) + sum_{i=0}^{Nc-1} u(k+i)' * R * u(k+i) + sum_{i=0}^{Nc-1} ( u(k+i) - u(k+i-1) )' * S * ( u(k+i) - u(k+i-1) ) s.t. x(k+i+1) = A * x(k+i) + B * u(k+i) delta_f_min <= delta_f(k+i) <= delta_f_max delta_dot_min <= delta_dot(k+i) <= delta_dot_max vx_min <= vx(k+i) <= vx_max

其中 Np 是预测时域，Nc 是控制时域，Q 是状态误差加权矩阵，R 是控制量加权矩阵，S 是控制增量惩罚矩阵。这个问题的关键区别在于：它把约束写进了优化问题，而不是通过饱和限幅模块来事后截断。饱和限幅的问题是，一旦执行器被截断在上下限，MPC 内部的预测模型仍然按照没截断的控制量预测，会产生模型失配并引发极限环振荡。所以凡是涉及转向执行器最大转角和最大转角变化率的约束，都必须显式写进 QP 问题的约束矩阵中。

在 Simulink 里直接使用 Model Predictive Control Toolbox 时，这些约束通过setconstraint对象配置，或者直接在 MPC Designer 里填写上下限。手写 QP 求解时，需要把所有不等式约束拼接成矩阵形式C * z <= b，其中 z 是堆叠的输入序列。约束矩阵的行数会被求解器用来判断问题规模，如果使用 FORCES Pro 求解器，约束过多会显著增加嵌入式代码的内存占用。常见工程做法是把轮胎侧偏角约束通过线性近似转成状态约束，而不是直接约束侧偏角本身，否则约束矩阵会变成非线性。

3. Simulink 实现：从 MPC 控制器模块到闭环仿真

3.1 使用 MPC Controller 模块搭建路径跟踪闭环

在 Simulink 中搭建 MPC 路径跟踪闭环的标准结构由四个部分组成：参考路径生成器（通常是查表或 MATLAB Function 块）、MPC 控制器、车辆动力学模型、信号观测与数据记录。MPC Controller 模块存在于 Model Predictive Control Toolbox 中，它接收五个输入信号：实际测量状态、参考状态、参考输入（前馈控制量）、干扰测量值（纵向车速）、以及用于约束调度的外部信号。

搭建具体步骤如下：

% 在 MATLAB 中创建 MPC 对象并设计控制器 mpcobj = mpc(sys_d, Ts); % sys_d 是第2章离散化后的模型 mpcobj.PredictionHorizon = 20; mpcobj.ControlHorizon = 5; % 加权矩阵 mpcobj.Weights.OutputVariables = [1.0, 0.8, 0.5, 0.2]; % 对应 [ey, epsi, vy, r] mpcobj.Weights.ManipulatedVariables = [0.1]; mpcobj.Weights.ManipulatedVariablesRate = [0.5]; % 约束 mpcobj.ManipulatedVariables.Min = -0.5; % 前轮转角下限 [rad] mpcobj.ManipulatedVariables.Max = 0.5; % 前轮转角上限 [rad] mpcobj.ManipulatedVariables.RateMin = -0.8; % 转向角速度下限 [rad/s] mpcobj.ManipulatedVariables.RateMax = 0.8; % 转向角速度上限 [rad/s] % 输出约束（横向位置误差） mpcobj.OutputVariables(1).Min = -2.0; mpcobj.OutputVariables(1).Max = 2.0;

创建完 mpcobj 后，Simulink 模型中的 MPC Controller 模块会直接关联到该对象。关键修改点：PredictionHorizon 与控制时域的比值大约在 3:1 到 5:1 之间。Np 太长会导致计算量显著上升 —— QP 问题的决策变量个数是 Nc 乘以输入维数，而约束个数与 Np 近似线性相关 —— 但 Np 太短时预测看不到弯道曲率变化，会导致转向滞后。对于采样时间 20ms、车速 72km/h 的工况，Np=20 意味着预测范围只有 8 米的道路，这对限速 30km/h 的园区道路够用，但高速场景要按车速动态调整。

模型里参考输入端口一般接一个常值或由参考路径计算出的前馈转角：

delta_ff = atan(L * kappa_ref)

其中 kappa_ref 是参考路径的曲率。前馈项的作用不是消除误差，而是让 MPC 的优化从一个接近最优解的点开始迭代，减少求解器首次迭代的初始残差。调试时可以先用拖拽模块手动连接端口，确认每个信号的数据类型是一维数组还是二维列向量，常见错误是参考路径为 1x4 行向量而 MPC 期望 4x1 列向量，会直接报维数不匹配。

3.1.1 手写 MPC 控制器时使用 MATLAB Function 块实现滚动优化

如果项目不使用 Model Predictive Control Toolbox，或者需要定制代价函数与约束形式，可以在 Simulink 中用 MATLAB Function 块封装一个简单的 MPC。代码结构如下：

function u_opt = my_mpc_controller(ym, yref, uref, vx_meas) % ym: 当前时刻测量输出 [ey, epsi, vy, r] % yref: 参考轨迹 [ey_ref, epsi_ref, vy_ref, r_ref] % uref: 前馈控制量 % vx_meas: 纵向车速测量值 Nx = 4; % 状态个数 Nu = 1; % 输入个数 Np = 20; % 预测时域 Nc = 5; % 控制时域 Ts = 0.02; % 采样时间 % 构建线性时变模型（调用外部函数 compute_A_B，返回离散状态矩阵） [A, B] = compute_A_B(vx_meas, Ts); % 堆叠预测方程，转换为标准 QP 形式 % 目标函数: min 0.5 * z' * H * z + f' * z % 约束: lb <= z <= ub % 状态序列: Y = Phi * x0 + Gamma * z % 使用 fmincon 求解（原型验证够用，实时性不够） options = optimoptions('fmincon', 'Algorithm', 'sqp', 'Display', 'off'); z0 = zeros(Nc*Nu, 1); costfun = @(z) costFunction(z, ym, yref, A, B, Np, Nc); u_opt = fmincon(costfun, z0, [], [], [], [], lb, ub, [], options); u_opt = u_opt(1); % 只取第一个控制量，滚动优化 end

这段代码用于原型验证没有问题，但如果目标是实时性要求较高的场景，不建议用 fmincon，因为它求导时使用有限差分，在高频控制下会产生数值噪声。在 Simulink 中仿真离散步长小于 1ms 时，每个周期调用 fmincon 会让仿真时间膨胀数十倍。更常见的做法是使用quadprog并预先展开预测方程，或者使用 FORCES Pro / HPIPM 生成 C 代码。

3.1.2 车辆动力学模型的选择：VDB 模块 vs C MEX S-Function

在 Simulink 中搭建车辆模型通常有三种选择：Vehicle Dynamics Blockset 中现成的车辆动力学参考应用模块，Simscape 搭建的更高精度多体模型，以及手写 C MEX S-Function。作为工程师，我建议分阶段使用：先用手写 S-Function 做控制器离线验证，再切换到 VDB 模块确认控制器的鲁棒性。

手写 C MEX S-Function 的一个好处是能完全控制模型自由度，并且可以直接生成代码烧录到硬件在环测试平台。下面是 S-Function 中最核心的mdlOutputs片段，用来说明在 Simulink 里如何实现每个控制周期更新系统状态：

static void mdlOutputs(SimStruct *S, int_T tid) { real_T *y = ssGetOutputPortRealSignal(S, 0); real_T *x = ssGetRealIWork(S, 0); // 状态变量 real_T *u = ssGetInputPortRealSignal(S, 0); // 状态方程: dx = f(x, u)，使用四阶龙格库塔离散更新 real_T k1[4], k2[4], k3[4], k4[4], xtmp[4]; real_T dt = 0.02; // 根据整车质量、轴距及轮胎侧偏刚度计算状态导数 // f1 = vx * cos(psi + beta) 等，具体与第2章模型一致 // 这里省略完整龙格库塔系数计算，实际工程中这部分必须逐项核对 for (i = 0; i < 4; i++) x[i] += (k1[i] + 2*k2[i] + 2*k3[i] + k4[i]) / 6.0; y[0] = x[0]; // 输出横向位置误差 }

实际操作中，S-Function 的编写工作量大且容易出错，最大的坑在于 Simulink 代数环检测。如果车辆动力学模型中包含没有延迟的反馈回路（比如把 y 直接用于计算 delta_f，而 delta_f 又用于计算 y 的导数），Simulink 会报代数环错误。解决方式是添加 Memory 块或 Unit Delay 块打破代数环，但这会引入一个采样周期的滞后。对于 MPC 控制器来说，这个滞后会在高频段降低相位裕度，所以更推荐把 MPC 控制器本身设计成带一个周期的计算延迟补偿，即在预测模型的输入端补一拍延迟状态。

4. 参数整定与常见坑：预测时域、权重矩阵、求解器选择

4.1 预测时域 Np 与控制时域 Nc 的整定步骤

关于 Np 和 Nc，工程上有一组可以快速上手的经验值：当采样时间 Ts=20ms、车速 vx=10m/s 时，Np=25（预测 0.5 秒），Nc=5。速度越低 Np 可以适当减小，速度越高 Np 必须增大，否则预测范围小于刹车距离，控制器会表现出“近视”。但 Np 增大到一定程度会出现收益递减：当 Np 已经覆盖了参考路径中最极端的曲率变化时，再增大 Np 只会增加求解时间，跟踪精度变化不大。

调试整定可以分三步走。

第一步，把 R 和 S 矩阵设得很大，Q 设得很小，观察系统是否稳定。如果连开环稳定的基线工况都不稳定，说明离散化模型有问题，不要继续调权重。这一步通常在 MATLAB 脚本中做闭环仿真，脚本循环调用mpcmove或手写 QP 求解器。

第二步，从 Q 的横向误差项开始调，逐步增大 Q(1,1)，观察横向偏差峰值的变化曲线。记录下偏差变量开始出现高频抖动的临界值，然后将 Q(1,1) 设为临界值的 50% 作为初始值。对于路径跟踪场景，状态量 ey 和 epsi 的权重之比一般取 1:0.5 到 1:2 之间，比值过大则航向角误差收敛慢，过小则横向误差收敛慢。

第三步，加入 S 矩阵惩罚控制增量，从较小值逐步增大，直到转向指令中的高频分量消失或峰值转向速率低于执行器限制。

% 用于整定的批量仿真脚本（离线循环） clear; clc; Np_list = [15, 20, 25, 30]; Q1_list = [0.5, 1.0, 2.0, 4.0]; results = table(); for i = 1:length(Np_list) for j = 1:length(Q1_list) mpcobj.PredictionHorizon = Np_list(i); mpcobj.Weights.OutputVariables(1) = Q1_list(j); % 运行 Simulink 仿真（通过 sim 命令） simOut = sim('path_tracking_mpc.slx', 'StopTime', '10'); ey = simOut.ey_signals.Data; max_ey = max(abs(ey)); results = [results; {Np_list(i), Q1_list(j), max_ey}]; end end

整定过程中可以用 Simulink 的 “Signal Logging” 功能记录 ey、delta_f 和 delta_f_dot 信号，通过仿真数据检查器对比不同参数组合下的阶跃响应。当仿真结果表现良好但实际车辆测试出现抖动时，需要检查执行器延迟模型是否在预测模型中体现，延迟项在整车控制器里通常表现为 CAN 报文周期（10ms 或 20ms）加上执行器响应时间。

4.2 权重矩阵的缩放技巧与约束归一化

权重矩阵的数值范围直接影响 QP 求解器的数值稳定性。常见错误是把 Q 中不同量纲的项放在同一个数量级，比如 ey 的量级是 0.1m，而 r 的量级是 0.5 rad/s。若 Q(1,1)=Q(4,4)=1，则优化时会过度惩罚横摆角速度，导致转向角指令出现毛刺。正确做法是对状态做归一化缩放：

具体做法是在 MPC 模块内部增加一个归一化矩阵 T，使优化变量变成 z_norm = T * z。在 MATLAB/Simulink 中，可以直接在状态测量进入 MPC Controller 模块前加一个 Gain 模块缩放信号，或者在 MPC Designer 的 “Scale” 属性中填写缩放因子。约束归一化后，QP 求解器（如 qpOASES）的主动集识别速度会显著提升，因为所有约束的量级都在 0.1 到 10 之间，KKT 条件的数值判定阈值在 1e-8 时不会误判约束激活状态。

另一个容易被忽略的参数是约束松弛变量。输出约束（如横向位置误差不能超过车道边界）在硬约束下可能导致 QP 无解。此时要在代价函数中加入松弛变量 epsilon，并对 epsilon 乘以一个大权重（比 Q 大两个数量级），公式变为输出约束形式。在 Model Predictive Control Toolbox 中只需勾选 “Soft constraints” 并设置松弛变量权重，而手写 QP 时需要在决策变量中增加一个维度。

% 软约束处理示例（手写 QP 中） % 原约束: ey_min <= ey <= ey_max % 新约束: ey_min - epsilon <= ey <= ey_max + epsilon % 代价函数增加项: 10000 * epsilon^2 % 决策变量: z = [delta_f(1:Nc), epsilon]

软约束虽然扩大了可行域，但可能会导致求解器总是选择放宽约束而不是努力跟踪，所以松弛变量权重必须足够大，同时又不能大到使约束矩阵的条件数劣化。

4.3 MPC 求解器选型与 Simulink 代码生成的关系

MPC 求解器的选择直接决定 Simulink 模型能否顺利生成嵌入式代码并部署到目标硬件。下图是四种主流求解器在车辆路径跟踪场景中的适用性对比，基于个人项目经验归纳，不构成工具评测结论：

从 Simulink 模型生成代码的典型步骤是：在 Simulink 中配置 Embedded Coder，选择 “ERT” 目标，将 MPC 控制器封装为 S-Function 或子系统，然后将模型导出为 C 代码。这里有一个我踩过多次的坑：如果 MPC 控制器模块内部存放了唯一句柄指向 MATLAB 的mpc对象，那么生成代码时会出现类型不匹配。解决办法是在生成代码前将 MPC 控制器转换为常量结构体，使用mpccodegen函数对控制器进行代码生成预处理。

% MPC 对象转成嵌入式结构体 mpcobj_codegen = mpccodegen(mpcobj, 'my_mpc_controller'); % 生成的 C 代码中会包含 my_mpc_controller.h 和 .c % 在 Simulink 中把 MPC Controller 模块替换为 C Caller 模块，调用生成的函数

替代做法是在 Simulink 的 “MATLAB Function” 块中调用mpcmove函数，然后通过 MATLAB Coder 将该函数转成 C 代码。这种方式保留的 MATLAB 语义更接近设计时仿真的结果，但生成代码的体积较大，在内存受限的 MCU 上可能超出 flash 空间。实际量产项目中，无论选哪条路线，都必须做一次 S-Function 的 in-the-loop 测试和处理器在环测试。

5. 双移线工况仿真与一个实用验证技巧

双移线（Double Lane Change）是评价路径跟踪控制器最经典的标准工况，它同时包含了直线行驶、紧急变道和回正三个子工况，对控制器的动态响应和稳态精度要求高于匀速圆周工况。在 Simulink 中搭建双移线工况的参考路径通常用内联函数或基于参数的 MATLAB Function 块生成：

function ref_xy = double_lane_change(~) % 双移线工况参考路径（单位：m） % 路径分为三段：前方 30m 直线、30m 至 60m 变道、60m 之后保持车道 x = (0:0.1:120)'; y = zeros(length(x), 1); % 变道曲线段使用 S 型函数，最大横向偏移 3.5m idx = find(x > 30 & x <= 60); y(idx) = 3.5 * (1 - cos(pi * (x(idx) - 30) / 30)) / 2; idx = find(x > 60 & x <= 90); y(idx) = 3.5; ref_xy = [x, y]; end

将这组参考点通过 lookup table 输入给 MPC 控制器的参考状态端口。注意 MPC 内部预测使用状态误差形式，所以必须把参考点转换为 ey_ref=0、epsi_ref=0、vy_ref=0、r_ref 的参考轨迹，具体做法是在 MATLAB Function 块里根据参考路径曲率计算 r_ref。

仿真完成后建议使用过程稳定性验证技巧：如果仿真步长为 1ms，而 MPC 采样周期为 20ms，那么需要开启 Simulink 的 “Block reduction” 并确保 MPC 模块类型为离散（Sample time 属性为 0.02）。判断闭环响应是否正常的一个实用指标是转向指令的差分信号 delta_f_dot 是否持续超过执行器限幅。若持续超过，说明 S 权重设置过大或预测时域过短，系统是稳定但有饱和风险，长期运行会导致执行器磨损或转向电机过热。

最后一个实用技巧能提高整个仿真项目的可复现性：把 MPC 控制器的所有参数（权重矩阵、约束、采样时间、时域）统一封装在 MATLAB 脚本生成的mpc_params.mat文件中，并在 Simulink 的 PreLoadFcn 回调中自动加载。当需要比较不同参数组的仿真结果时，只需要切换 mat 文件而不需要修改模型。对生成代码后还要做处理器在环测试的项目，这个技巧能保证离线仿真与 PIL 测试使用的是同一套参数，避免模型和代码之间因人工修改而产生偏差。

本文还有配套的精品资源，点击获取