状态空间MPC三种增量式公式对比与Matlab实现

做模型预测控制MPC有一段时间了最大的感受是这个领域公式太多经常在状态空间MPC这一个名词下面看到好几种长得完全不一样的写法。有的优化变量直接是控制量 u有的是输入增量 Δu还有的把 u 塞进状态里做增广模型。读文献的时候感觉自己都看懂了真正打开 Matlab 准备写代码才发现连预测矩阵该按哪个公式推都得重新捋一遍。这篇文章把我最近做的一项对比研究完整摊开同样走输入增量Δu这条路线状态空间 MPC 至少有三种不同的公式写法分别是 u(k-1) 扩维的速度形式、误差状态增广形式、参考增量/输出差分形式。我把它们全部在 Matlab 里实现了跑了同一套仿真任务做了对比也把推导过程、代码框架和调试中踩过的坑一起记录下来。如果你正在学 MPC、要写课程设计或者工作中需要做控制方案选型这篇内容能帮你省掉大量推导和调 Bug 的时间读完可以直接照着复现。1. 状态空间MPC为什么要强调输入增量1.1 位置式MPC与增量式MPC的核心差别先花点时间说清楚最基本的事。状态空间 MPC 的核心逻辑是用被控对象的状态空间模型去预测未来若干步的输出再通过求解一个带约束的优化问题得到当前时刻应该施加的控制量。这个控制量怎么定义就分出了两条路线。位置式 MPC 的决策变量就是 u(k)、u(k1)……向量里直接是每一个未来时刻的实际控制量。这样建模很自然但如果模型与实际对象之间有偏差、或者系统有常值扰动比如负载突变位置式 MPC 很容易出现稳态静差就是输出始终偏离目标值一个小量怎么调权重都消不掉。增量式 MPC 就不一样了它优化的不是 u 本身而是 Δu(k) u(k) - u(k-1)即相邻两个采样周期控制量的变化量。最终施加的实际控制量通过累加得到u(k) u(k-1) Δu(k)。这么做有一个非常重要的性质本质上就是给系统引入了一个积分环节——即使模型不精确只要用增量作为决策变量稳态时 Δu 归零、输出会自动收敛到目标值附近静差问题被大大抑制。这也是为什么工程里大多数 MPC 落地都倾向于用增量式而不是位置式。1.2 输入增量到底带来了哪些实际好处除了解消静差输入增量还有几个很实在的好处。第一是约束处理更贴近执行机构。很多执行器阀门、电机驱动器真正关心的是变化速率而不是绝对位置。比如伺服电机有加速度限制如果直接优化 u你只能给它设一个幅值上限很难表达每步最多只能变化多少。而增量式 MPC 对 Δu 加上限幅就天然对应执行机构的速度或加速度约束这是位置式做不到的。第二是方便做软约束和防跳变。增量式 MPC 的 QP 问题里Δu 的二次项权重矩阵可以直接压住控制动作的剧烈程度让输出曲线更平滑。我在实际调试中发现想要抑制控制器输出毛刺加权重 R 比给 u 加限幅更管用因为限幅是硬约束会让优化经常顶在边界上反而引发振荡。第三是系统无静差的能力并不仅依赖于精确模型。位置式 MPC 之所以存在静差本质是模型预测的输出与实际输出之间存在偏差而控制器没有把偏差记下来。增量式因为把前一步控制量当成状态的一部分带入了预测模型相当于是变相记住了一次偏差的积分所以对模型失配和恒定扰动的抵抗力强很多。1.3 关于不同公式的第一层理解目标函数决定形式那为什么同样是输入增量还会有不同的状态空间 MPC 公式关键在于状态向量的选择和参考轨迹的处理方式。因为 Δu 是决策变量但模型 x(k1) Ax(k) Bu(k) 里写的是 u(k)所以必须想办法把 u(k) 表示成 u(k-1) 加 Δu 的累加。最常见的做法是扩维把 u(k-1) 作为一个新状态写进状态向量。但扩维之后你想让优化目标去跟什么、误差放在哪里就会衍生出不同的公式写法。有的把误差放到状态里做误差增广有的把参考轨迹也做差分形式不一样但底层都是同一个物理系统、同一组预测方程。理解了这一点后面看三种公式就不会觉得乱了。2. 三种常见的状态空间MPC公式拆解与推导2.1 公式Au(k-1)扩维的速度形式MPC这是最经典的增量式公式也常叫 velocity form MPC。它把上一时刻的控制量并入状态z(k) [x(k); u(k-1)]然后状态方程变成z(k1) A_z * z(k) B_z * Δu(k)其中A_z [A, B; 0, I]B_z [B; I]C_z [C, 0]注意这里的 A、B、C 是离散状态空间模型的矩阵I 是输入维数的单位阵。预测输出就从 C_z 里取也就是只看 x(k)u(k-1) 只是陪跑。这个公式的好处是思路直白扩维后直接在原有模型上推算预测矩阵即可。QP 的目标函数写成J (R - Y)^T Q_e (R - Y) ΔU^T R_u ΔU其中 Y 是预测输出向量R 是参考轨迹向量Q_e 是输出误差权重矩阵R_u 是 Δu 的权重矩阵。我在推这个公式时踩过一个小坑扩维后的 C_z 很容易写错。有人习惯把 C_z 写成 [C, 0]也有人写成 [0, C]这取决于你把 u(k-1) 放在状态向量的哪一边。只要 z 的定义是 [x; u_prev]输出就得从 x 里取所以是 [C, 0]。2.2 公式B误差状态增广形式MPC公式A处理的是模型本身而误差状态增广形式把输出误差直接加进状态里。定义误差状态 e(k) y(k) - r(k)其中 r(k) 是当前参考值。同时保留 Δu(k) 作为决策变量。状态增广方式可以写成x_aug(k1) [A, 0; C, I] * [x(k); e(k)] [B; 0] * Δu(k)这里第二行表示误差的演化e(k1) e(k) y(k) - r(k)如果 r 保持常值的话。实际推导时更常见的写法是把 y(k1) 拆开得到e(k1) CA x(k) CB Δu(k) e(k) - r(k1)可以看到参考值 r 会以外部输入的形式出现所以在构造 QP 时f 项里需要把参考轨迹的差分项算进去。这个公式的核心思想是让控制器对误差直接做积分理论上跟踪常值给定值时无静差性能更好尤其适合设定值频繁切换的场合。但也要注意一个代价状态变量多了一维误差维预测矩阵的规模也变大计算量会略微上升。对于大多数工控场景这个增长基本无所谓但如果在嵌入式或者小单片机上跑就得多看看资源占用。2.3 公式C参考增量/输出差分形式MPC第三种公式比较特别它不对状态做复杂增广而是对参考信号和输出差分做文章。思路是这样的既然 Δu 是增量那干脆把目标函数里的输出误差也换成差分形式定义一个输出增量Δy(k) y(k) - y(k-1)这样系统模型变成以 Δx 为状态、Δu 为输入的形式Δx(k1) A Δx(k) B Δu(k) Δy(k) C Δx(k)然后把参考信号的增量 Δr(k) r(k) - r(k-1) 作为目标值目标函数惩罚 (Δy - Δr) 的偏差。这个形式下控制器天然具备无静差地跟踪斜坡信号的能力因为它在目标里本身就把参考的差分考虑进去了。这套公式我在做斜坡给定跟踪的仿真时用过效果很稳。但说实话如果只是跟踪阶跃公式C和公式B差别不大它的优势主要体现在参考变化频繁、甚至带斜坡或平滑过渡的场景。代价是推导略绕对刚入门的朋友不太友好我第一次实现时把 Δr 的维度搞错导致参考轨迹整体偏移了一个相位排查了很久。2.4 三种公式的矩阵对比与选型建议三种公式都是基于输入增量的状态空间 MPC但侧重点不同。为了方便直接对比我把总结列成一张表公式状态定义决策变量主要优势典型场景公式A 速度形式[x; u_prev]Δu结构简单推导直接通用增量式MPC大多数工业对象公式B 误差增广[x; e]Δu积分作用显式静态跟踪精准设定值频繁切换、需要强抗扰公式C 参考增量差分位置或差分状态Δu跟踪斜坡、平滑参考无静差伺服轨迹跟踪、缓变给定选型上我的经验是如果是常规工业控制直接用公式A代码最容易写调参也直观如果给定值是常值但有持续扰动公式B会让你少为静差操心如果要跟斜坡或平滑轨迹公式C明显更合适。其余情况三者差异不大。3. Matlab代码实现从模型离散化到QP求解3.1 被控对象与离散化处理为了对比三种公式我在 Matlab 里用了一个简单的二阶对象做测试。这里选二阶对象是因为它既有动态特性又不会因为阶次太高让预测矩阵推导变得冗长很适合演示。假设连续域对象为G(s) 2 / (s^2 1.2s 2)先用状态空间形式描述再离散化Ac [0 1; -2 -1.2]; Bc [0; 2]; Cc [1 0]; Dc 0; sysc ss(Ac, Bc, Cc, Dc); Ts 0.1; % 采样周期 sysd c2d(sysc, Ts); % 连续转离散 A sysd.A; B sysd.B; C sysd.C; D sysd.D;当然实际工程里被控对象可能是电机、热力过程或化工塔器但代码流程完全一样先拿到离散状态空间模型再根据你选择的 MPC 公式构造预测矩阵。3.2 预测矩阵构造Phi、Gamma、H与f无论是哪种公式预测过程都可以统一写成Y Phi * x_now Gamma * dU这里的 Y 是未来 Np 步的预测输出向量dU 是未来 Nc 步的决策变量向量x_now 是当前状态扩维后的状态。Phi 和 Gamma 就是从状态方程递推出来的预测矩阵。以公式A为例扩维后状态维数是32个原始状态 1个 u_prev核心代码nx size(A, 1); nu size(B, 2); % 公式A扩维 Az [A B; zeros(nu, nx) eye(nu)]; Bz [B; eye(nu)]; Cz [C zeros(size(C,1), nu)];然后构造预测矩阵Np 10; % 预测时域 Nc 3; % 控制时域 Phi zeros(Np, size(Az, 1)); Gamma zeros(Np, Nc); for i 1:Np Phi(i, :) Cz * Az^i; for j 1:min(i, Nc) Gamma(i, j) Cz * Az^(i-j) * Bz; end end这段代码需要注意Az^i 表示状态预测中的迭代幂次实际仿真时用矩阵乘法和循环也可以实现但幂次写法在 Matlab 里最清晰。构造完成后QP 问题就变成H Gamma * Qe * Gamma Ru * eye(Nc); f Gamma * Qe * (Phi * z_now - ref);其中 Qe 是输出误差权重矩阵Ru 是 Δu 的权重系数。ref 是 Np×1 的参考轨迹向量。如果是公式B和公式C只需要改 Az、Bz、Cz 的定义以及 ref 的生成方式其他框架完全复用。这也是我坚持把这三种公式统到一个代码框架里的原因——一份主脚本切换公式只需要换几个矩阵和一段参考构造逻辑。3.3 带约束QP问题的quadprog求解与滚动优化主循环MPC 的优化问题必须带约束才有意义不然本质上就是 LQR。常见约束包括Δu 的上下限、实际控制量 u 的上下限、有时还有输出 y 的上下限。用 quadprog 求解时决策变量是 Nc 维的 dU。Δu 的上下限可以直接写成 lb 和 ublb -0.5 * ones(Nc, 1); ub 0.5 * ones(Nc, 1);但由于 u(k) u_last cumsum(dU(1:k))对 u 的约束不能直接写成 lb/ub需要转化为线性不等式 A_ineq * dU b_ineq。例如要求每个时刻的控制量不超过 umax% u(k) u_last 前k个dU之和 A_ineq tril(ones(Nc, Nc)); b_ineq umax - u_last; % 这是逐时刻上界的向量 A_ineq_all [-A_ineq; A_ineq]; b_ineq_all [u_last - umin; umax - u_last];然后把所有约束传给 quadprogoptions optimoptions(quadprog, Display, off); dU quadprog(H, f, A_ineq_all, b_ineq_all, [], [], lb, ub, [], options);滚动优化的主循环就简单了每次只取 dU 的第一个元素更新实际控制量并施加到被控对象上u_applied u_last dU(1); u_last u_applied; x_next A * x_now B * u_applied;然后令下一时刻的状态等于 x_next继续循环。这里有一个容易忽略的点dU(1) 和后续的 dU(2)、dU(3) 在下一周期都要重新计算所以 MPC 每一拍都要重新求解 QP而不是把第一次算出的 Nc 步全部执行掉。这也是滚动优化名字的由来。3.4 三种公式如何复用同一套求解框架我把三种公式全部包装成函数返回 Az、Bz、Cz 以及参考轨迹构造逻辑这样主程序完全一致。对公式A状态是 [x; u_prev]对公式B状态是 [x; e]对公式C状态是差分量。实际切换只需要调用不同函数而已。这个做法的好处很明显对比时大家用同一套求解器、同一组参数差异只来自公式本身不会因为代码实现方式不同带来额外偏差。建议你也按照这个思路组织代码——先统一求解框架再去研究每种公式的特殊处理会少很多莫名其妙的对不齐。4. 仿真对比三种公式在相同任务下的表现4.1 测试场景与评价指标我做了两组对比实验。第一组是单位阶跃跟踪给定值从 0 阶跃到 1系统初始状态为 0考察三种公式的上升时间、超调和稳态误差。第二组在模型失配条件下做也就是把仿真用的对象模型里的参数改掉比如把对象增益从 2 改成 1.8但控制器仍然用原来的模型做预测考察谁还能保持无静差。评价指标选了四个上升时间、超调量、控制量总能量衡量执行机构负担、稳态误差。4.2 阶跃跟踪与无静差能力对比在理想模型条件下三种公式的阶跃响应其实差别不大都能在 2~3 秒内进入稳态且稳态误差都在百万分之一量级差别主要来自数值误差。这也是很多新手一开始看不出它们区别的原因——理想仿真环境太平静了公式 A、B、C 最后都会收敛到同一个设定值。差别主要体现在过渡过程中。公式A的响应稍微活泼一点因为它的积分作用藏在 u_prev 里对误差的累积作用不是那么直接。公式B的误差增广让积分作用显式过渡过程更稳超调略小。公式C因为目标里用的是 Δr 而不是 r阶跃时的表现更接近一个带前馈的控制器上升速度稍微快一点但控制量也会更用力。4.3 模型失配和噪声下的鲁棒性对比第二组实验才是关键。当我把仿真对象增益改成 1.8 时位置式 MPC 的稳态输出停在 0.9 左右死活上不去——这就是静差。而三种增量式公式的稳态误差都在 0.005 以内说明输入增量的抗模型失配能力确实不是玄学是靠结构保证的。再加测量噪声之后公式A和公式B的输出曲线会轻微抖动公式C因为输出做了差分高频噪声会被进一步放大表现差一些。这个现象让我很意外因为公式C在跟踪上更强但对噪声会更敏感。所以如果你现场测量噪声大选公式C要谨慎可能得在 QP 里把 Δu 权重调大或者先对测量信号做滤波。4.4 关键参数Np、Nc、权重的影响规律参数调优上我试了一圈把规律总结成三条经验预测时域 Np 要看得够远。Np 太小控制器像近视眼约束处理也容易违反。比如 Np3 时模型只预测 0.3 秒约束基本没起到作用输出会有明显超调Np 拉到 10 以上响应就平稳了再往上加收益递减反而增加计算量。控制时域 Nc 别设太大。对二阶这种相对简单的对象Nc3 就够用了设成 Np 一样大不仅拖慢求解还容易出现加权矩阵奇异。Nc 本质上是决定控制决策自由度的自由度太多时 QP 解出来的控制序列更容易剧烈变化。权重 Q 和 R 是调性格的关键。Q 大 R 小控制器激进响应快但控制量抖动大Q 小 R 大控制器保守输出平滑但到达稳态慢。我习惯先用 Q10、R1 起步观察响应曲线再按输出超调了优先降 Q控制量抖了优先升 R的顺序调整。5. 实操中遇到的坑与排查技巧实录5.1 约束矩阵构造错误导致的控制量跳变我最开始做增量式 MPC 时只给 Δu 加了限幅没有对 u 加约束。结果仿真里控制量 u 慢慢漂到了一个非常大但物理上完全不可能的值。原因是 Δu 的约束是每步变化量的限幅你允许它每步变 0.5但如果你不限制 u 的范围经过 100 步累积它能飞到 50。执行机构早就饱和了控制器却毫不知情。所以增量式 MPC 一定不能忘了把 u 的上下界折算成 Δu 的累积不等式。上面代码里的 A_ineq_all 段就是干这个的看起来简单但要特别注意 b_ineq 里要用 u_last也就是当前时刻的实际控制量而不是初始值或者 0。5.2 初始状态处理不当引起的首个控制周期异常运行仿真时我注意到在第一个控制周期控制量经常直接顶满限幅输出还会反冲一下。排查后发现是初始的 u_prev 没设对。比如对象从零开始很多人会顺手把 u_prev 设成 0这没错但如果你的参考值是 1那么第一个周期就需要一个很大的 Δu 才能让控制量冲到能推动输出的水平于是直接撞上限。更合理的做法是让 u_prev 等于当前稳态工作点附近的值比如先做一个开环预仿真记录对象在目标附近的稳态输入。初始状态处理好首个周期的控制量就不会突兀。5.3 quadprog求解失败与数值病态问题用 quadprog 求解时最常见的报错是 H must be positive definite 或者 Unbounded solution。我绝大部分情况都是 H 矩阵构造有问题。H Gamma * Qe * Gamma Ru * eye(Nc)如果 Ru 设得特别小比如 1e-10加上 Gamma 可能存在秩亏H 就会病态。解决办法有两个一是给 H 加一个很小的单位阵扰动即 H H 1e-8 * eye(Nc)这是工程上常见的正则化做法二是检查 Nc 是否设得过大控制自由度过剩也会让 Hessian 矩阵条件数变差。5.4 模型失配下的静差问题与积分作用权衡虽然增量式 MPC 对模型失配有很好的抵抗力但它不是万能的。在第 4 组的失配实验中当我把模型增益误差拉到 50% 时三种公式的稳态误差都开始变大只是没有位置式那么夸张。这说明增量式的积分作用是缓释而不是消除模型误差对大幅失配还是力不从心。另一个值得注意的地方是增量式 MPC 的强积分作用有时会带来过冲式收敛。比如对象有纯滞后时控制器会持续增加控制量直到误差消失结果往往是超调。这时可以在 QP 的目标函数里加入对实际控制量 u 的轻微惩罚只对 u 加权不约束或者减小 Δu 的权重让控制器别那么急。5.5 排查问题的通用套路如果你自己写的 MPC 跑出来效果不对我建议按下面的顺序排查先不约束、不加噪声看理想模型下能不能跟踪上阶跃。如果这一步都不行基本是预测矩阵或扩维矩阵写错了。检查扩维矩阵 Az、Bz、Cz 的维度是否匹配。我最常犯的错就是扩维后 C 矩阵少写一列零。给控制量加约束后观察 u 是否顶在边界上。如果一直顶边界可能是约束不够或者参考目标太激进。加噪声后曲线抖动优先调大 Ru再考虑加测量滤波。对比仿真与实验曲线时务必确认采样周期 Ts 一致MPC 对 Ts 非常敏感。6. 一些经验总结个人向6.1 我的公式选型建议做完整组对比之后我的结论是如果你不是专门做轨迹规划或者面对特别苛刻的跟踪任务直接用公式Au(k-1) 扩维的速度形式 MPC就够了。它是三种里理解成本最低的代码最容易维护而且增量和积分结构已经带来了足够的无静差能力。公式B适合你明确知道系统有持续扰动、且设定值频繁切换的场景它会让你的静差问题更可控。公式C则只在参考轨迹明显带斜坡或平滑曲线时值得考虑——它会给你更好的跟踪效果但也要付出噪声放大的代价。6.2 可以继续扩展的方向这次做的是线性定常系统的状态空间 MPC但同一个代码框架可以扩展到很多方向上。比如把模型换成调度式的线性参数变化模型就可以做线性变参数 MPC把预测模型用一组轨迹线性化可以做简单的非线性 MPC代价是每个周期都要重新计算预测矩阵如果想把理论做得更深还可以引入终端约束来保证闭环稳定性或者用鲁棒不变集来对抗更厉害的模型失配。我个人在实际项目中的体会是MPC 的门槛其实不在公式推导而在于你能不能把公式落到可运行、可调试、可复现的代码上。先把一种公式跑通再横向对比其他公式曲线出来了、问题踩过了这个算法你才算真正掌握。这次三种公式的对比实现对我来说就是把看懂了真正变成了做出来了。如果你照着这篇文章复现大概率也会遇到我上面写的某些坑但只要能定位到公式矩阵和约束变换上问题一般都能解决。