简介：《无人驾驶车辆模型预测控制》配套MATLAB程序包，面向智能车辆、自动控制领域的初学者、研究生及工程师，旨在弥补教材理论学习后缺少可运行代码的空白，帮助读者从仿真中直观理解MPC的预测、优化与反馈机制。资源压缩包共16个文件，包括13个.m脚本、1个.mdl仿真模型、1个.c程序及1个txt说明文档，整体仅36KB。脚本覆盖车辆动力学建模、Pacejka轮胎模型、横纵向运动控制、MPC控制器求解等关键环节，mdl文件可搭建Simulink可视化仿真环境，c文件便于对比算法效率或向嵌入式平台迁移，txt说明则提供了运行顺序与参数设置建议。目录按教材章节组织，第2章至第6章代码可对照书籍逐步验证，适合课程设计、毕业设计或算法复现时直接参考改造。已有2944人学习下载，是一份轻量、完整且易于上手的无人驾驶模型预测控制实践资源。
1. 无人驾驶车辆的模型预测控制：在 MATLAB 里做轨迹跟踪，先想清楚这三件事
想用 MATLAB 把无人驾驶车辆的模型预测控制跑起来，多数人第一步拿到的是一份“无人驾驶车辆模型预测控制 PDF”，照着公式敲完代码，却发现车辆在弯道里直接偏离参考轨迹。问题往往不在你抄错了公式，而在三个地方没想清楚：用的车辆模型是运动学还是动力学、预测时域和权重拿什么值起步、以及每个采样周期里“线性化—求解—施加”到底怎么循环。MPC 的核心思想并不复杂：每一时刻基于当前状态，往前预测一段轨迹，求解一个带约束的优化问题，然后只执行第一步结果。但要把这个“滚动优化”在 MATLAB 里跑成一条稳定的跟踪曲线，模型、参数、实现顺序一个都躲不掉。这篇笔记按我自己调试这类系统的顺序来讲，适合正在做课设、毕设或预研原型的人直接上手。
2. 车辆模型怎么选：运动学还是动力学，直接决定 MPC 的预测偏差
2.1 单车模型是最容易跑通的起点
绝大多数教材里的无人驾驶车辆模型预测控制，第一步都是建立车辆的运动学模型，也就是把车简化成自行车模型（单车模型）。后轴中心作为参考点，前轮转角作为转向输入，加上一个纵向速度，就得到状态量 ξ = [x, y, ψ, v]ᵀ，控制量 u = [a, δ]ᵀ，其中 ψ 是航向角，δ 是前轮转角。连续时间下的运动学方程是：
ẋ = v·cosψ
ẏ = v·sinψ
ψ̇ = v·tanδ / L
v̇ = a
L 是轴距。这个模型的物理直觉很直接：车辆瞬时转向半径由前轮转角和轴距决定，纵向加速度改变速度大小。选它做 MPC 的预测模型，不是因为精度高，而是因为状态少、雅可比矩阵容易求、实时性好。对低速园区车、泊车场景、以及大部分 MATLAB 课程设计来说，运动学模型足够。我见过不少把动力学模型硬搬到低速场景的工程实现，计算量翻了几倍，跟踪效果却没有本质提升——这就是模型选型没想清楚。
2.2 从运动学推第一个线性化模型：在参考轨迹上做泰勒展开
MPC 预测轨迹时，直接用非线性运动学模型做滚动优化当然可以，但 MATLAB 里求解非线性优化问题（fmincon 这种）的速度，和求解二次规划（quadprog）完全不在一个量级。所以工程上最常见的做法，是在参考轨迹点附近把运动学模型线性化。设参考轨迹由 (x_r, y_r, ψ_r, v_r) 描述，在每个参考点处做一阶泰勒展开，定义状态偏差 ξ̃ = ξ - ξ_r，控制偏差 ũ = u - u_r，得到线性时变误差模型：
ξ̃̇ = A(t)·ξ̃ + B(t)·ũ
其中 A(t) 和 B(t) 是雅可比矩阵，在参考点处取具体数值。比如 A 矩阵里会出现 -v_r·sinψ_r 和 v_r·cosψ_r 这几项，B 矩阵里会有 (v_r·tanδ_r)/L 和 v_r/(L·cos²δ_r) 这些项。写代码时最容易漏的是：线性化跟随参考点变化，每个采样周期都必须重新计算 A、B，而不是在仿真开始前算一次就复用。原因是车辆当前状态偏离参考轨迹越远，固定线性化点的误差就越大，控制器就越“看不见”真实车辆在哪里。
2.3 什么时候该换动力学模型：速度上 60 km/h 之后的事
如果你做的是高速公路场景，或者参考轨迹里包含紧急变道、大曲率弯道，运动学模型的预测偏差就会暴露出来。原因在于它假设轮胎始终处于线性附着区，没有考虑侧偏力、侧偏角和横摆角速度的耦合。此时需要切换到动力学单车模型，状态通常取 [v_x, v_y, ψ̇, ψ] 或者加上路面摩擦系数，控制量还是前轮转角 δ 和纵向力。动力学模型的线性化更复杂，但预测精度在高速场景下明显更好。一个务实的做法是：参数辨识或仿真对比速度超过 60 km/h 时用动力学模型，低于这个速度就用运动学模型——这也是很多量产预控制器采用的策略，低速用运动学降低算力开销，高速才上完整动力学。
3. 控制器内部怎么设计：预测时域、权重、约束是一组要反复调的参数
3.1 预测时域 Hp 和控制时域 Hc：选短了看得近，选长了算得慢
在线性时变 MPC 里，预测时域 Hp 决定你往前看多少步。Hp 太小，控制器像个近视眼，等看到弯道再打方向就晚了；Hp 太大，优化问题的决策变量变多，求解耗时成倍上升，而且在 MATLAB 解释执行的环境里特别吃亏。一组常见的起步值是 Ts = 0.1s，Hp = 20，Hc = 5。这意味着预测窗口总长 2 秒，但控制量只有前 5 步自由变化，后面 15 步的控制量保持不变。这个设计的工程含义是：让车辆提前 2 秒看到前方的曲率变化，同时压缩决策变量维度保证实时性。如果参考轨迹曲率变化剧烈，我一般会把 Hp 拉到 25～30，但 Hc 保持在 3～5 之间不变——自由控制步数多了反而容易引起求解器抖动。
3.2 Q、R 权重矩阵到底怎么给：横向偏差、航向偏差和控制量不是一回事
成本函数一般写成二次型形式：
J = Σ ξ̃(i)ᵀ·Q·ξ̃(i) + Σ Δu(j)ᵀ·R·Δu(j)
Q 和 R 是半正定对角矩阵。Q 对应位置、航向偏差的惩罚，R 对应控制量变化幅度的惩罚。起步值通常取 Q = diag([100, 100, 10])，R = diag([100, 100])，对应横向偏差权重 100、航向偏差权重 10、加速度增量和转角增量权重 100。别小看这组数值的比例关系，它直接决定车辆性格：Q 的横向项权重远大于航向项，车辆会优先削平横向误差，表现为“坚决回到参考线”；如果航向项权重也很大，车辆则倾向于保持当前航向，走长弧线慢慢修正。你拿到的“无人驾驶车辆模型预测控制 pdf”里如果给了参数，可以直接先用，但仿真时务必观察两组指标的此消彼长：横向误差峰值和转向盘平滑度。调权重的过程没有公式可套，本质是拿仿真结果反推，属于典型的“先跑通再调参”的活。
3.3 约束不是越多越好：把转向角限幅和增量限幅一起写进 QP 问题
MPC 相比纯跟踪和 PID 的核心优势就是显式处理约束。车辆物理上都有转向角限幅（比如前轮转角 ±30°）和执行器变化率限幅（每步转角变化不超过 ±0.5°）。这些约束写进优化问题时，体现为线性不等式组 A_ineq·u ≤ b_ineq。需要提醒的是，约束越满，QP 求解器越容易遇到无解或收敛慢的问题。一个常见工程处理是把转角限幅写成硬约束，把转角增量限幅写成软约束，即 QP 里加上松弛变量 ε 并给一个大惩罚系数。这样即便某一步约束冲突，求解器也能给出一个带小违约但可执行的控制量，而不是直接返回 infeasible。MATLAB 的 quadprog 本身不区分软硬约束，这需要在构造 cost function 时手动加松弛项，代价是维度多了几个，但对实时性影响很小。
4. 在 MATLAB 里实现线性时变 MPC 轨迹跟踪：从搭建到跑通的最小框架
4.1 准备数据流：参考轨迹、状态初始化和车辆参数
实现前先把变量理清。车辆用运动学单车模型，轴距 L = 2.5m，初始状态放在参考轨迹起点附近故意偏一点，才能看出 MPC 的纠偏能力。参考轨迹这里用一个平滑变化的螺旋线，横纵坐标用参数 t 生成，航向角通过 atan2 求相邻点差分得到。代码里把参考轨迹按照 Ts 等间隔采样好，存成数组，后续每个采样周期按索引取参考点。
Ts = 0.1; Hp = 20; Hc = 5; L = 2.5; % 轴距 x0 = 0; y0 = 0; psi0 = 0; v0 = 0; % 初始状态 t_total = 50; % 仿真时长 num_steps = round(t_total / Ts); % 生成参考轨迹：螺旋线 t_ref = linspace(0, t_total, num_steps + 1); r = 10 + 0.5 * t_ref; % 半径随时间增大 x_r = r .* cos(0.2 * t_ref); y_r = r .* sin(0.2 * t_ref); psi_r = atan2(diff(y_r), diff(x_r)); psi_r = [psi_r, psi_r(end)]; % 补最后一个点 v_r = 5 * ones(size(t_ref)); % 期望车速 5 m/s逻辑说明：这段代码先把时间离散化，再按参数方程生成参考轨迹。注意 psi_r 用相邻差分求，末尾补齐，否则索引对不齐。v_r 在低速场景取恒定值，后续如果你想测速度跟踪，也可以改成斜坡函数。
参数说明：Hp = 20 指的是预测窗口 20 步，对应 2 秒，Hc = 5 表示控制量只有前 5 步自由。这套取值适合曲率变化不剧烈的轨迹。如果你发现弯道跟踪误差大，优先把 Hp 涨到 25 或 30，而不是去动 Hc。
4.2 每个采样步的线性化与 QP 求解：MPC 的“优化循环”怎么写
这是核心循环。每走一个时间步 k，做五件事：取当前参考点 (x_r(k), y_r(k), psi_r(k), v_r(k))，计算该点处的 A、B 矩阵，离散化得到 Ad、Bd，构造预测方程，拼出 QP 的 H 矩阵和 f 向量，调 quadprog 求解，然后只取解的第一个分量作为控制量。
% 状态量 [x; y; psi; v]，控制量 [a; delta] % 每个周期内的预测序列用循环累积 for k = 1:num_steps % 1. 取当前参考点 xr = x_r(k); yr = y_r(k); psir = psi_r(k); vr = v_r(k); % 2. 线性化：状态偏差 [x-xr; y-yr; psi-psir; v-vr] A = [0, 0, -vr*sin(psir), cos(psir); 0, 0, vr*cos(psir), sin(psir); 0, 0, 0, tan(delta_ref)/L; 0, 0, 0, 0]; B = [0, 0; 0, 0; 0, vr/(L*cos(delta_ref)^2); 1, 0]; % delta_ref = 上一时刻参考转角，初值取 0 % 离散化：最简单的一阶欧拉 Ad = eye(4) + Ts * A; Bd = Ts * B; % 3. 构造预测方程 X = Psi*x_tilde + Gamma*U % 这里直接搭矩�阵，窗口尺寸 Hp=20, Hc=5 Psi = zeros(4*Hp, 4); Gamma = zeros(4*Hp, Hc); for i = 1:Hp % 幂次项 Ai = Ad^i; Psi((i-1)*4+1:i*4, :) = Ai; % 控制输入映射 for j = 1:min(i, Hc) % 第 j 个控制作用在 i 时刻的状态影响 Gamma((i-1)*4+1:i*4, j) = Ad^(i-j) * Bd; end end % 4. 构造 QP: min 0.5*u'*H*u + f'*u Q = diag([100, 100, 10, 0.1]); % 状态权重 R = diag([1, 1]); % 控制增量权重（此处直接用控制量加权） H_QP = Gamma' * kron(eye(Hp), Q) * Gamma ... + kron(eye(Hc), R) + 1e-6 * eye(Hc); % 当前状态偏差 x_tilde = [x_veh(k)-xr; y_veh(k)-yr; psi_veh(k)-psir; v_veh(k)-vr]; f_QP = x_tilde' * Psi' * kron(eye(Hp), Q) * Gamma; % 5. 约束：转角限幅 30 度，加速度限幅 +-1 m/s^2 lb = [-1; -deg2rad(30)] * ones(Hc, 1); ub = [ 1; deg2rad(30)] * ones(Hc, 1); options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set'); [U_star, ~, exitflag] = quadprog(H_QP, f_QP, [], [], [], [], lb, ub, [], options); % 6. 取第一步控制量并更新车辆状态 a_cmd = U_star(1); delta_cmd = U_star(2); % 状态更新用非线性运动学模型，避免模型失配导致仿真失真 x_veh(k+1) = x_veh(k) + Ts * v_veh(k) * cos(psi_veh(k)); y_veh(k+1) = y_veh(k) + Ts * v_veh(k) * sin(psi_veh(k)); psi_veh(k+1) = psi_veh(k) + Ts * v_veh(k) * tan(delta_cmd) / L; v_veh(k+1) = v_veh(k) + Ts * a_cmd; end逻辑说明：这段代码每步做了三件事中的核心两件——线性化和 QP 求解。需要特别指出的是预测方程里 Psi 和 Gamma 的拼法，Psi 是自由状态演化，Gamma 是控制输入的累积影响。如果你拿到的 PDF 里公式写法不同，最终 H_QP 和 f_QP 的形式可能会变，排查方法是先设 Hc = Hp = 1，手算一遍预测方程和 quadprog 的输入做对比，能对上再往大窗口扩。
参数说明：Q 里第四项是速度权重 0.1，因为参考速度恒定，速度偏差的惩罚不需要太大；R 给了 [1, 1]，如果车辆晃得厉害，把第二维（转角项）调到 10 或 100。注意 lb/ub 只约束了控制量幅值，没做增量约束，实际项目中建议额外加 U_star(k) - U_prev 的边界，这个后处理在避坑章节详细说。
4.3 把结果画出来：可视化不出来，等于没调
跟踪效果必须可视化，否则调参就是盲人摸象。最少画三张图：参考轨迹和实际轨迹的对比、横向偏差随时间变化、前轮转角随时间变化。横向偏差的计算方式很关键，不能简单用欧氏距离，因为你在弯道里离参考线近距离不代表横向误差小。正确做法是找到实际点位在参考轨迹上的最近投影点，取两点距离作为横向偏差，MATLAB 里可以用 dsearchn 或者自行遍历索引。
% 计算横向偏差：对每个实际轨迹点找最近参考点 for k = 1:length(x_veh) dist = sqrt((x_r - x_veh(k)).^2 + (y_r - y_veh(k)).^2); [min_dist, ~] = min(dist); lateral_error(k) = min_dist; end figure(1); plot(x_r, y_r, 'b--', 'LineWidth', 1.5); hold on; plot(x_veh, y_veh, 'r-', 'LineWidth', 1.2); legend('参考轨迹', '实际轨迹'); xlabel('x (m)'); ylabel('y (m)'); axis equal; grid on; figure(2); plot((0:num_steps)*Ts, lateral_error, 'k-'); xlabel('时间 (s)'); ylabel('横向偏差 (m)'); title('横向偏差');逻辑说明：最近点搜索用全轨迹遍历，简单但对长轨迹会比较慢。若轨迹点数上万，可以用 KDTree（MATLAB 的 rangesearch）来加速。横向偏差峰值如果控制在 0.1m 以内，对低速运动学 MPC 是合格水平；如果超过 0.5m，回头查 3.2 节的权重，以及 Hp 是否太短。
参数说明：图的横轴时间用 (0:num_steps)*Ts 而不是 1:num_steps，这样坐标轴语义正确。legend 和图例里的中文在 MATLAB 老版本可能出现乱码，如果遇到，直接把文本改成英文。
5. 避坑指南：MPC 参数和模型误差的 5 个典型翻车现场
5.1 现象：直线跑得好好的，一进弯道就甩出去
原因不难找：直线段参考曲率接近零，线性化误差小，MPC 表现很好；弯道段曲率突变，线性化点在参考轨迹上，但车辆实际状态与参考点偏差大，一阶泰勒展开的误差迅速放大。更本质的原因是你用的是运动学模型，高速进弯时侧偏角带来的横摆响应根本没被建模。
解决：先把速度降到 5 m/s 以下确认逻辑正确，再逐渐提速。弯道里如果横向误差持续增长，把 Hp 从 20 涨到 30，同时把 Q 的横向项权重从 100 提到 300。如果 30 的 Hp 仍然压不住误差，大概率是模型维度不够，该换动力学模型了。我自己的习惯是跑一个固定轨迹、多组速度的扫描实验来判断模型上限：每一档速度跑完，记录横向误差 RMS，误差超过 0.3m 的速度就是当前模型的极限。
5.2 现象：求解时间越来越长，仿真速度肉眼可见地掉帧
这个问题的根源往往不在 MPC 本身，而在于你在每个采样周期里调用的 quadprog 的算法选项。MATLAB 的 quadprog 默认算法可能是 interior-point-convex，对小型 QP 反而慢。另一个原因是每步重复构造 kron 大矩阵，MATLAB 解释执行这行代码的开销比求解器本身还大。
解决：把 optimoptions 的 Algorithm 显式指定为 'active-set'，并把 Hessian 矩阵改成稀疏矩阵赋值。更彻底的做法是：模型不变、参考轨迹不变的情况下，预测矩阵 Psi 其实可以预先算好一次（因为 Ad 在匀速场景变化很小），每个周期只需更新 f_QP。如果实时性依然不够，考虑把预测窗口内的求和用矩阵乘法向量化，而不是 for 循环累加。我实测过同样参数下，向量化写法比 for 循环快 5 倍左右。
注意：active-set 算法对小规模 QP 极快，但约束过多过密时迭代次数会激增，此时可以先用 interior-point 跑一次，记录求解器统计信息再决定。
5.3 现象：方向盘高频抖舵，车辆走 S 形
抖舵的直观原因是控制量在相邻周期内来回跳动。MPC 每步只取第一步控制量，后续预测被舍弃，但成本函数里如果没有控制增量约束，求解器每次给出的 u 可能在可行域边缘来回横跳。特别是在权重 R 很小时，控制器对偏差反应过激，转角一步到位然后又回摆。
解决：给优化问题加上控制增量约束。具体做法是把 U_star(k) 和 U_prev 的差作为额外不等式写入约束矩阵。MATLAB 里可以拉长决策变量维度把增量显式化，也可以偷懒一点：求解后对转角做一阶低通滤波，delta_cmd_filtered = alpha * delta_cmd + (1-alpha) * delta_prev，alpha 取 0.7 左右。但滤波会引入相位滞后，适合原型验证，不适合精度要求高的场景。正规做法是重构 QP，让决策变量成为增量 Δu 而不是 u。
5.4 现象：参考轨迹里存在小毛刺，横向误差在毛刺处爆表
这是一类极易忽略的坑：你生成参考轨迹时用了很多离散点，其中个别点曲率突变（比如用鼠标画的轨迹，或者由多个线段拼接的折线）。运动学模型对这个点的响应是转角瞬间打满，然后 MPC 在后续看到的是已偏离的轨迹，拼命纠偏。
解决：先平滑参考轨迹。用 MATLAB 自带的 smoothdata，或者自己实现一个简单的低通滤波器对 x、y 分别滤波。平滑后重新计算航向角，注意不能直接对平滑前的 psi_r 平滑，而要用平滑后相邻点差分重算。另一个做法是限制参考轨迹每步的曲率变化率，超过阈值的点用线性插值替代。
提示：很多人拿到网上下载的轨迹数据集直接当成参考轨迹用，里面常带着传感器噪声，不预处理就喂给 MPC，后期的调参工作大半是在对抗噪声而不是调控制器本身。
5.5 现象：仿真跑了十几秒之后，速度越来越慢，最后卡死
如果你在循环里用了 plot、drawnow 每个采样步都刷新图形窗口，MATLAB 的图形系统到后期会积累大量图形句柄，交互速度肉眼可见地下降。这是 MATLAB 仿真最常见的坑，跟 MPC 求解器无关。
解决：把可视化移出主循环，每 5 步或者 10 步画一次，或者直接关掉动态绘图，等仿真结束一次性画三张静态图。优化后你会发现所谓的“MPC 求解太慢导致实时性不够”有相当一部分其实是绘图拖后腿。判断方法是在循环里加 tic/toc 分别统计求解耗时和绘图耗时，数据说话。
6. 进阶一步：从线性 MPC 换到非线性 MPC，以及怎么验证你的结果
线性时变 MPC 在低速场景够用，但如果你要做的轨迹里包含倒车入库、原地掉头这类大转角的泊车动作，运动学模型的线性化误差就会大到收敛失败。此时值得换到非线性 MPC。常见做法是用 fmincon 配合 SQP 算法直接优化非线性运动学模型，代价是从一个采样周期内的运行时间来看，fmincon 的收敛速度和稳定性都不如专用 QP 求解器。想做得更扎实，用 CasADi 在 MATLAB 里队编译成 MEX 求解，可以保持 MATLAB 的调试环境又能获得接近 C++ 的求解速度。这套路线适合动手能力强的读研或预研场景，课设阶段用线性 MPC 出成果已足够。
验证方面我习惯看三个数字：横向误差 RMS（全程）、横向误差 P95（代表极端情况，比峰值稳）、控制量变化率的最大值（判断抖舵程度）。RMS 小于 0.05m、P95 小于 0.2m、转角变化率没有超过执行器物理上限，就算合格。数据之外的可视化验证，推荐画一个“横向误差随时间变化”的带尾巴图，把弯道入口、出口的误差段高亮标出，这样参考轨迹的哪一段让 MPC 最吃力一目了然。
关于 MPC 这行统了近十年的控制器调参，我自己的血泪经验是：先保证状态更新用的是非线性运动学方程，再谈参数优化。因为线性预测模型和非线性被控对象之间的失配是正常现象，但如果把预测模型和被控对象的更新都写成同一套线性化公式，仿真结果会“好看到失真”——你调的参数在真实车上根本不成立。最后一条建议，别迷信论文里的权重参数，你拿到的 PDF 只是给了算法骨架，车辆参数和轨迹特性一变，参数就得重调，这不是玄学，是把模型失配和权重整定的关系想清楚。希望帮到你。
本文还有配套的精品资源，点击获取
