简介基于MATLAB的MPC模型预测控制源码包从线性MPC原理推导出发借助quadprog函数封装出一套可复用的线性MPC控制器并在此基础上提供双积分控制、倒立摆控制、车辆运动学模型控制与车辆动力学模型控制四个可直接运行的Demo覆盖从基础对象到复杂车辆系统的典型控制场景适合自动控制、机器人或智能车辆方向的初学者与研究人员对照学习。资源包共574个文件压缩后约49.35MB核心文件包括MATLAB的m脚本、C/C源文件h/cpp/c、Markdown说明文档与PDF文档同时附有13个Visual Studio工程文件和构建脚本方便在MATLAB与C环境中迁移、调试与二次开发。包内目录结构完整包含C工程配置、第三方依赖与图形界面相关组件便于复现实验环境并可视化控制效果。目前已有525人学习下载特别适合希望从原理推导到代码落地、快速建立MPC完整认知的读者。1. 为什么用MPC模型预测控制而不是PID——先看一个实际场景当一个被控对象同时存在输入饱和、状态约束和强耦合时PID 调起来会非常痛苦。比如无人机姿态控制电机转速有上下限还要避免超调碰撞单回路 PID 需要反复试凑多回路之间还会互相牵制。MPC 模型预测控制的思路完全不同在每个采样周期先利用当前状态向前预测未来 N 步然后在线求解一个带约束的二次规划问题执行第一步下一周期重新做一遍。这种“预测 滚动优化”机制让约束可以被显式写进优化目标而不是用手摸出来的积分限幅。MATLAB 自带 quadprog、lsqlin 等优化求解器也有成熟的 Model Predictive Control Toolbox所以从原理到代码实现MATLAB 是验证和部署 MPC 最常见的工作台。这篇文章就围绕“原理到代码”这条主线讲清楚预测模型、滚动优化和反馈校正三个环节再直接给出一段能跑的 MATLAB 代码最后聊透参数怎么调、非线性系统怎么处理以及从 MATLAB 走向嵌入式时求解器该注意什么。2. MPC原理拆解预测模型、滚动优化与约束处理MPC 不是一个固定的控制器而是一种控制算法框架。它的本质是用模型代替人脑去“想象未来”把控制问题变成优化问题。这一章我会把三个核心环节拆开并给出对应的数学表达和 MATLAB 里的表达方式。2.1 预测模型用状态空间方程描述未来动态MPC 最常用的是离散线性时不变状态空间模型x(k1) A x(k) B u(k)y(k) C x(k)其中 x 是 n 维状态u 是 m 维输入y 是输出。工程上通常从连续模型出发用 MATLAB 的c2d把连续系统离散化采样周期 Ts 的选择会影响预测精度和实时性。% 连续系统离散化示例 Ac [0 1; -1 -1]; Bc [0; 1]; Cc eye(2); Dc [0; 0]; Ts 0.1; sysc ss(Ac, Bc, Cc, Dc); sysd c2d(sysc, Ts); Ad sysd.A; Bd sysd.B; Cd sysd.C; Dd sysd.D;这段代码里c2d默认采用零阶保持器即假设输入在两个采样点之间保持不变。对于实际执行器如 DA 输出这个假设通常是成立的。如果系统本身就是在 FPGA 或嵌入式里实现的离散控制也可以直接用离散模型。Ad、Bd是预测未来的基础后面所有矩阵组装都围绕它们进行。为了把“预测未来 Np 步”统一表达通常把未来状态堆成一个列向量 X(k) [x(k1|k); x(k2|k); ...; x(kNp|k)]同时把未来控制输入堆成 U(k) [u(k|k); u(k1|k); ...; u(kNc-1|k)]其中 Nc 是控制时域Nc ≤ Np。反复代入状态方程可以得到X Φ x(k) Γ U其中 Φ 和 Γ 只与 A、B、Np、Nc 有关可以在算法初始化时提前算好在线只需要做矩阵乘法和矩阵加减。这样能在每个周期省掉大量 for 循环也是手写 MPC 代码性能优化的关键。下面这张表整理了预测模型里常见的符号含义方便后续看代码。符号含义典型取值Np预测时域预测多少步10~30Nc控制时域优化多少步输入2~10Q状态/输出权重矩阵半正定对角阵R控制输入权重矩阵正定对角阵Ts采样周期被控对象响应时间的 1/10~1/20Φ预测矩阵中的状态传递部分由 A 和 Np 推导Γ预测矩阵中的输入传递部分由 A、B、Np、Nc 推导2.2 滚动优化每个采样周期都要解一次QPMPC 在每个采样周期 k 基于当前测量 x(k)在满足约束的前提下计算一组控制序列使目标函数 J 最小。目标函数常见写法如下J Σ (y(ki) - r(ki))^T Q (y(ki) - r(ki)) Σ Δu(ki-1)^T R Δu(ki-1)其中 Δu u(ki) - u(ki-1)表示控制增量限幅。这样可以在目标函数里隐含积分作用避免稳态误差。约束包括u_min ≤ u(ki) ≤ u_maxΔu_min ≤ Δu(ki) ≤ Δu_max有时还需要对系统输出或状态加约束例如 y_min ≤ y ≤ y_max。把上述目标函数展开并整理成矩阵形式后就变成标准的二次规划问题0.5 * U^T H U f^T U → mins.t. A_ineq U ≤ b_ineq, A_eq U b_eq, lb ≤ U ≤ ub其中H 2 * (Γ^T Q̄ Γ R̄)f 2 * Γ^T Q̄ (Φ x(k) - r_vec)这里 Q̄ 和 R̄ 是将 Q、R 按 Np 和 Nc 重复堆成的块对角矩阵r_vec 是未来参考轨迹向量。MATLAB 的quadprog可以直接求解这种凸 QP求解器的算法可以选择interior-point-convex或active-set其中内点法对大矩阵收敛更稳定但每次迭代计算量稍大主动集法在小规模问题上速度更快。我的习惯是 Np 低于 20 用 active-set超过 30 用 interior-point-convex。2.3 反馈校正为什么MPC不怕模型误差纯“开环预测”不可靠所以 MPC 在每个周期开始前都会重新读取当前状态 x(k)而不是用上一周期预测出来的值。即使预测模型不够精确模型误差也会通过状态刷新被不断修正这构成了闭环反馈。更工程化的做法是在模型中额外加入一个扰动项 d(k)把状态方程写成x(k1) A x(k) B u(k) B_d d(k)d(k) 通常用卡尔曼滤波器或扩张状态观测器从历史输出中估计出来。这样能让 MPC 对未建模动态、摩擦、偏差等扰动保持鲁棒性。在 MATLAB 的 MPC Toolbox 里这个功能对应默认的“输入扰动模型”工具箱会自动加入一个积分器从而消除稳态误差。如果你手写 MPC则需要自己维护一个状态估计器。最简单的做法是在每次采样时用测量到的输出修正预测状态例如 x_est x_pred L * (y_meas - C x_pred)L 可以理解成一个常增益观测器增益。3. 基于MATLAB的MPC实现路径工具箱与手写脚本怎么选MATLAB 里实现 MPC 有两条成熟路径一条是使用 Model Predictive Control Toolbox另一条是基于优化工具箱手写代码。两条路各有适用场景这一章对比清楚再给出各自的最小范例。3.1 内置MPC Toolbox到底帮你做了哪些事MPC Toolbox 提供了mpc、sim、mpcdesign等函数外部输入是对象模型和权重输出是一个 MPC 控制器对象。它把预测矩阵构建、QP 标准化、观测器设计全部封装起来还自带界面工具mpctool适合快速验证算法可行性。典型代码只有几行% 使用MPC Toolbox创建控制器 mpcobj mpc(sysd, Ts, Np, Nc); mpcobj.Weights.MV 0.1; % 控制输入权重 mpcobj.Weights.OV [1 1]; % 输出权重 mpcobj.MV(1).Min -1; % 输入下限 mpcobj.MV(1).Max 1; % 输入上限 mpcobj.MV(1).RateMin -0.2; % 输入变化率下限 mpcobj.MV(1).RateMax 0.2; % 输入变化率上限 sim(mpcobj, 30, 1);这段代码里Weights.MV对应 RWeights.OV对应 QRateMin和RateMax对应 Δu 约束。工具箱会自动处理状态估计和不可测扰动甚至能自动生成 Simulink 模块。对于系统维数较高、约束较多、需要快速搭原型的场景这是最省力的方式。但它的缺点是黑盒程度较高一旦实际行为和仿真不一致调试者很难从内部 QP 矩阵入手排查也不容易无缝移植到单片机。3.2 手写MPC时quadprog和lsqlin怎么选如果没有 MPC Toolbox 许可证或者你是做嵌入式部署需要完全掌控代码手写 MPC 是常见做法。手写核心在构建 H、f 以及约束矩阵最后交给优化工具箱的quadprog或lsqlin求解。quadprog是通用的二次规划求解器目标函数可以带任意线性约束lsqlin是线性最小二乘问题求解器内部也是 QP但接口更偏最小二乘适合目标函数中只有输出误差的场合。实际上lsqlin本质上是为“最小化 ‖C x - d‖²”设计的但 MPC 代价函数展开后会出现 f^T U 项用quadprog更自然。除非你能把问题整理成 min_U ‖Gamma*U - target‖² 这种纯二范数的形式并且没有状态约束否则我还是建议quadprog。我一般在代码里直接调用options optimoptions(quadprog, Display, off, Algorithm, active-set); [u_opt, ~, exitflag] quadprog(H, f, A_ineq, b_ineq, [], [], lb, ub, u0, options);参数A_ineq和b_ineq用于装配约束例如把输入增量约束 Δu 转换成线性不等式。如果不需要等式约束传[]即可。u0是上一次的最优解把它作为 warm start 可以显著加快求解速度。3.3 两种路径对代码结构和调试的影响工具箱写出来的代码行数少但问题难定位约束冲突时提示信息往往只告诉你是哪组约束不可行却很难快速知道是哪一步矩阵溢出。手写代码则把每一步都暴露在眼前从构建 Φ、Γ到 H、f 都可以打印出来检查但工作量确实更大。我的建议是研究原理和调参时先用工具箱快速看到算法效果一旦需要上真机或者做实时控制再把手写代码作为基础。下表给出了两条路径的适用对比方便你选型。维度MPC Toolbox手写 quadprog开发效率高中可定制性中高对原理理解要求低高嵌入式移植难相对易调试手段黑盒透明许可证需专用工具箱仅需优化工具箱4. 从零写一个可复跑的MPC代码双积分系统示例这一章直接给出一段可以在 MATLAB 里运行的手写 MPC 代码。控制对象是双积分器也就是一个带有加速度输入的质点模型位置对加速度的传递函数是 1/s²。这是 MPC 教程里最典型的被控对象却能清晰展示预测、优化、约束处理的全过程。4.1 控制对象与离散化双积分器的状态空间双积分器状态取为x [位置; 速度]u 加速度连续状态空间矩阵Ac [0 1; 0 0]Bc [0; 1]Cc [1 0]Dc 0采样周期 Ts 设为 0.1 秒使用c2d离散化。这个模型虽然简单但已经包含了积分环节和二阶动态用来验证 MPC 对超调和约束的作用非常直观。4.2 构建预测矩阵批量把未来状态堆成大矩阵预测矩阵 Φ 和 Γ 是手写 MPC 的骨架。用循环逐列填充初始化阶段计算一次之后每个采样周期只需要矩阵乘法。填充代码逻辑如下对每个预测步 iΦ 的第 i 行块对应 A^iΓ 的第 i 行块对应从第 1 步到第 min(i,Nc) 步的输入传递矩阵。最终 X Φ x Γ U。4.3 完整MATLAB代码quadprog求解带约束MPC% 双积分器 MPC 控制器手写 quadprog 版本 clear; clc; close all; % 1. 系统模型 Ac [0 1; 0 0]; Bc [0; 1]; Cc [1 0]; Dc 0; Ts 0.1; sysd c2d(ss(Ac, Bc, Cc, Dc), Ts); Ad sysd.A; Bd sysd.B; Cd sysd.C; Dd Dc; % 2. MPC 参数 Np 20; % 预测时域 Nc 5; % 控制时域 Q diag([100 1]); % 状态权重位置权重100速度权重1 R 0.1; % 控制输入权重 r (k) 1; % 位置参考轨迹这里设为常数1 % 3. 构建预测矩阵 Phi 和 Gamma n size(Ad, 1); m size(Bd, 2); Phi zeros(n*Np, n); Gamma zeros(n*Np, m*Nc); for i 1:Np Phi((i-1)*n1:i*n, :) Ad^i; for j 1:min(i, Nc) Gamma((i-1)*n1:i*n, (j-1)*m1:j*m) Ad^(i-j) * Bd; end end % 4. 块对角权重矩阵 Qbar kron(eye(Np), Q); Rbar kron(eye(Nc), R); % 5. 输入约束-1 u 1 lb -ones(Nc, 1); ub ones(Nc, 1); % 6. 仿真 x [0; 0]; % 初始状态位置0速度0 Nsim 50; % 仿真步数 xhist zeros(n, Nsim1); xhist(:,1) x; uhist zeros(1, Nsim); for k 0:Nsim-1 r_vec r(k1) * ones(n*Np, 1); % 参考向量展开 H 2 * (Gamma * Qbar * Gamma Rbar); f 2 * Gamma * Qbar * (Phi * x - r_vec); [u_opt, ~, flag] quadprog(H, f, [], [], [], [], lb, ub); if flag 0 warning(QP求解失败使用零输入); u_opt zeros(Nc, 1); end u u_opt(1); % 只执行第一步 uhist(k1) u; x Ad * x Bd * u; % 状态更新 xhist(:, k11) x; end % 7. 绘图 t 0:Nsim; subplot(2,1,1); plot(t, xhist(1,:), LineWidth, 1.5); ylabel(位置); grid on; subplot(2,1,2); stairs(0:Nsim-1, uhist, LineWidth, 1.5); ylabel(输入); xlabel(采样步); grid on;代码逻辑说明每个采样周期先用当前 x 计算 H 和 f注意 H 只是初始化时算一次也可以但为便于理解我放在循环里。quadprog求解得到的u_opt是 Nc 维向量但 MPC 只执行第一个分量其余分量为下一周期的热启动做准备。状态更新用的是离散状态方程这是模拟被控对象的部分如果接入真实硬件这里就换成 DA 输出和 AD 采集。参数说明Q(1,1)100 意味着位置误差的惩罚远大于速度误差系统会倾向于快速到位R0.1 越小输入控制越激进越容易接近输入约束边界。lb和ub长度等于 Nc表示未来 Nc 步的控制输入都限制在 [-1,1]。如果你需要约束增量 Δu就需要额外构造线性约束矩阵A_ineq篇幅关系这里不展开。5. 调参实战预测时域、控制时域、权重矩阵对MPC性能的影响很多新手跑通代码后直接把 Np 改成 100结果发现系统响应变慢或者振荡这其实是对时域含义理解不到位。这一章讲清三个最核心参数的物理意义和调节趋势。5.1 预测时域Np设多大才算合理Np 决定了控制器“看得多远”。Np 太小预测视野太短控制器看不到远方的事件遇到约束时容易反应过激Np 太大预测矩阵维度上升QP 求解时间变长而且由于模型误差远期预测价值下降。经验值是从开环系统阶跃响应时间 T_settle 推算令 Np * Ts 大约等于 T_settle 的 60% 到 80%。对于双积分器Ts0.1 时Np 取 15~25 比较合理。你可以通过打印norm(Phi, fro)来观察状态传播的强度Np 增大后 Φ 的范数增长很快数值敏感性也会提升。5.2 控制时域Nc越小系统越保守Nc 是“未来只优化几步输入”的意思。Nc 较小时控制自由度少系统响应更平滑鲁棒性更强但性能会打折扣Nc 增大控制自由度增加可以让输入更快变化提高响应速度但代价是 QP 变量维数线性增加。一般 Nc 取 Np 的 1/5 到 1/2。比如 Np20 时Nc 取 4~10 合适。如果 NcNp预测矩阵 Γ 会变得接近方形QP 可能因为自由度过多而出现治标不治本的高频控制。5.3 权重Q和R怎么调对标量/向量权重 Q 和 R 直接反映控制目标偏好。工程上我习惯先做归一化处理把位置误差和目标值放到同一个量级再设置 Q 相对 R 的比例。一个常用起点是如果被控输出是位移单位 m和速度单位 m/sQ 对角元素可以取 [100, 1]因为速度误差通常比位移小一个数量级。R 则根据执行机构的容许力度调整。你可以在仿真里让 R 减半观察输入的峰值是否逼近约束边界。下面这张表总结了参数变化趋势前提是其他参数固定。参数调整响应速度超调输入峰值求解时间Np 增大减慢略减略减增加Np 减小加快增加增加减少Nc 增大加快略增略增增加Nc 减小减慢略减略减减少Q 相对 R 增大加快增加增加基本不变Q 相对 R 减小减慢减少减少基本不变调参顺序我建议先固定 Nc 为 Np 的 1/3然后调整 Q/R 让输入峰值大约在约束范围的 70% 左右最后再微调 Np。调试时除了看输出曲线还要打印exitflag和cond(H)如果cond(H)过大说明权重矩阵病态可能造成 QP 求解失败。用rcond(H)可以快速检查。6. 非线性系统与求解器MPC落地的进阶技巧和坑线性 MPC 只适用于工作点附近的控制但工程上大多数被控对象存在明显非线性。遇到这类问题时有两条常见路线要么在每个采样点做线性化得到时变模型再套 QP要么直接使用非线性 MPC用fmincon求解非凸优化问题。我的建议是优先走线性化路线因为fmincon求解时长通常在百毫秒级实时性很难保证。6.1 模型失配与扰动加一个状态估计器如果仿真好、实机差问题多半出在模型失配上。解决方案是引入扩张状态观测器或卡尔曼滤波器把未建模动态估计到扰动项 d 中再在预测模型里补偿。MATLAB 的kalmn函数或ssest都能辨识增量模型。短期做法是给 MPC 内部的积分器提高权重也就是在目标函数里增加 U 的积分偏差惩罚这等价于给对象增加一个积分环节能抹掉大部分稳态误差。6.2 非线性MPC用fmincon替代quadprog当被控对象是简单的非线性函数例如机械臂的 sin 项且控制周期较慢如 100 ms 以上可以用fmincon直接求解非线性优化问题。写法是把预测模型写成非线性函数供fmincon调用function J cost_fun(U, x0, ref, param) % 非线性预测模型所有状态通过仿真递推 x x0; J 0; for i 1:param.Np x param.f(x, U(i)); % 实际对象函数需替换 J J (x(1)-ref)^2 param.R * U(i)^2; end endfmincon需要设置算法推荐sqp或interior-point并且提供初始猜测 U0 为上一周期的最优解。注意这种写法每个周期要调用多次cost_fun计算量成倍上升所以只适合慢系统。6.3 求解器选型与实时性从MATLAB到嵌入式做实时控制时quadprog的通用实现可能不够快。业界常用嵌入式 QP 求解器如 OSQP、ADMM、dq 等。OSQP 的典型接口也是传入 H、f、A、lb、ub格式和 MATLAB 的quadprog很接近可以直接迁移到 C 语言环境。如果你最终要把 MPC 部署到单片机或 FPGA建议在 MATLAB 里把 H、f 的计算固定为矩阵乘法矩阵再为求解器单独封装接口。最后一个小技巧是在 MATLAB 中对比quadprog(H, f, ..., lb, ub)和 OSQP 的结果如果最大误差小于 1e-6说明数值实现基本等价。这样你就能放心理顺采样周期、约束边界和参考轨迹推进代码从仿真到落地。本文还有配套的精品资源点击获取