无人驾驶车辆模型预测控制：MATLAB仿真代码实战与二次开发

简介：这套由北京理工大学提供的无人驾驶车辆模型预测控制第二版程序代码，面向需要深入理解模型预测控制理论并落地实践的自动驾驶研究者、研究生与工程师，覆盖路径规划、速度控制、避障等典型驾驶场景。压缩包共349个文件，约1.52MB，以MATLAB脚本文件（.m）、数据文件（.mat）、C语言源码（.h/.c）以及Simulink仿真模型（.mdl）为主，并包含批量执行脚本与动态链接库，便于在仿真环境中直接运行和修改。目前已有784人学习下载。资源内含车辆横向与纵向动力学模型、考虑路面和空气阻力等因素的预测模型、优化问题求解、控制律生成以及实时更新等核心模块，并提供实验验证数据与结果分析。通过研读代码和仿真实验，学习者可以完整掌握模型预测控制在无人驾驶车辆中的实施细节，提升控制算法设计与编程解决能力。

1. 无人驾驶车辆模型预测控制：这套配套代码解决了什么问题

当你发现手里那套 PID 在低速直线工况还像模像样，一到连续弯道就开始画龙时，缺的不是调参玄学，而是一个能往前多看几步的控制器架构。模型预测控制的核心就是每步都对未来一段时域做预测、带约束地求解最优控制量，而这套《无人驾驶车辆模型预测控制（第二版）》的程序代码，把教材里的那一堆公式变成了能直接跑的仿真闭环。它覆盖了从车辆运动学/动力学建模、预测模型线性化离散化、目标函数与约束构建，到滚动时域求解和结果绘制的完整链路。适合正在做车辆横纵向控制或路径跟踪的研究生、工程师——你可以拿它跑通第一个 MPC 仿真，也可以在此基础上改模型、改约束，做成自己的控制器原型。

2. 技术栈与工程结构：MATLAB 环境下的 MPC 系统是怎么组织的

2.1 技术选型：为什么这套代码用 MATLAB + C MEX 而不是纯 Python

写 MPC 不挑语言，但做车辆控制的人大多默认在 MATLAB 里把事做完。这套配套代码选 MATLAB 不是没有道理。第一，车辆的 Simulink 仿真生态非常成熟，从简单的运动学自行车模型到带轮胎魔术公式的动力学模型都有现成库，省去自己对着实车标定一大半时间的痛苦；第二，MATLAB 自带的 quadprog、fmincon 等求解器开箱即用，做数值验证的速度比自己手写内点法快得多；第三，论文级图表的绘制和算法对比在 MATLAB 里是顺手的事，这对需要做实验分析的人来说体验很友好。

我拿到一套 MATLAB 控制类代码的第一步从来不是双击运行，而是先确认环境。这套代码里的求解部分在常见实现里会用到 C 语言写成 MEX 文件来提速，或者直接调用优化工具箱。如果是 MEX 方案，C 编译器配置就成了启动翻车的高发区。另一个容易忽略的是版本问题，R2016a 和 R2019b 在某些优化接口上的行为有明显差异，建议优先使用文档标注的推荐版本，而不是自己机器上顺手的那一版。

2.2 目录结构与模块职责

这类教材配套代码通常按章组织，一个目录对应教材里的一个实验或一个算法变体。目录内部一般分四类文件：初始化脚本、模型函数、控制器函数、主仿真脚本。初始化脚本负责把工作区变量、路径、参数一次性配好；模型函数是实现车辆微分方程的位置；控制器函数是 MPC 算法的主战场；主仿真脚本把前面串起来，跑循环画图。

我用一个表格把职责分层列出来，方便对照手边的代码找文件。不同版本的教材代码可能把求解器单独拆成子函数和 C 文件，但职责边界大致都落在这四类里。

如果手边代码的目录命名不完全一致，不用慌，按职责去对应即可。我一般会先打开主仿真脚本，看它调用了哪几个函数，再顺着往下点几层，整个结构十分钟就能梳理清楚。

2.3 一次完整的仿真流程从哪里进、到哪里出

流程的入口永远是主仿真脚本，出口是结果图和日志数据。整个仿真循环的骨架可以用一段标准 MATLAB 代码概括，这也是绝大多数教材代码的通用组织方式。

