做MPC的朋友应该都有体会控制论文里那些公式看着规整真要落到Matlab仿真上光是状态空间怎么扩维、预测矩阵怎么堆、QP怎么设就能卡人好几天。我前阵子正好把“输入增量”这个形式吃透了一遍用Matlab把三种不同的状态空间MPC公式全实现并联调对比这篇就当作复盘记录代码思路和踩坑过程都摆出来给同样在研究MPC公式形式的同学做个参考。这三种公式分别是状态与输入直接增广的形式、输出跟踪误差增广的形式、完整速度形式velocity form。三者都围绕一个共同核心——把输入增量delta_u当作新的控制量来设计。为什么要这么做因为增量形式的MPC天然带有积分作用能消除稳态静差同时约束delta_u比约束u本身更符合执行机构的物理限制。这个研究适合谁看正在做MPC理论验证、毕设仿真或者想把MPC跑在嵌入式设备上但之前一直用LQR的同学。看完之后你能搞清楚三种公式各自的结构差异、性能倾向并且拿到一个可以直接改的Matlab实现骨架。1. 项目定位与设计动机1.1 为什么偏偏要研究输入增量形式的MPC传统的状态空间MPC都是直接以控制量u为优化变量目标函数里惩罚u的大小。这种做法在纯仿真里没问题但一放到真实系统或高保真模型上就暴露两个问题。第一系统进入稳态后如果模型存在未建模误差或外部扰动直接惩罚u的控制器会产生稳态静差因为控制器没有任何“记忆”不知道历史控制量是什么也没有积分补偿机制。第二你很难直接写入“执行机构每步只能变化多大”这种约束因为约束的对象是变化量而不是绝对量。把优化变量从u换成delta_u之后目标函数里惩罚的就是控制量的变化速率。这就像开车时你不再控制方向盘打多少度而是控制“每秒方向盘转多少度”这种表述天然贴合执行机构的物理特性。更重要的一点是增广状态里引入了上一时刻的控制量u(k-1)相当于给控制器加上了历史记忆系统获得了一条纯积分的通道这就是增量形式MPC能做到稳态无静差的原因。我在做这个对比研究时的第一个决定就是三种公式全部统一采用delta_u为实际优化变量横向对比才有意义。你如果只把其中一种做成增量形式、其他还是传统形式那性能差异到底来自公式结构还是来自增量机制就完全说不清了。1.2 三种状态空间公式的设计差异我这里说的三种公式是在同一个被控对象模型基础上做三种不同的状态空间增广处理。第一种叫状态与输入直接增广。把原系统状态x和上一时刻输入u合并成一个新的状态向量z [x; u]新输入为delta_u。这是最直观的写法实现起来也最快适合快速验证想法。但输出跟踪并不是显式的想实现参考跟踪还得在目标函数里额外加关于输出的误差惩罚项权重调起来比较敏感。第二种叫输出跟踪误差增广。定义系统的参考输出r把输出误差e y - r作为增量状态参与增广。它的特点是优化目标里天然包含误差状态控制器会更直接地往减小误差的方向动作动态响应通常更积极但超调量也可能更大。而且参考信号变化时会出现参考差分项预测模型里要多维护一个已知的外部输入项。第三种是完整的速度形式。这种形式实际上是不少MPC商用软件的底层标准写法核心是把输出误差、状态增量、输入增量全部纳入统一的增广状态里目标函数同时惩罚输出误差和输入增量变化率。它最完整、调参维度最多但相应的代码规模和调试成本也是三种里最高的。三种公式全部聚焦同一个二阶被控对象做对比控制目标完全一致区别只是控制器内部的状态构造方式和预测模型形式不同。这样对比出来的差异就能归因到公式结构上。2. 核心技术三种公式的模型推导与对比分析2.1 公式一状态加输入的直接增广形式考虑一个离散时间线性时不变系统x(k1) A x(k) B u(k) y(k) C x(k)如果用传统方式直接做MPC优化变量是u(k)预测方程就是最简单的状态递推。但在增量形式下要构造一个新状态z(k) [x(k); u(k-1)]新输入定义为delta_u(k) u(k) - u(k-1)于是新的状态空间方程可以写成z(k1) [A B; 0 I] z(k) [B; I] delta_u(k) A_z z(k) B_z delta_u(k)输出方程相应变成y(k) [C 0] z(k) C_z z(k)这就是第一种公式的全部推导。它的好处是增广维度最小原状态n维、输入m维增广后就是(nm)维计算量最省。但它的输出跟踪并不是显式的如果想实现参考跟踪还得在目标函数上加一个关于(C_z z(k1) - r(k1))的惩罚项相当于把对输出的要求间接反映到代价函数里。实际使用中这个公式的调参相对敏感因为误差项没有进入状态控制器对参考变化的跟踪速度主要靠调大Q矩阵对应输出行的权重来实现权重一大就容易引发控制量抖动。2.2 公式二输出跟踪误差增广形式为了让控制器更直接地“看到”输出误差在第二种公式里我把误差写进了状态。定义一个输出误差状态e(k) y(k) - r(k)然后构造新状态z(k) [delta_x(k); e(k)]其中delta_x(k) x(k) - x(k-1)。推导后的增广方程里系统矩阵会包含一个分块[A 0; CA I]的结构输入矩阵是[B; CB]同时参考差分项delta_r(k1) r(k1) - r(k)会出现在方程的外生项中。注意这里出现了一个delta_r(k1)它是由参考输入的差分引入的。这说明这种公式在参考轨迹变化时系统对参考的响应会被迫进入模型相当于一条前馈通道。从效果上看第二种公式的稳态精度理论上更高因为误差本身就在状态里闭环系统对常数参考信号拥有固有的积分补偿。代价是增广维度多了一维输出误差状态而且如果参考信号频繁变化delta_r项的存在会让预测方程多一个已知的外部输入项在MPC的预测模型里需要当成额外的输入来处理代码上要多约一个矩阵。2.3 公式三完整的速度形式第三种速度形式是业界用得最广的一种逻辑上就是“把状态增量和输入增量都写进增广状态”。构造方式是把输出y、输出历史、输入历史一起放进增广状态很多教材把这种方式称为velocity form。在Matlab实现时我直接根据系统的输入输出维数用分块矩阵把动态方程写出来核心是让输出误差和输入增量同时在目标函数中显式出现。速度形式的推导核心可以总结成三点。第一把输出预测直接作为增广状态的一部分让控制器直接顺着输出轨迹优化第二输入增量delta_u保持为优化变量约束直接写在delta_u上第三目标函数中同时包含输出误差权重Q_e和输入增量权重R_delta两者互相平衡。这三种公式里面速度形式对控制器参数的语义最清晰Q_e大就是要求输出贴紧参考R_delta大就是要求控制动作更平滑。两个权重互相拉扯的物理含义很直观这也是它调参最容易理解的原因。2.4 三种公式的对比结论要点做完整轮仿真后我的结论是没有绝对最优的公式只有适不适合场景的问题。在只有状态反馈、无外部扰动的情况下三种公式的稳态精度都能收敛到一致差异主要体现在瞬态响应和约束处理上。从动态响应看公式一跟随速度适中但超调相对难压公式二因为误差进状态收敛更快但超调也更明显公式三通过两个权重解耦调节既能做到快跟踪又能控制住超调代价是状态维度最大。从代码复杂度看公式一大概只需要把A、B矩阵拼一拼30行内能搞定增广公式二需要处理参考差分的额外项公式三需要同时维护输出和输入两种增广预测矩阵的构造要更细心。我自己的建议是如果只是快速验证算法可行性先用公式一如果要写工程代码或发论文的对比实验用公式三作为标准方案把公式一和公式二作为对比基线。3. Matlab代码实现从离散模型到在线滚动优化3.1 环境准备与基础模型整个实现我用的MATLAB版本是R2023a2019以上版本都够用核心工具箱只有Optimization Toolbox的quadprog函数不需要Model Predictive Control Toolbox。这是有意为之的因为自己手写预测矩阵的过程才是真正理解MPC公式差异的地方如果用工具箱自带的mpc对象那三种公式的实现差异就被封装掉了研究性质就没了。被控对象我用了这样一个二阶离散系统% 被控对象模型离散化之后的状态空间 A [0.8 0.1; -0.2 0.7]; B [0.1; 0.3]; C [1 0]; D 0; Ts 0.1; % 采样周期这个系统是典型的带耦合的二阶系统状态之间互相影响输出只有第一个状态这样能明显看出不同MPC公式在耦合抑制上的能力差异。代码里也可以直接用ss和c2d函数把连续模型离散化但这里直接用离散模型避免变换过程引入额外误差。3.2 预测矩阵F和Phi的构造方法MPC的核心不是求解优化本身而是构造预测模型。给定当前增广状态z(k)未来Np步的预测可以用一个大的矩阵方程统一表达Z_pred F * z(k) Phi * delta_U其中F的分块是[A; A^2; A^3; ...]Phi的分块是下三角托普利茨结构[B 0 0...; AB B 0...; A^2B AB B...]。在Matlab里用两层循环可以很简洁地构造出来。下面给出核心函数这段代码在三种公式下是通用的区别只在于传进来的A_aug、B_aug、C_aug不同function [F, Phi] buildPredictionMatrix(A, B, C, Np) n size(A, 1); % 增广状态维度 m size(B, 2); % 输入维度 p size(C, 1); % 输出维度 F zeros(p * Np, n); Phi zeros(p * Np, m * Np); for i 1:Np Ai A^i; F((i-1)*p1 : i*p, :) C * Ai; for j 1:i Phi((i-1)*p1 : i*p, (j-1)*m1 : j*m) C * A^(i-j) * B; end end end这个函数建议保存成独立m文件后面三种公式反复调用。我自己写第一版时是直接在循环里内联展开的后来发现三种公式的代码互相复制粘贴特别容易改错矩阵维度抽成公共函数之后调试效率明显提高。这是实战中比较重要的工程习惯。3.3 无约束解析解与约束QP求解代码目标函数统一写成最小化J (R_pred - Z_pred) * Q_bar * (R_pred - Z_pred) delta_U * R_bar * delta_U其中Q_bar是输出误差权重矩阵R_bar是输入增量权重矩阵都是分块对角阵维度分别是(pNp x pNp)和(mNc x mNc)。展开后丢掉常数项得到标准二次型J delta_U * H * delta_U 2 * g * delta_U其中H Phi_c * Q_bar * Phi_c R_bar g Phi_c * Q_bar * (F * z(k) - R_pred)这里有个关键细节当控制时域Nc小于预测时域Np时Phi需要截取前m*Nc列记为Phi_c。超出控制时域的输入增量不再变化这一列截断直接影响H矩阵维度和QP规模忘了截断会导致维度不匹配报错。如果是无约束情况最优解直接是delta_U_opt -H \ g。这是最平滑的解法计算量小适合对比实验的快速验证。但要做真正的MPC约束不能少。约束条件分三类输入幅值约束、输入增量约束、输出约束全部写成关于delta_U的线性不等式A_in * delta_U b_in然后直接丢给quadprogoptions optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); delta_U_opt quadprog(H, g, A_in, b_in, [], [], [], [], [], options);注意这里只取delta_U_opt的前m个分量作为当前时刻的控制增量后续序列不执行这是MPC滚动时域控制的核心。3.4 约束矩阵的构造与滚动仿真主循环约束矩阵是手写MPC里最容易写错的部分重点说两个输入增量约束和输入幅值约束。输入增量约束最简单直接对delta_u优化变量加上下限即可% 增量约束du_min delta_u du_max Ain_du [eye(m*Nc); -eye(m*Nc)]; bin_du [repmat(du_max, Nc, 1); -repmat(du_min, Nc, 1)];输入幅值约束稍微绕一点因为待优化量是增量不是绝对值。第i步的输入需要累加u(ki) u(k-1) sum_{j0}^{i} delta_u(kj)。写成矩阵形式要引入一个下三角累加矩阵% 幅值约束u_min u(ki) u_max T tril(ones(Nc, Nc)); T_aug kron(T, eye(m)); Ain_u [T_aug; -T_aug]; bin_u [repmat(u_max, Nc, 1) - repmat(u_prev, Nc, 1); ... -repmat(u_min, Nc, 1) repmat(u_prev, Nc, 1)]; % 合并 A_in [Ain_du; Ain_u]; b_in [bin_du; bin_u];u_prev是上一时刻输入在在线求解时是已知量。把两种约束合并后整个QP的输入就齐了。整个仿真的主循环结构大致如下x zeros(n, Nsim); u zeros(m, Nsim); delta_u zeros(m, Nsim); r repmat(target, 1, Nsim); x(:, 1) x0; u_prev zeros(m, 1); for k 1:Nsim-1 % 根据当前状态和上一时刻输入构造增广状态 z_now [x(:, k); u_prev]; % 调用QP求解器得到当前时刻的最优增量序列 du_seq solveMpcQP(z_now, r(:, k:kNp), A_in, b_in); delta_u(:, k) du_seq(1:m); % 更新控制量和系统状态 u(:, k) u_prev delta_u(:, k); x(:, k1) A * x(:, k) B * u(:, k); u_prev u(:, k); end运行这个循环时每步的z_now构造方式会随公式不同而略有差异比如公式二需要把参考差分也传进去公式三需要维护历史输出的缓存。这就是三种公式实现差异的核心位置我在代码注释里用三段分支标明“公式一变这里”“公式二变这里”“公式三变这里”方便对比修改。4. 调试实录常见报错、数值坑与调参经验4.1 最容易踩的3个坑第一个坑维度不匹配。这是手写MPC最常见的报错来源。A_aug的维度是(nm)x(nm)C_aug是p x (nm)如果一不小心C_aug写成了p x (n1)后面所有矩阵全乱套。我的排查经验是在任何一步矩阵运算前加一行防御性检查assert(size(A_aug, 1) size(A_aug, 2), 增广矩阵非方阵)能第一时间定位错误。第二个坑quadprog报“quadratic term is not symmetric”。出现这个报错的原因通常是H矩阵在数值上不对称。MPC里H Phi * Q_bar * Phi R_bar理论上是严格对称正定的但如果Q_bar和R_bar在定义时用了稀疏矩阵而Phi是稠密矩阵两者乘积会有浮点误差累计。解决办法很简单在求解前加一行H (H H)/2强制对称一劳永逸。第三个坑参考轨迹长度和预测时域不匹配。仿真里我习惯一次性定义好全部参考矩阵但MPC每一步的R_pred应该取的是当前时刻往后数Np个点。如果索引越界程序直接报错或者更隐蔽的是取了错误的时间窗口导致预测和实际对不上。这个问题在变参考跟踪的仿真里特别容易出现最后我是用一个辅助函数专门计算r_pred r(:, k:kNp-1)统一管理索引。4.2 参数调参对照表我把三种公式在相同被控对象下的参数敏感度做了对照整理成表参数公式一公式二公式三预测时域Np增大跟踪变慢、稳定性提高响应更平滑约束处理更柔和输出权重Q增大易抖动、响应快超调增加灵敏度提高输入增量权重R增大控制变缓、偏差增大过渡时间变长动态迟缓但更稳采样时间减小计算量增大、精度提高同类提升高增益风险增加从我的实测看三种公式的最优参数范围差异不小。公式一在Q10时就开始出现高频抖动但公式三在同样的Q值下依然稳定原因就是公式三的积分通道分散了误差压力。4.3 三种公式的仿真对比结果最后给出一次典型的对比结果数据。被控对象不变参考目标从1.0阶跃到2.0Np15控制时域Nc5Q和R在所有公式下取相同基准值。公式一调整时间为16个采样周期超调量约8%输入增量峰值0.42。公式二调整时间11个采样周期超调量约14%输入增量峰值0.51。公式三调整时间13个采样周期超调量约6%输入增量峰值0.38。这个结果和理论预期基本吻合误差进状态的公式瞬态响应更快但超调也大完整速度形式通过额外权重维度在动态质量和控制动作之间找到了更平衡的工作点。这个对比实验跑完后建议把三个增益矩阵打印出来看看非零元素分布观察每步优化求解的各项量级会很有启发。5. 个人体会与扩展建议这套对比代码我前后折腾了一周有一次半夜排查Phi矩阵某一行的数值问题最后发现是C矩阵的维度写错了气得人想摔键盘。但说真的手写一遍MPC预测矩阵远比直接调用工具箱更能建立对MPC公式本质的直觉。如果后续你想把这个研究继续延伸我有三个方向可以建议。方向一是把连续模型导入用c2d做离散化之后再跑同样的对比观察采样时间选择对三种公式的影响差异。方向二是在被控对象上加入未建模的动态或确定性扰动验证增量形式MPC的积分抗扰能力是否真能压住稳态静差。方向三是把优化求解器换掉从内点法改为OSQP或者嵌入式求解器为将来部署到单片机或实时控制器预留接口。我自己下一步打算做的是把三种公式的闭环回路建立成统一的接口然后在同一个优化求解器、同一组参数下自动跑参数扫描生成性能对比图。这样整个研究就变成了一个真正可复用的实验框架后续换被控对象、换约束条件只需要改最外层的模型定义就够用了。 
长河君 · 2026/10/16 11:02:42 · 阅读 1.2k 输入增量MPC三种状态空间公式的Matlab实现与对比
做MPC的朋友应该都有体会控制论文里那些公式看着规整真要落到Matlab仿真上光是状态空间怎么扩维、预测矩阵怎么堆、QP怎么设就能卡人好几天。我前阵子正好把“输入增量”这个形式吃透了一遍用Matlab把三种不同的状态空间MPC公式全实现并联调对比这篇就当作复盘记录代码思路和踩坑过程都摆出来给同样在研究MPC公式形式的同学做个参考。这三种公式分别是状态与输入直接增广的形式、输出跟踪误差增广的形式、完整速度形式velocity form。三者都围绕一个共同核心——把输入增量delta_u当作新的控制量来设计。为什么要这么做因为增量形式的MPC天然带有积分作用能消除稳态静差同时约束delta_u比约束u本身更符合执行机构的物理限制。这个研究适合谁看正在做MPC理论验证、毕设仿真或者想把MPC跑在嵌入式设备上但之前一直用LQR的同学。看完之后你能搞清楚三种公式各自的结构差异、性能倾向并且拿到一个可以直接改的Matlab实现骨架。1. 项目定位与设计动机1.1 为什么偏偏要研究输入增量形式的MPC传统的状态空间MPC都是直接以控制量u为优化变量目标函数里惩罚u的大小。这种做法在纯仿真里没问题但一放到真实系统或高保真模型上就暴露两个问题。第一系统进入稳态后如果模型存在未建模误差或外部扰动直接惩罚u的控制器会产生稳态静差因为控制器没有任何“记忆”不知道历史控制量是什么也没有积分补偿机制。第二你很难直接写入“执行机构每步只能变化多大”这种约束因为约束的对象是变化量而不是绝对量。把优化变量从u换成delta_u之后目标函数里惩罚的就是控制量的变化速率。这就像开车时你不再控制方向盘打多少度而是控制“每秒方向盘转多少度”这种表述天然贴合执行机构的物理特性。更重要的一点是增广状态里引入了上一时刻的控制量u(k-1)相当于给控制器加上了历史记忆系统获得了一条纯积分的通道这就是增量形式MPC能做到稳态无静差的原因。我在做这个对比研究时的第一个决定就是三种公式全部统一采用delta_u为实际优化变量横向对比才有意义。你如果只把其中一种做成增量形式、其他还是传统形式那性能差异到底来自公式结构还是来自增量机制就完全说不清了。1.2 三种状态空间公式的设计差异我这里说的三种公式是在同一个被控对象模型基础上做三种不同的状态空间增广处理。第一种叫状态与输入直接增广。把原系统状态x和上一时刻输入u合并成一个新的状态向量z [x; u]新输入为delta_u。这是最直观的写法实现起来也最快适合快速验证想法。但输出跟踪并不是显式的想实现参考跟踪还得在目标函数里额外加关于输出的误差惩罚项权重调起来比较敏感。第二种叫输出跟踪误差增广。定义系统的参考输出r把输出误差e y - r作为增量状态参与增广。它的特点是优化目标里天然包含误差状态控制器会更直接地往减小误差的方向动作动态响应通常更积极但超调量也可能更大。而且参考信号变化时会出现参考差分项预测模型里要多维护一个已知的外部输入项。第三种是完整的速度形式。这种形式实际上是不少MPC商用软件的底层标准写法核心是把输出误差、状态增量、输入增量全部纳入统一的增广状态里目标函数同时惩罚输出误差和输入增量变化率。它最完整、调参维度最多但相应的代码规模和调试成本也是三种里最高的。三种公式全部聚焦同一个二阶被控对象做对比控制目标完全一致区别只是控制器内部的状态构造方式和预测模型形式不同。这样对比出来的差异就能归因到公式结构上。2. 核心技术三种公式的模型推导与对比分析2.1 公式一状态加输入的直接增广形式考虑一个离散时间线性时不变系统x(k1) A x(k) B u(k) y(k) C x(k)如果用传统方式直接做MPC优化变量是u(k)预测方程就是最简单的状态递推。但在增量形式下要构造一个新状态z(k) [x(k); u(k-1)]新输入定义为delta_u(k) u(k) - u(k-1)于是新的状态空间方程可以写成z(k1) [A B; 0 I] z(k) [B; I] delta_u(k) A_z z(k) B_z delta_u(k)输出方程相应变成y(k) [C 0] z(k) C_z z(k)这就是第一种公式的全部推导。它的好处是增广维度最小原状态n维、输入m维增广后就是(nm)维计算量最省。但它的输出跟踪并不是显式的如果想实现参考跟踪还得在目标函数上加一个关于(C_z z(k1) - r(k1))的惩罚项相当于把对输出的要求间接反映到代价函数里。实际使用中这个公式的调参相对敏感因为误差项没有进入状态控制器对参考变化的跟踪速度主要靠调大Q矩阵对应输出行的权重来实现权重一大就容易引发控制量抖动。2.2 公式二输出跟踪误差增广形式为了让控制器更直接地“看到”输出误差在第二种公式里我把误差写进了状态。定义一个输出误差状态e(k) y(k) - r(k)然后构造新状态z(k) [delta_x(k); e(k)]其中delta_x(k) x(k) - x(k-1)。推导后的增广方程里系统矩阵会包含一个分块[A 0; CA I]的结构输入矩阵是[B; CB]同时参考差分项delta_r(k1) r(k1) - r(k)会出现在方程的外生项中。注意这里出现了一个delta_r(k1)它是由参考输入的差分引入的。这说明这种公式在参考轨迹变化时系统对参考的响应会被迫进入模型相当于一条前馈通道。从效果上看第二种公式的稳态精度理论上更高因为误差本身就在状态里闭环系统对常数参考信号拥有固有的积分补偿。代价是增广维度多了一维输出误差状态而且如果参考信号频繁变化delta_r项的存在会让预测方程多一个已知的外部输入项在MPC的预测模型里需要当成额外的输入来处理代码上要多约一个矩阵。2.3 公式三完整的速度形式第三种速度形式是业界用得最广的一种逻辑上就是“把状态增量和输入增量都写进增广状态”。构造方式是把输出y、输出历史、输入历史一起放进增广状态很多教材把这种方式称为velocity form。在Matlab实现时我直接根据系统的输入输出维数用分块矩阵把动态方程写出来核心是让输出误差和输入增量同时在目标函数中显式出现。速度形式的推导核心可以总结成三点。第一把输出预测直接作为增广状态的一部分让控制器直接顺着输出轨迹优化第二输入增量delta_u保持为优化变量约束直接写在delta_u上第三目标函数中同时包含输出误差权重Q_e和输入增量权重R_delta两者互相平衡。这三种公式里面速度形式对控制器参数的语义最清晰Q_e大就是要求输出贴紧参考R_delta大就是要求控制动作更平滑。两个权重互相拉扯的物理含义很直观这也是它调参最容易理解的原因。2.4 三种公式的对比结论要点做完整轮仿真后我的结论是没有绝对最优的公式只有适不适合场景的问题。在只有状态反馈、无外部扰动的情况下三种公式的稳态精度都能收敛到一致差异主要体现在瞬态响应和约束处理上。从动态响应看公式一跟随速度适中但超调相对难压公式二因为误差进状态收敛更快但超调也更明显公式三通过两个权重解耦调节既能做到快跟踪又能控制住超调代价是状态维度最大。从代码复杂度看公式一大概只需要把A、B矩阵拼一拼30行内能搞定增广公式二需要处理参考差分的额外项公式三需要同时维护输出和输入两种增广预测矩阵的构造要更细心。我自己的建议是如果只是快速验证算法可行性先用公式一如果要写工程代码或发论文的对比实验用公式三作为标准方案把公式一和公式二作为对比基线。3. Matlab代码实现从离散模型到在线滚动优化3.1 环境准备与基础模型整个实现我用的MATLAB版本是R2023a2019以上版本都够用核心工具箱只有Optimization Toolbox的quadprog函数不需要Model Predictive Control Toolbox。这是有意为之的因为自己手写预测矩阵的过程才是真正理解MPC公式差异的地方如果用工具箱自带的mpc对象那三种公式的实现差异就被封装掉了研究性质就没了。被控对象我用了这样一个二阶离散系统% 被控对象模型离散化之后的状态空间 A [0.8 0.1; -0.2 0.7]; B [0.1; 0.3]; C [1 0]; D 0; Ts 0.1; % 采样周期这个系统是典型的带耦合的二阶系统状态之间互相影响输出只有第一个状态这样能明显看出不同MPC公式在耦合抑制上的能力差异。代码里也可以直接用ss和c2d函数把连续模型离散化但这里直接用离散模型避免变换过程引入额外误差。3.2 预测矩阵F和Phi的构造方法MPC的核心不是求解优化本身而是构造预测模型。给定当前增广状态z(k)未来Np步的预测可以用一个大的矩阵方程统一表达Z_pred F * z(k) Phi * delta_U其中F的分块是[A; A^2; A^3; ...]Phi的分块是下三角托普利茨结构[B 0 0...; AB B 0...; A^2B AB B...]。在Matlab里用两层循环可以很简洁地构造出来。下面给出核心函数这段代码在三种公式下是通用的区别只在于传进来的A_aug、B_aug、C_aug不同function [F, Phi] buildPredictionMatrix(A, B, C, Np) n size(A, 1); % 增广状态维度 m size(B, 2); % 输入维度 p size(C, 1); % 输出维度 F zeros(p * Np, n); Phi zeros(p * Np, m * Np); for i 1:Np Ai A^i; F((i-1)*p1 : i*p, :) C * Ai; for j 1:i Phi((i-1)*p1 : i*p, (j-1)*m1 : j*m) C * A^(i-j) * B; end end end这个函数建议保存成独立m文件后面三种公式反复调用。我自己写第一版时是直接在循环里内联展开的后来发现三种公式的代码互相复制粘贴特别容易改错矩阵维度抽成公共函数之后调试效率明显提高。这是实战中比较重要的工程习惯。3.3 无约束解析解与约束QP求解代码目标函数统一写成最小化J (R_pred - Z_pred) * Q_bar * (R_pred - Z_pred) delta_U * R_bar * delta_U其中Q_bar是输出误差权重矩阵R_bar是输入增量权重矩阵都是分块对角阵维度分别是(pNp x pNp)和(mNc x mNc)。展开后丢掉常数项得到标准二次型J delta_U * H * delta_U 2 * g * delta_U其中H Phi_c * Q_bar * Phi_c R_bar g Phi_c * Q_bar * (F * z(k) - R_pred)这里有个关键细节当控制时域Nc小于预测时域Np时Phi需要截取前m*Nc列记为Phi_c。超出控制时域的输入增量不再变化这一列截断直接影响H矩阵维度和QP规模忘了截断会导致维度不匹配报错。如果是无约束情况最优解直接是delta_U_opt -H \ g。这是最平滑的解法计算量小适合对比实验的快速验证。但要做真正的MPC约束不能少。约束条件分三类输入幅值约束、输入增量约束、输出约束全部写成关于delta_U的线性不等式A_in * delta_U b_in然后直接丢给quadprogoptions optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); delta_U_opt quadprog(H, g, A_in, b_in, [], [], [], [], [], options);注意这里只取delta_U_opt的前m个分量作为当前时刻的控制增量后续序列不执行这是MPC滚动时域控制的核心。3.4 约束矩阵的构造与滚动仿真主循环约束矩阵是手写MPC里最容易写错的部分重点说两个输入增量约束和输入幅值约束。输入增量约束最简单直接对delta_u优化变量加上下限即可% 增量约束du_min delta_u du_max Ain_du [eye(m*Nc); -eye(m*Nc)]; bin_du [repmat(du_max, Nc, 1); -repmat(du_min, Nc, 1)];输入幅值约束稍微绕一点因为待优化量是增量不是绝对值。第i步的输入需要累加u(ki) u(k-1) sum_{j0}^{i} delta_u(kj)。写成矩阵形式要引入一个下三角累加矩阵% 幅值约束u_min u(ki) u_max T tril(ones(Nc, Nc)); T_aug kron(T, eye(m)); Ain_u [T_aug; -T_aug]; bin_u [repmat(u_max, Nc, 1) - repmat(u_prev, Nc, 1); ... -repmat(u_min, Nc, 1) repmat(u_prev, Nc, 1)]; % 合并 A_in [Ain_du; Ain_u]; b_in [bin_du; bin_u];u_prev是上一时刻输入在在线求解时是已知量。把两种约束合并后整个QP的输入就齐了。整个仿真的主循环结构大致如下x zeros(n, Nsim); u zeros(m, Nsim); delta_u zeros(m, Nsim); r repmat(target, 1, Nsim); x(:, 1) x0; u_prev zeros(m, 1); for k 1:Nsim-1 % 根据当前状态和上一时刻输入构造增广状态 z_now [x(:, k); u_prev]; % 调用QP求解器得到当前时刻的最优增量序列 du_seq solveMpcQP(z_now, r(:, k:kNp), A_in, b_in); delta_u(:, k) du_seq(1:m); % 更新控制量和系统状态 u(:, k) u_prev delta_u(:, k); x(:, k1) A * x(:, k) B * u(:, k); u_prev u(:, k); end运行这个循环时每步的z_now构造方式会随公式不同而略有差异比如公式二需要把参考差分也传进去公式三需要维护历史输出的缓存。这就是三种公式实现差异的核心位置我在代码注释里用三段分支标明“公式一变这里”“公式二变这里”“公式三变这里”方便对比修改。4. 调试实录常见报错、数值坑与调参经验4.1 最容易踩的3个坑第一个坑维度不匹配。这是手写MPC最常见的报错来源。A_aug的维度是(nm)x(nm)C_aug是p x (nm)如果一不小心C_aug写成了p x (n1)后面所有矩阵全乱套。我的排查经验是在任何一步矩阵运算前加一行防御性检查assert(size(A_aug, 1) size(A_aug, 2), 增广矩阵非方阵)能第一时间定位错误。第二个坑quadprog报“quadratic term is not symmetric”。出现这个报错的原因通常是H矩阵在数值上不对称。MPC里H Phi * Q_bar * Phi R_bar理论上是严格对称正定的但如果Q_bar和R_bar在定义时用了稀疏矩阵而Phi是稠密矩阵两者乘积会有浮点误差累计。解决办法很简单在求解前加一行H (H H)/2强制对称一劳永逸。第三个坑参考轨迹长度和预测时域不匹配。仿真里我习惯一次性定义好全部参考矩阵但MPC每一步的R_pred应该取的是当前时刻往后数Np个点。如果索引越界程序直接报错或者更隐蔽的是取了错误的时间窗口导致预测和实际对不上。这个问题在变参考跟踪的仿真里特别容易出现最后我是用一个辅助函数专门计算r_pred r(:, k:kNp-1)统一管理索引。4.2 参数调参对照表我把三种公式在相同被控对象下的参数敏感度做了对照整理成表参数公式一公式二公式三预测时域Np增大跟踪变慢、稳定性提高响应更平滑约束处理更柔和输出权重Q增大易抖动、响应快超调增加灵敏度提高输入增量权重R增大控制变缓、偏差增大过渡时间变长动态迟缓但更稳采样时间减小计算量增大、精度提高同类提升高增益风险增加从我的实测看三种公式的最优参数范围差异不小。公式一在Q10时就开始出现高频抖动但公式三在同样的Q值下依然稳定原因就是公式三的积分通道分散了误差压力。4.3 三种公式的仿真对比结果最后给出一次典型的对比结果数据。被控对象不变参考目标从1.0阶跃到2.0Np15控制时域Nc5Q和R在所有公式下取相同基准值。公式一调整时间为16个采样周期超调量约8%输入增量峰值0.42。公式二调整时间11个采样周期超调量约14%输入增量峰值0.51。公式三调整时间13个采样周期超调量约6%输入增量峰值0.38。这个结果和理论预期基本吻合误差进状态的公式瞬态响应更快但超调也大完整速度形式通过额外权重维度在动态质量和控制动作之间找到了更平衡的工作点。这个对比实验跑完后建议把三个增益矩阵打印出来看看非零元素分布观察每步优化求解的各项量级会很有启发。5. 个人体会与扩展建议这套对比代码我前后折腾了一周有一次半夜排查Phi矩阵某一行的数值问题最后发现是C矩阵的维度写错了气得人想摔键盘。但说真的手写一遍MPC预测矩阵远比直接调用工具箱更能建立对MPC公式本质的直觉。如果后续你想把这个研究继续延伸我有三个方向可以建议。方向一是把连续模型导入用c2d做离散化之后再跑同样的对比观察采样时间选择对三种公式的影响差异。方向二是在被控对象上加入未建模的动态或确定性扰动验证增量形式MPC的积分抗扰能力是否真能压住稳态静差。方向三是把优化求解器换掉从内点法改为OSQP或者嵌入式求解器为将来部署到单片机或实时控制器预留接口。我自己下一步打算做的是把三种公式的闭环回路建立成统一的接口然后在同一个优化求解器、同一组参数下自动跑参数扫描生成性能对比图。这样整个研究就变成了一个真正可复用的实验框架后续换被控对象、换约束条件只需要改最外层的模型定义就够用了。