简介面向需要掌握模型预测控制落地方法的控制领域学习者压缩包内用MATLAB实现了基于quadprog函数的线性MPC函数并依次给出双积分系统、倒立摆、车辆运动学模型、车辆动力学模型四个控制demo覆盖从基础被控对象到移动平台的多类场景能直观看到MPC原理推导如何转化为可运行的仿真代码也便于对比不同系统下预测时域、约束设置与权重整定的差异。文件总数达574个包体约49.35MB除15个m脚本外还包含大量C/C源码h、cpp、cmake工程配置、构建脚本及md/pdf说明文档适合配合源码逐段调试、用于课程设计或以其中MPC函数为基础进行二次扩展。目前已有525人学习对研究生、工程师和准备相关竞赛的读者而言这套实现包兼顾理论讲解与工程实操不仅给出可直接运行的示例代码也保留了中间构建文件能有效缩短从算法理解到代码落地的距离。1. 为什么还用quadprog手写MPC从黑盒工具箱到可改装的预测控制器拿到一个用Matlab的quadprog从零实现的线性MPC资源包时第一反应是有点反直觉——Matlab自带的Model Predictive Control Toolbox已经很成熟为什么还要自己动手写一个答案在于工具箱的黑盒特性让人很难看清预测模型、代价函数、约束矩阵在求解器内部是如何被装配的。当你想把MPC从仿真搬到实车、实盘或实验平台上修改代价函数中的某一项权重、加入自定义不等式约束、或把求解器换成嵌入式C代码自带工具箱的封装反而成了障碍。这套资源包用不到200行的核心函数替代了工具箱用quadprog完成了线性MPC的QP求解并挂上了四个demo——双积分、倒立摆、车辆运动学、车辆动力学。对正在做控制算法研究、无人车路径跟踪、或准备把MPC部署到实车上的人来说这套代码的价值在于每一行矩阵操作都能看到、能改、能断点调试。适合想真正理解MPC内部机理而不是停留在调用mpc对象的工程师和学生。2. MPC预测模型与QP装配把滚动优化写成quadprog标准型2.1 预测模型的矩阵展开从单步递推到预测序列线性MPC的核心是先有一个离散状态空间模型x(k1) A·x(k) B·u(k) y(k) C·x(k)其中x是状态向量u是控制输入A、B分别为系统矩阵和输入矩阵。MPC要做的事情是在当前时刻k拿到实测状态x(k)后预测从k1到kNp共Np步的状态轨迹并优化这Np步内的控制序列。Np称为预测时域。把上述递推式反复代入可以将未来Np步的状态序列X [x(k1); x(k2); …; x(kNp)]表示为当前状态和未来控制序列的线性组合X Phi·x(k) Psi·U其中U [u(k); u(k1); …; u(kNc-1)]是未来Nc步的控制序列Nc为控制时域。Phi和Psi是由A、B按分块方式组装出来的常数矩阵。在Matlab代码中这两个矩阵的构造通常用两层循环或blkdiag完成% 预测域 Np控制域 Nc % 构造 Phi第 i 块对应 A^i Phi zeros(Np*nx, nx); Psi zeros(Np*nx, Nc*nu); for i 1:Np Phi((i-1)*nx1 : i*nx, :) A^i; for j 1:min(i, Nc) Psi((i-1)*nx1 : i*nx, (j-1)*nu1 : j*nu) ... A^(i-j) * B; end end这段代码的思路是第i步预测状态表示为A^i·x(k)加上前i个控制输入的贡献每个控制输入的贡献为A^(i-j)·B。由于Nc通常小于等于Np超过Nc之后控制量保持不变或不再参与优化因此内层循环只到min(i, Nc)。Phi和Psi只依赖系统矩阵、预测域和控制域不依赖当前状态所以可以在仿真循环外只计算一次避免重复开销。2.2 代价函数改写为二次型H矩阵与f向量的装配规则MPC的代价函数一般写成状态偏差和控制量加权的二次型J sum_{i1}^{Np} x(ki)·Q·x(ki) sum_{j0}^{Nc-1} u(kj)·R·u(kj)Q是状态权重矩阵半正定R是控制权重矩阵正定。把上一步的X Phi·x(k) Psi·U代进去J就展开成关于U的二次型。Matlab的quadprog求解的是如下标准形式min_U 0.5·U·H·U f·U所以需要把J中二次项和一次项分别提取出来。这里的关键推导是将X代入后X·Q̄·X展开会出现U·(Psi·Q̄·Psi)·U和2·x(k)·Phi·Q̄·Psi·U两个部分其中Q̄是Np个Q构成的分块对角矩阵。最终装配规则为% 分块对角权重 Q_bar blkdiag(kron(eye(Np), Q)); % Np*nx 维方阵 R_bar blkdiag(kron(eye(Nc), R)); % Nc*nu 维方阵 % 二次项和一次项系数 H 2 * (Psi * Q_bar * Psi R_bar); f 2 * Psi * Q_bar * Phi * x0;注意这里H乘了2是因为quadprog的目标函数自带0.5系数。如果不乘2求解器实际优化的会是0.5·U·H·Uf·U导致权重与设计意图相差一倍。这是手写MPC最常见的错误之一。f向量依赖当前状态x0所以每个控制周期都要重新计算而H只要模型和权重不变就可以一直复用。2.3 约束的规范化不等式约束、等式约束与上下界在quadprog中的映射quadprog支持四类约束不等式A·U b、等式Aeq·U beq、上界ub、下界lb。在MPC中约束主要来自执行器饱和和状态限幅。最常见的是控制量幅值约束u_min u(kj) u_max, j 0, …, Nc-1这可以直接映射到lb和ub向量的每个分块都填充相同的上下界。另一种常见约束是控制增量约束|Δu| Δu_max这需要用不等式矩阵表达。将Δu(kj) u(kj) - u(kj-1)展开成一阶差分矩阵% 控制增量约束|du| du_max % 构造差分矩阵 D使 D*U [u(k)-u(k-1); u(k1)-u(k); ...] D zeros(Nc*nu, Nc*nu); for i 1:Nc D((i-1)*nu1 : i*nu, (i-1)*nu1 : i*nu) eye(nu); if i 1 D((i-1)*nu1 : i*nu, (i-2)*nu1 : (i-1)*nu) -eye(nu); end end % 不等式D*U du_max u(k-1)-D*U du_max - u(k-1) A_ineq [D; -D]; b_ineq [du_max u_prev; du_max - u_prev];第一个分块u(k) - u(k-1)中u(k-1)是上一时刻的实际控制量是已知量所以要挪到不等式右侧放入b_ineq。将A_ineq、b_ineq、lb、ub一起传给quadprog求解器就能在满足约束的前提下找到最优控制序列。下面用一个表格总结quadprog参数与MPC变量的对应关系方便写代码时对照quadprog参数MPC中的含义维度备注H代价函数二次项系数(Nc·nu) × (Nc·nu)需乘以2f代价函数一次项系数Nc·nu × 1依赖当前状态x0A不等式约束矩阵m × (Nc·nu)如增量约束b不等式约束右端项m × 1含上一时刻控制量lb/ub控制量上下限Nc·nu × 1按分块填充Aeq/beq等式约束通常无空矩阵[]一般不使用3. my_mpc核心实现与状态空间建模控制对象如何变成A、B矩阵3.1 主函数接口与QP求解循环资源包里的核心函数名为my_mpc接口设计大致如下function u_opt my_mpc(A, B, Q, R, Np, Nc, x0, u_prev, u_min, u_max, du_max) nx size(A, 1); nu size(B, 2); % 构造预测矩阵 Phi, Psi [Phi, Psi] build_prediction_matrix(A, B, Np, Nc); % 装配 H, f H 2 * (Psi * kron(eye(Np), Q) * Psi kron(eye(Nc), R)); f 2 * Psi * kron(eye(Np), Q) * Phi * x0; % 控制量上下界 lb repmat(u_min, Nc, 1); ub repmat(u_max, Nc, 1); % 控制增量约束矩阵 [A_ineq, b_ineq] build_rate_constraint(Nc, nu, du_max, u_prev); % 求解QP options optimoptions(quadprog, Display, off, ... Algorithm, interior-point-convex); [U, ~, exitflag] quadprog(H, f, A_ineq, b_ineq, [], [], lb, ub, [], options); if exitflag 0 warning(MPC:QPFailed, quadprog exitflag%d, 使用上一时刻控制量, exitflag); u_opt u_prev; else u_opt U(1:nu); % 只取第一个控制量 end end注意build_prediction_matrix和build_rate_constraint是辅助函数分别完成2.1节中的矩阵展开和约束构造。QP求解之后只取解向量U中的前nu个元素作为当前时刻的控制量这是滚动优化的核心——每次只执行第一步下一时刻重新用新状态求解。exitflag是QUADPROG返回的求解状态标志为负值表示求解失败此时资源代码会回退到上一时刻控制量并给出警告避免控制器输出异常值。3.2 离散化方法连续模型到离散状态空间的转换MPC本质是离散控制但物理系统通常是连续时间的。资源包四个demo中双积分和倒立摆都用连续微分方程描述需要离散化为x(k1)Ax(k)Bu(k)。常用的离散化方法有前向欧拉和零阶保持。前向欧拉最简单A_dis I A_con·Ts B_dis B_con·TsTs是采样周期。这个方法的缺点是采样周期大时精度会下降。零阶保持离散化则假设控制量在一个采样周期内保持不变通过矩阵指数计算精度更高。Matlab中可用c2dm旧版或直接调用expm% 连续系统矩阵 Ac, Bc采样周期 Ts M expm([Ac, Bc; zeros(nu, nxnu)] * Ts); A_dis M(1:nx, 1:nx); B_dis M(1:nx, nx1:nxnu);推荐后一种方式尤其是倒立摆这类本身不稳定的系统前向欧拉在步长稍大时会导致离散模型失真控制效果与连续系统偏差明显。在代码中看到demo用哪种离散化方法直接搜索脚本里的expm或c2d即可。3.3 系统矩阵参数化四个对象如何抽象成同一套接口my_mpc函数只认A、B、Q、R四个输入因此四个demo之间的差异全部体现在系统建模上。双积分模型的状态是位置和速度输入是加速度倒立摆模型的状态是小车位置和摆杆角度输入是外力车辆运动学模型的状态是位置和航向角输入是速度和前轮转角车辆动力学模型的状态则多了侧向速度和横摆角速度。这里的核心技巧是无论模型多复杂只要能在工作点附近线性化成A、B矩阵就能直接套用同一个my_mpc函数。实际代码中每个demo的脚本结构大致是定义系统参数 → 计算A、B矩阵 → 设定权重Q、R→ 进入仿真循环在循环内调用my_mpc并获得控制量然后用实际模型更新状态。4. 实例拆解双积分、倒立摆、车辆运动学与动力学控制4.1 demo1双积分控制——最简闭环结构双积分模型的物理含义是状态x [位置; 速度]控制量u为加速度连续方程dot(p) v, dot(v) u。这个模型虽然简单却涵盖了MPC的全部要素。离散化后系统矩阵为Ts 0.1; A [1 Ts; 0 1]; B [Ts^2/2; Ts];权重设置时位置偏差的关注度通常高于速度因此Q diag([10, 1])控制量权重R 0.1。预测时域Np取20控制时域Nc取5约束为|u| 2。运行demo时观察到的现象是MPC给出的控制序列不会像LQR那样一次性施加大幅控制量而是会提前减速——因为它能看到未来20步的状态轨迹在逼近目标位置前就开始规划减速。这个demo最适合用于验证my_mpc函数的正确性。如果控制效果发散或震荡优先检查H矩阵是否乘以2以及lb/ub中的行数是否与Nc*nu一致。4.2 demo2倒立摆控制——不稳定系统对预测域的要求倒立摆是验证MPC鲁棒性的经典对象。线性化后连续状态空间模型为dot(theta) omega dot(omega) (g/l)·theta (1/(m·l²))·u其中theta为摆角omega为角速度g为重力加速度l为摆长。注意这个模型假设摆角在小角度范围内当角度超过约15度时线性化误差会明显增大。参数设置建议l 1m 1。对状态[theta; omega]分别设置权重Q diag([100, 1])控制权重R 0.01。倒立摆模型有一个关键特性A矩阵含有正实部特征值不稳定模态。这要求Np必须足够大使MPC能预见不稳定发散的后果否则控制力会不足。Np为10时系统可能勉强稳住Np取到30以上时控制明显更平滑。这是MPC对比LQR的一个优势——LQR是无限时域最优而MPC的有限时域可以让工程师显式设置预测距离在不稳定系统中这直接决定了稳定裕度。4.3 demo3车辆运动学模型控制——轨迹跟踪的几何约束车辆运动学模型又常被称为自行车模型状态为[x; y; psi]横向位置、纵向位置、航向角输入为[v; delta]车速与前轮转角。连续模型为dot(x) v·cos(psi) dot(y) v·sin(psi) dot(psi) v·tan(delta) / L这是一个非线性系统直接套用线性MPC的前提是线性化。资源包的常见做法是沿参考轨迹做泰勒展开取参考速度v_ref和参考航向角psi_ref得到误差状态的线性模型。以直线参考轨迹为例设psi_ref 0则小角度近似下的线性模型为% 误差状态 [x_err; y_err; psi_err]输入 [v_err; delta] A [0 0 -v_ref*Ts; 0 0 v_ref*Ts; 0 0 0]; B [Ts 0; 0 v_ref*Ts/L; 0 v_ref*Ts/L];L是轴距v_ref是参考车速。Q中对横向偏差y_err的权重通常要设得比较大比如100psi_err权重次之x_err权重可以小一些因为纵向位置误差可以由车速修正。前轮转角的物理约束为delta 30°约0.52 rad这在lb/ub中直接设置即可。运行这个demo时最常见的调试需求是观察车辆是否平滑收敛到参考线。如果轨迹出现来回振荡多半是R矩阵中对delta的权重太小增大对应项即可抑制前轮频繁摆动。4.4 demo4车辆动力学模型控制——侧偏特性与稳定性边界车辆动力学模型比运动学模型多考虑轮胎侧偏力。常见的二自由度模型状态为[v_y; r]侧向速度、横摆角速度输入为前轮转角delta连续方程为dot(v_y) -(C_f C_r)/(m·v_x)·v_y - (v_x (C_f·l_f - C_r·l_r)/(m·v_x))·r C_f/m·delta dot(r) -(C_f·l_f - C_r·l_r)/(I_z·v_x)·v_y - (C_f·l_f² C_r·l_r²)/(I_z·v_x)·r C_f·l_f/I_z·delta其中C_f、C_r为前后轮侧偏刚度l_f、l_r为质心到前后轴的距离I_z为横摆转动惯量v_x为纵向车速这里视为常数。侧偏刚度受路面附着系数影响明显干燥沥青与湿地差别巨大实际工程中通常要在线估计或做鲁棒设计。动力学模型的MPC控制器通常输出delta与参考横摆角速度的偏差约束条件包括前轮转角饱和±30°和横向加速度限制防止轮胎超出线性侧偏区。这里的Q矩阵权重放在v_y与r上当横向速度偏差持续增大时说明车辆正在偏离稳定行驶状态。这个demo与运动学模型demo的最大区别在于运动学模型适用于低速如泊车动力学模型适用于高速如高速变道两者的工作点完全不重叠。5. Np与权重的整定技巧及quadprog数值故障诊断5.1 预测时域与控制时域的配合规则Np决定了控制器向前看多远Nc决定控制器有多自由。实际调试中先把Nc固定在3到5然后由小到大调整Np。一个实用的经验值是让Np·Ts约等于系统从当前状态到稳态的主要时间常数如倒立摆振荡周期的2到3倍。Np太小则约束作用不明显Np太大则计算量增大且远期预测对模型误差敏感。如果四个demo都用默认参数能稳定运行优先尝试修改Np观察系统响应变化——这是理解滚动优化机理成本最低的实验。5.2 权重Q、R的调节方向与系统性方法Q和R的相对大小决定了控制器的激进程度。若跟踪误差收敛太慢增大Q中对应状态的对角元素若控制量振荡或变化剧烈增大R。对多输入系统R的不同对角项还会影响各输入之间的优先级——例如车辆运动学demo中如果希望优先用速度修正纵向误差其次再用转角修正横向误差就将R中对应v的权重设得比delta小。遇到多变量耦合不易整定时按先Q后R、先主对角后非对角的顺序逐一调整每次只改一个参数。5.3 求解失败时的系统化定位方法quadprog返回exitflag为负值并不都意味着代码写错了还可能是数值问题。按以下顺序排查第一步检查H矩阵是否正定在Matlab命令行直接输入eig(H)若最小特征值为负则说明H装配有误最常见的是漏乘2或Psi构造时循环边界出错第二步检查约束是否自相矛盾尤其注意lb与ub的维度是否为Nc*nu而不是nu第三步检查状态数值量级如果某维状态达到1e3而其他维为1e-2QP的KKT条件会出现尺度问题调整该状态的权重或将其归一化后再求解。5.4 一个实用的验证技巧用零输入检验模型装配在仿真循环前先做一个开环测试将控制序列设为零或常数用第2.1节的Phi和Psi公式手动计算预测状态序列同时用原始状态空间模型递推同一段轨迹对比两者是否一致。这个测试只需要几行代码却能在几分钟内验证矩阵装配正确性避免把问题带到闭环调试阶段。具体做法是在脚本开头加入如下检查x_pred Phi * x0; % U0 时的预测 x_sim x0; for i 1:Np x_sim A * x_sim; end if norm(x_pred(end-nx1:end) - x_sim) 1e-8 error(预测矩阵 Phi 装配有误); end这一步做完后再跑闭环仿真时遇到收敛性问题就可以把原因锁定在权重调节或约束设置上而不是矩阵装配。这也是手写MPC对比黑盒工具箱的最大优势——每个环节都可以单独验证。本文还有配套的精品资源点击获取