% 主仿真循环（常见教材代码骨架） dt = 0.05; % 控制周期 50ms T = 20; % 仿真时长 20s N = round(T / dt); % 总步数 x = x0; % 初始状态 [X; Y; phi; v] for k = 1 : N % 截取从当前时刻开始的预测窗口内的参考轨迹 refWindow = traj(k : min(k + Np, size(traj, 1)), :); % 控制器内部：线性化 + 离散化 + 构建QP + 求解，返回第一步控制量 u = mpcController(x, refWindow); % 车辆模型积分一步，得到下一时刻状态 x = vehicleModel(x, u, dt); % 记录每一步的状态与控制量用于后处理 logState(k, :) = x'; logU(k, :) = u'; end plotResults(logState, logU, traj);

这段代码虽然不到二十行，却把整套代码的调用关系写透了。mpcController 是下一章的重点，它内部要做线性化、离散化、目标函数构建、约束构建和 QP 求解五件事；vehicleModel 是车辆微分方程的离散近似，它的精度直接决定仿真结果是贴近真实还是空洞失真。参数说明：dt 选 0.01 到 0.1 秒之间，越小越接近连续系统但仿真越慢；Np 是预测时域步数，工程上取 10 到 30 步比较常见，取太小 MPC 的远见就没了，取太大 QP 的求解压力成倍增加。

3. 核心模块拆解：从车辆模型到滚动优化求解的完整链路

3.1 车辆运动学与动力学模型：状态量、输入量与参数标定

MPC 的第一个前提是有一个能预测的模型。教材第一版的内容重心在运动学模型，第二版明显加重了动力学模型的分量，代码里也保留了两种模型供切换。运动学模型用的是自行车模型，状态量是车辆位姿和速度四个维度，输入是前轮转角和加速度两个维度，方程是标准的运动学关系。动力学模型在运动学基础上加入轮胎侧偏特性，状态量增加横摆角速度、侧偏角等，需要标定侧偏刚度、转动惯量、质心到前后轴的距离等参数。

很多读者在这套代码上花的时间大头不是在 MPC 求解，而是在动力学模型上。原因很直接——运动学模型的参数只有轴距 L，一个数；动力学模型的参数一摆就是七八个，任何一个标定得不合理，仿真结果都会显得飘。我拿到代码后的第一个习惯是看状态量和输入量的维度，判断当前激活的是哪套模型，再决定改哪些参数。这个判断用不了半分钟，却能提前避开后面维度不匹配的坑。

% 运动学自行车模型（连续状态方程） function dx = kinematicModel(x, u, L) % 状态量 x = [X; Y; phi; v] % 控制量 u = [delta; a]，delta 为前轮转角，a 为纵向加速度 phi = x(3); v = x(4); dx = zeros(4, 1); dx(1) = v * cos(phi); % X方向速度 dx(2) = v * sin(phi); % Y方向速度 dx(3) = v * tan(u(1)) / L; % 航向角速度 dx(4) = u(2); % 纵向加速度 end

这个模型函数是全代码里最直白的一块。前两行是位置的速度分解，第三行是自行车模型的航向角速度公式，第四行是纵向速度的积分关系。参数 L 是轴距，默认值在 2.6 到 3.0 米之间，改小会让车辆转弯更灵敏但容易过冲。注意 tan 函数在 delta 接近 90 度时发散，所以控制量限幅在 MPC 约束里是必须存在的，不只是为了舒适性，更是数值稳定的需要。

3.2 预测模型的线性化与离散化处理

车辆模型是非线性的，而二次规划求解器处理的是线性不等式约束和二次目标函数，所以 MPC 每个控制周期要做一件事：把非线性模型在参考轨迹点附近线性化，得到一个线性状态空间方程，再离散化成代码可用的递推形式。这是教材第二版里反复强调的步骤，也是很多人读理论时没注意、写代码时才发现绕不开的一步。

线性化本质上是求雅可比矩阵。手推公式容易错，常见做法是用 MATLAB 的符号工具箱或者直接写出近似的雅可比解析式。离散化则几乎都用前向欧拉法，因为控制周期短的时候精度够用、实现最简单。整个过程的代码形式一般是这两个矩阵函数加一个离散化步骤：

% 每个控制周期重新线性化并离散化 function [Ad, Bd] = linearizeAndDiscrete(x_r, u_r, dt) A = computeJacobianA(x_r, u_r); % 状态雅可比矩阵 B = computeJacobianB(x_r, u_r); % 输入雅可比矩阵 Ad = eye(size(A)) + A * dt; % 前向欧拉离散 Bd = B * dt; % 输入矩阵离散 end

这里最关键的一点是"每个控制周期重新计算"，而不是"算一次用到结束"。如果 A、B 矩阵一直用工作点的值，低速场景下问题不大，一旦做高速换道或急转弯，模型失配会让预测轨迹和真实轨迹越拉越远，表现为跟踪误差发散的诡异现象——明明单步仿真一切正常，闭环就翻车。这正是 MPC 实现里最常见的隐性错误。

3.3 目标函数与约束条件：权重矩阵和控制量限幅怎么设计

MPC 把跟踪问题写成带约束的二次规划，目标函数由两块组成：一是预测时域内状态偏差的平方和，二是控制量增量或控制量本身的平方和。前者由状态权重矩阵 Q 加权，后者由控制权重矩阵 R 加权。Q 和 R 的比值决定了控制器的性格——Q 相对大，控制器就更激进地拉回误差；R 相对大，控制器就更省力地慢慢纠偏。

代码里这个 QP 问题的构建有固定套路：先把预测模型展开成"当前状态加输入序列"的显式表达，得到预测矩阵，然后组装成二次规划的标准形式。下面这段代码省略了 Phi 矩阵的递推细节，但保留了 QP 构建的主干：

% 构建QP问题 function u_opt = solveMpcQp(x_k, x_ref, Q, R, u_min, u_max, Np) % Phi为预测矩阵，Psi为状态递推项 Phi = buildPredictMatrix(Ad, Bd, Np); % (Np*nx) x (Np*nu) Psi = buildStatePropagation(Ad, Np); % (Np*nx) x nx H = 2 * (Phi' * Q * Phi + R_tilde); % 海森矩阵 f = 2 * (Psi * x_k - x_ref)' * Q * Phi; % 线性项 % 控制量限幅 Ain = [eye(nu * Np); -eye(nu * Np)]; bin = [repmat(u_max, Np, 1); -repmat(u_min, Np, 1)]; u_seq = quadprog(H, f, Ain, bin, [], []); u_opt = u_seq(1 : nu); % 只取序列的第一步 end

逻辑说明：buildPredictMatrix 把预测时域内每一步的递推关系显式写成矩阵乘法，这是从迭代递推到线性代数表达的关键步骤，也是理解 MPC 代码的核心难点。quadprog 求解得到的是整个预测时域上的控制序列，但代码只取第一段作为实际控制量，下个周期重新求解——这就是滚动时域控制的落地形态。参数说明：Q 至少是半正定对角矩阵，位置误差权重通常比速度误差权重大一个数量级；如果结果发散，先看约束是否给得太宽和 R 是否太小，而不是一味加大 Q，否则系统容易走向极限环状态。

3.4 滚动时域的实现：求解器调用与状态更新

滚动时域这个名词听上去高级，代码里就一个循环外加"只取第一步"的动作。每个控制周期，控制器接收当前状态和参考窗口，重新线性化、重组 QP、重新求解，然后把得到的控制序列第一步交给车辆模型去积分。其他控制量被丢弃，下个周期全部重来。

这种设计有个容易被忽视的好处：控制器是一个无内部记忆的纯函数。只要输入状态和参考轨迹相同，输出就确定。这意味着调试时你可以把任何一步的输入存下来，反复回放求解过程，而不必从仿真开头重跑整条链。很多二次开发会破坏这种纯函数特性，比如加了需要记忆上一时刻控制量的控制增量惩罚，这时就需要你自己维护控制器内部状态，这是改代码时要有意识的一步。

4. 复现步骤：把示例仿真跑起来

4.1 环境准备：版本与工具箱检查

在跑任何东西之前，先做三件基建：确认 MATLAB 版本、确认优化工具箱、确认编译器。版本方面，建议用 R2019b 或更新的版本，旧版本在 quadprog 接口和 Simulink 模型格式上会有兼容问题；优化工具箱是必须的，因为 quadprog 属于这个工具箱；如果代码里带 MEX 文件，还需要在 MATLAB 里运行 mex -setup 选好 C 编译器，常见的问题是装了 MinGW 却和 MATLAB 版本不匹配。

接下来是把整个工程复制到一个无中文无空格的纯英文路径下。这一步经常被跳过，然后出现各种找不到函数或者路径包含非法字符的报错。建议放在像 C:\Projects\mpc_book 这样的位置。放好后用 MATLAB 打开这个目录作为当前工作目录，不要用双击 .m 文件的方式启动——双击会改变工作目录，函数调用链容易断。

4.2 第一个仿真实验：初始化脚本、闭环仿真与结果图

现在找到主仿真脚本，命名一般是 main.m、demo.m、run_simulation.m 之一。找到后先打开看注释，确认哪些是初始化脚本哪些是主脚本。我的习惯是先运行初始化脚本，确认工作区里出现了一堆参数变量，再运行主脚本。跳步直接跑主脚本的后果通常是报"未定义函数或变量"，因为路径没加进去。

运行过程中如果一切顺利，MATLAB 命令行会不断输出步数或状态量，结束后弹出若干张图。第一张主图是跟踪轨迹对比，参考轨迹和实际轨迹画在一起。看这张图关注三点：起始段是否收敛、中段是否贴合、弯道是否 overshoot。然后看误差曲线，正常情况是一个快速衰减到稳态小波动的形态。最后看控制量曲线，如果控制量频繁在上下限之间反弹，说明权重设计偏激进，后面调整时用得上。

4.3 切换场景与参考轨迹：改哪里才能看到效果变化

复现跑通之后，做一次换轨迹实验就能确认你真的看懂了代码。参考轨迹通常由一个独立的轨迹生成函数负责，可能是从 mat 文件加载，也可能是一段多项式计算出来的双移线或正弦曲线。把参考轨迹改成一段带 S 弯的曲线再跑一次，观察 MPC 的行为变化。

这里有个值得注意的点：参考轨迹的总时长必须大于仿真的预测窗口。如果参考轨迹只有 10 秒，而仿真是 20 秒，后半程 MPC 会发现无轨迹可预测，表现是控制量在末尾突然失控或归零。改轨迹时把时长一并加长，或者在轨迹生成函数里做循环拼接，这两种做法在代码注释里都有体现，照着来就行。

4.4 对照组实验：验证代码逻辑是否正确

到这一步如果时间允许，做两组对照实验来验证你对代码的理解。第一组改预测时域 Np：从默认值改成 10，观察跟踪误差变大、弯道提前量变小；改成 30，观察误差变小但求解时间变长。第二组改权重 Q 的位置项：放大 10 倍，观察跟踪变紧但控制量出现抖动；缩小 10 倍，观察车辆懒洋洋地过弯。这两组实验做完，MPC 的预测时域和权重这两个旋钮各自的用途，比读十页书都直观。

如果对照组实验完全符合预期，说明这套代码在你的环境里跑通了，且你的理解与代码行为一致。接下来就可以进入二次开发阶段。

5. 避坑指南：跑这套代码最常见的 5 个坑

5.1 现象 → 原因 → 解决

第一个坑是 MEX 文件无法编译。现象是运行到某个函数时报错"未找到已编译的 MEX 文件"，或者 mex 命令直接红字。原因基本是没有配置 C 编译器，或 MATLAB 版本不识别当前安装的编译器。解决办法是运行 mex -setup 手动切换编译器，如果是新版 MATLAB 配老版本 MinGW 会失败，换装配套的编译器版本即可。这个坑在 Windows 平台上出现率最高，我在这上面花的冤枉时间最多。

第二个坑是仿真中途发散，轨迹直接飞出图外。现象是前几十步正常，某一步开始状态量爆炸式增长。原因通常有两种：一是离散化步长太大，前向欧拉法在快速工况下数值失稳；二是约束给得太宽，控制量在限幅边缘时线性化的预测模型失真严重。解决方式是先调小 dt 到 0.01 量级确认是不是数值问题，不是的话再收窄转角限幅。

第三个坑是换道永远慢半拍。现象是直道跟得不错，换道时实际轨迹明显落后参考轨迹。原因是预测时域 Np 太小，控制器看不到弯道的提前量；或者一致性权重太低，控制器更重视姿态而非位置。解决办法是把 Np 从 10 调到 20 左右，优先增大 Q 的位置误差权重而不是增大 R。这个坑最容易被误判成控制参数没调好，很多人白白调了一下午权重，结果问题出在预测时域上。

第四个坑是用 Simulink 版本时仿真速度越来越慢。现象是刚开始跑得很快，几分钟后明显卡顿。原因是日志记录变量被逐步扩容，MATLAB 在循环里不断 resize 数组，产生大量内存拷贝。解决方法是提前用 zeros 预分配日志矩阵，或者改用 Simulink 的 Data Store 固定缓冲区。这类性能问题在脚本版里不易出现，在 Simulink 版里几乎是必踩。

第五个坑是动力学模型状态维度对不上。现象是切换动力学模型后报错矩阵维度不一致。原因是运动学状态是 4 维，动力学状态是 6 维或 7 维，切换模型时初始化脚本、控制器状态变量、权重矩阵的维度没有同步改。解决办法是先跑通运动学版，再按注释逐行同步维度扩展，权重矩阵用 diag 方式构造而不是硬编码。

5.2 快速排查技巧

遇到诡异问题先别急着改参数，加日志永远是最快的定位手段。我一般会在 mpcController 的入口和出口各加一行 disp，打印当前步数、状态范数和控制量范数。如果状态范数正常而控制量异常，问题在求解器或约束；如果状态范数就已经起飞，问题在模型或积分器。这套二分定位的方法比盯着代码发呆有用得多。

另外保留一份能跑通的初始配置。路径是复制一份 init 脚本改个名，里面存着所有默认参数。每次调参调翻车了，直接重新执行这份脚本回到起点，而不是靠记忆去复原参数。因为调参的过程往往是好几个参数一起变，出了问题你根本说不清是哪一步导致的。

6. 二次开发思路：把教材代码改成可运行的控制器原型

6.1 从运动学模型扩展到动力学模型

如果你最终要面对的是实车或高保真仿真平台，运动学模型往往不够用。扩展方向是在 kinematicModel 基础上增加横摆角速度状态，把前轮转角到横摆角的响应写成含侧偏刚度的一阶或二阶传递关系。注意动力学模型对参数标定的敏感度远高于运动学，我习惯先用单位参数跑通结构，确认稳定后再逐项标定实车参数。

% 动力学模型扩展骨架（示意） function dx = dynamicModel(x, u, params) % x = [X; Y; phi; v; beta; omega] % beta为质心侧偏角，omega为横摆角速度 Fy_f = params.Cf * (u(1) - (x(3) + x(5))); % 前轮侧向力 Fy_r = params.Cr * (-x(5)); % 后轮侧向力 dx(5) = (Fy_f + Fy_r) / (params.m * x(4)) - x(6); dx(6) = (params.Lf * Fy_f - params.Lr * Fy_r) / params.Iz; % 其余状态导数沿用运动学部分 end

这段骨架展示了从运动学跨到动力学的本质变化：把航向角速度从运动学几何关系改成由侧向力和横摆力矩驱动的动力学方程。参数说明：Cf 和 Cr 是前后轴等效侧偏刚度，单位是 N/rad，和轮胎型号与气压强相关；Iz 是绕 Z 轴的转动惯量，一般乘用车在 2000 到 4000 量级。扩展后控制器里的权重矩阵要补上新状态的对应项，建议初始把侧偏角和横摆角速度的权重设小，位置误差权重设大，这是工程上最稳的起点。

6.2 把参考轨迹接到你自己的路径规划模块

教材代码的参考轨迹通常是离线生成的固定曲线，而你自己的系统里参考轨迹大概率来自上游规划模块实时输出。改造思路是把轨迹生成函数替换成一个接口函数，输入当前时间或状态，输出未来 Np 步的参考序列。接口保持和控制器的输入约定一致，控制器本身不用大改。

我用过一次的接法是写一个外部轨迹提供模块，内部维护一个轨迹队列，上游规划每帧 push 新点进来，控制器每周期从队列头部拉取 Np 步参考。这样做的好处是解耦：先单独验证轨迹提供者的波形正确，再接进闭环，如果出问题，能明确到底是控制器的问题还是上游轨迹的问题。

6.3 验证方法

改造完成后，回归验证的顺序是从简到繁：先跑直线工况，确认基本跟踪不偏；再跑双移线，确认动态工况稳定；最后加干扰量（比如初始状态偏差或风阻项），观察 MPC 能否自己纠偏回来。每一步都用对照组模式跑，把改前改后的误差曲线叠在一张图上对比。我自己的教训是从前拿到这类代码总想一步到位直接换模型，结果每次翻车都在模型和控制器维度不对齐这一步。从那以后我每次动结构都先打开初始化脚本核对状态维度、再核对控制器入口注释里的向量长度，强制走一遍"模型 → 权重 → 约束 → 参考轨迹"四联检查，这套习惯帮我省掉了大量无意义的查错时间。希望这些经验能帮你少踩几个坑，顺利把教材代码跑成自己的控制器。

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