输入增量状态空间MPC公式对比与Matlab代码实现

【MPC】使用输入增量实现了不同的状态空间MPC公式研究Matlab代码实现做MPC的人应该都遇到过这样的问题同样的被控对象同样的控制目标换一种方式写状态空间模型算出来的控制器行为可能差很多。尤其是把控制增量写进优化变量之后整个预测方程的形态、约束表达、甚至QP矩阵的条件数都会发生变化。我最近在Matlab里把几种常见的基于输入增量的状态空间MPC公式逐一实现了对比了它们的结构差异和实际效果这篇文章就来聊聊这套东西怎么搭、怎么调、怎么避坑。这篇内容主要面向已经接触过MPC理论、但还没把各种公式在代码层面理顺的读者。如果你正在写自己的MPC求解器或者想把教科书里的状态空间MPC改成适合嵌入式部署的增量形式这篇文章可以提供一套可复现的Matlab实现思路。文章会从模型设计讲起逐步拆解预测方程、QP构造、求解与闭环验证最后给出我在调试中踩过的坑和解决手段。1. 为什么非要用输入增量形式的状态空间MPC教科书里最常见的MPC形式是把当前状态作为初始条件直接优化未来的控制量序列。这种形式理论推导最简洁但在工程实现中容易暴露出两个问题一是稳态误差难以消除二是约束处理不够直观。引入输入增量之后优化变量变成控制量的变化量从根上改变了问题的性质。1.1 标准状态空间MPC的先天不足先看最原始的形式。假设被控对象为线性离散状态方程x(k1) A x(k) B u(k)y(k) C x(k)MPC的优化目标通常写成J sum_{i1}^{N} || y(ki) - y_ref ||^2_Q sum_{i0}^{N-1} || u(ki) ||^2_R这个形式的决策变量是未来的控制量 u(k), u(k1), ..., u(kN-1)。理论上没问题但在实际系统里模型失配和未建模扰动会导致稳态时状态偏离预测值如果没有积分作用控制器无法消除稳态误差。很多人会通过加入外部扰动估计来补救但这样会把问题复杂化。标准形式的另一个麻烦在于约束处理。实际执行器一般有幅值限幅和变化率限幅幅值限幅可以直接写在 u 上可变化率限幅则需要额外引入上一时刻的控制量 u(k-1)表达起来比较绕。如果直接把增量约束加进去标准形式反而要做变量替换绕一圈。1.2 输入增量形式的本质相当于内置积分器把控制量写成增量形式即定义Δu(k) u(k) - u(k-1)然后将 Δu 作为优化变量。此时预测方程需要重构通常的做法是把状态增广成 [x; u]得到新的状态空间模型[x(k1); u(k)] [A B; 0 I] [x(k); u(k-1)] [B; I] Δu(k)y(k) [C 0] [x(k); u(k-1)]你看这个增广系统对 Δu 来说本质上就是一个积分环节。由于优化变量是增量控制器天然包含积分作用只要闭环稳定稳态误差就会被自动消除。这种特性在工程上非常宝贵因为这意味着不需要额外设计扰动观测器就能获得无静差调节能力。我在实际项目中多次对比过这两种形式的闭环效果。在同样的模型失配条件下增量形式让系统重新跟踪参考输入的能力明显更稳尤其是对象本身包含积分特性或者存在恒定负载扰动时增量形式的优势非常突出。1.3 不同增量公式的核心区别在哪同样是增量形式不同教材或论文给出的具体实现并不相同。我梳理下来主要分歧在于增广状态的选取。最常见的是把 [x; u] 作为新状态但也有只把输出 y 和输入 u 作为增广状态的“输出增量”形式还有直接对原系统做差分、再构造二重积分形式的做法。这些公式看似相近实际预测矩阵的构造、QP决策变量的含义、约束矩阵的系数都有区别。在Matlab中实现时尤其是想要统一接口、方便切换公式时必须把每种形式的维度和逻辑理清楚。下面几个章节我会逐一展开。2. 三种典型增量式状态空间MPC公式的推导与对比我在这套研究中实现了三种典型形式分别命名为“增广状态型”、“差分模型型”和“输出跟踪型”。它们各有适用场景需要结合控制目标来决定用哪种。2.1 公式一增广状态型最常用这种形式就是前面提到过的 [x; u] 增广。连续域或离散域下的原始模型为 A, B, C增广后的系统矩阵为A_aug [A, B; zeros(1,nx), eye(nu)]B_aug [B; eye(nu)]C_aug [C, zeros(ny,nu)]状态维度从 nx 变为 nxnu。决策变量是 Δu 序列。预测方程可以写成X A_aug^k x_aug(0) S_x ΔUY C_aug X其中 S_x 是由 A_aug 和 B_aug 构造的卷积矩阵Y 是未来 N 步输出序列。这种形式的优点是模型直观推导预测矩阵时不必再做额外变换Matlab代码里直接用 kron 和 tril 这类矩阵运算就能快速生成。缺点是增广后状态矩阵A_aug的右下角是单位阵如果原始系统开环不稳定增广系统的特征值会包含单位圆边界上的极点这在数值上会轻微增加预测矩阵的条件数不过实际影响一般可接受。2.2 公式二差分模型型适合稳定系统取原始系统的差分方程即定义Δx(k1) A Δx(k) B Δu(k)此时系统的输入就是 Δu输出为 Δy C Δx。这种形式没有额外增广状态但在预测时需要同时计算 Δx 的初值也就是需要用当前时刻采样值求出 Δx(k) x(k) - x(k-1)。优化完成后实际控制量需要累积u(k) u(k-1) Δu(k)这种公式的预测矩阵维度较小QP求解速度较快。但它有一个明显限制它描述的是系统状态变化量之间的关系无法直接约束输出 y 的绝对值幅值必须有额外机制来恢复 y 的预测。通常做法是把 y 预测也写成递推形式或者在优化目标中加入 y 的跟踪项时需要单独展开 y 的表达式。我在某个快速伺服控制 demo 中用过这种形式对象是一个二阶线性系统模型精度较高。差分模型型跑出来的控制动作比较平滑对噪声有一定抑制作用。但如果模型误差偏大差分放大噪声后预测精度会下降此时不如增广状态型稳。2.3 公式三输出跟踪型适合参考轨迹跟踪第三种做法把注意力放在输出上。定义新的状态为 [x; u; 1] 或者扩展一个参考偏移量以方便在目标函数中直接表示输出跟踪误差。一种常见实现是把参考信号也纳入增广状态形成内模结构。具体来说可以构造如下增广系统[x(k1); u(k); r(k1)] [A, B, 0; 0, I, 0; 0, 0, I] [x(k); u(k-1); r(k)] [B; I; 0] Δu(k)这样在预测时参考信号 r 也作为一个“状态”向前递推目标函数中直接写J sum || y(ki) - r(ki) ||^2好处是对于时变参考轨迹预测阶段就能把未来参考值纳入优化跟踪效果更直接。代价是系统矩阵维度进一步增加并且要求参考信号的未来序列已知至少是可预测的。我在做某轨迹跟踪项目时曾用过这种形式参考轨迹是预先生成的路径点序列预测时直接读取未来的参考点跟踪精度提升明显。但需要额外小心的是如果参考信号跳变剧烈增量形式的积分作用会导致超调此时需要增大 Q 矩阵中对 Δu 的惩罚或者对参考信号做预滤波。2.4 三种公式的对比小结公式类型增广状态维度积分作用输出绝对值约束参考轨迹跟踪适用场景增广状态型nx nu天然有容易实现需额外处理通用场合最省心差分模型型nx天然有较麻烦需自己扩展模型精确、要求快速求解输出跟踪型nx nu nr天然有容易实现直接支持预知参考轨迹的跟踪任务从我的实测经验看如果没有特殊理由优先推荐增广状态型。它公式简洁、约束表达方便、代码后期维护成本低。差分模型型适合想压减计算量的场合但前提是你对模型精度有把握。输出跟踪型则是在参考轨迹可预知时的一个很好增强。3. Matlab代码实现的关键环节光有公式不够还得在Matlab里把整个MPC闭环跑起来。这一节我会分享一段可运行的实现框架并重点拆解预测矩阵构造、QP求解、闭环更新三个核心环节。3.1 预测矩阵批量构造别用循环用分块矩阵很多初学者会用一个 for 循环逐列计算 A^i B这样在 N 不大的时候没问题但一旦 N 超过 20速度就会明显下降。更优雅的做法是直接构造分块下三角矩阵。假设增广状态系统为 A_aug, B_aug预测时域为 N。定义状态预测矩阵 Sx维度 (nxnu)*N 行nxnu 列输入预测矩阵 Su维度 (nxnu)N 行nuN 列。可以用如下方式构造% 假设 A_aug, B_aug 已知 nx_aug size(A_aug, 1); nu size(B_aug, 2); N 10; % 构造状态预测矩阵 Sx zeros(nx_aug * N, nx_aug); for i 1:N Sx((i-1)*nx_aug1 : i*nx_aug, :) A_aug^i; end % 构造输入预测矩阵 Su zeros(nx_aug * N, nu * N); for i 1:N for j 1:i Su((i-1)*nx_aug1 : i*nx_aug, (j-1)*nu1 : j*nu) A_aug^(i-j) * B_aug; end end虽然这里有循环但循环次数等于 N 或 N^2/2远小于预测方程内部的矩阵乘法次数。如果 N 很大比如 50 以上可以用 cellfun 或预先分配矩阵一次性填充。不过对于大多数应用N 在 10 到 30 之间上述代码已经足够快。重点在于后续所有QP构造都可以基于 Sx 和 Su 展开。如果需要输出预测矩阵就在 Su 前面乘以 C_aug 的块对角形式即 Sy kron(eye(N), C_aug) * Su。这样写的好处是直观调试时还能借助矩阵尺寸检查来验证正确性。3.2 目标函数与QP矩阵的组装增量MPC的标准目标函数往往写成J sum_{i1}^{N} || y(ki) - r(ki) ||^2_Q sum_{i0}^{N-1} || Δu(ki) ||^2_R注意第三个术语没有对 u 幅值的二次惩罚而是对 Δu 的惩罚。这样做的好处是控制动作趋势稳定不容易因为参考值突变而剧烈振荡。把预测输出表示为Y Sy * x_aug(k) Sdu * ΔU其中 ΔU 是待优化向量Sdu Sy如果输出就是增广状态的对应行。然后目标函数可以改写为J (Sdu ΔU p - R) Q_bar (Sdu ΔU p - R) ΔU R_bar ΔU这里 Q_bar 和 R_bar 是时域上的块对角权重矩阵p 是自由响应项由当前增广状态决定。展开后QP的标准形式 Hessian 矩阵 H 和线性项 f 为H 2 * (Sdu * Q_bar * Sdu R_bar)f 2 * (p - R) * Q_bar * Sdu注意很多教材中写 H 时丢弃了系数 2因为不影响最优解但在约束存在时系数会影响对偶变量间接影响求解稳定性。我习惯保留 2避免不必要的数值偏差。3.3 约束条件的标准化写法增量MPC的约束通常包括控制幅值约束u_min u(ki) u_max控制增量约束Δu_min Δu(ki) Δu_max输出约束y_min y(ki) y_max控制幅值约束需要写成关于 ΔU 的不等式。因为 u(ki) u(k-1) sum_{j0}^{i} Δu(kj)所以这是一个累积和。可以构造一个下三角系数矩阵 T 使得u_all u_prev_vec T * ΔU其中 u_prev_vec 是 u(k-1) 重复 N 次的列向量T 是下三角全1矩阵对应累加。于是幅值约束变成u_min_vec u_prev_vec T * ΔU u_max_vec增量约束直接作用于 ΔU输出约束则通过预测方程转为 ΔU 的不等式。把这些不等式统一写成 A_cons * ΔU b_cons 的形式就能交给求解器。我在这里遇到过一个典型问题如果直接拼接多个约束时忘记把变量重复维度搞对会导致求解器报错“维度不匹配”或“无可行解”。最常见的错误是构造 T 矩阵时用了 tril(ones(N,N))但没注意变量顺序是按列拼接还是按行拼接。我建议统一按“未来时刻优先”的顺序排列即 ΔU [Δu(k); Δu(k1); ...; Δu(kN-1)]这样 T tril(ones(N,N)) 正好对应。3.4 QP求解器选择与调用示例Matlab 自带的 quadprog 是最稳妥的选择。对于中小规模问题决策变量小于 100quadprog 的内点法和 active-set 方法都能很快求解。如果部署到嵌入式可以改用 OSQP 或自定义的 Hildreth 投影算法。这里给出一个使用 quadprog 的完整核心片段% 设 H, f, A_cons, b_cons, lb, ub 已组装好 options optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); [deltaU_opt, fval, exitflag] quadprog(H, f, A_cons, b_cons, [], [], lb, ub, [], options); % 取第一个控制增量 du deltaU_opt(1:nu); u_next u_prev du;在实际闭环里只使用第一个 Δu然后重新测量状态、重新构造预测矩阵、再次求解。这就是滚动时域控制的基本节奏。特别提醒一下exitflag 一定要检查如果出现负值说明求解失败需要及时切换备份策略比如丢掉约束直接解无约束解或者沿用上一时刻的控制增量。3.5 闭环仿真主循环我习惯把MPC封装成一个函数输入当前状态、上一时刻控制量、参考轨迹输出当前控制量这样闭环仿真就很干净。function u mpc_controller(xk, u_prev, r_seq, params) % 增广状态 xaug [xk; u_prev]; % 构造预测矩阵提前缓存亦可 [Sdu, p] build_prediction_matrix(params, xaug); % 目标函数 H 2 * (Sdu * params.Qbar * Sdu params.Rbar); f 2 * (p - r_seq) * params.Qbar * Sdu; % 约束 [A_cons, b_cons, lb, ub] build_constraints(params, u_prev); % 求解 opts optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); [dU, ~, flag] quadprog(H, f, A_cons, b_cons, [], [], lb, ub, [], opts); if flag 0 dU zeros(params.N * params.nu, 1); end du dU(1:params.nu); u u_prev du; end在主循环里调用时注意每一时刻把仿真模型的状态更新函数与MPC预测模型分开仿真模型可以采用非线性或带扰动的模型而MPC内部则使用名义线性模型这样可以直观看到模型失配下的鲁棒性表现。4. 必须重视的数值问题与参数调节细节代码能跑起来不等于跑得好。增量MPC在实践中有几个容易被忽略的数值问题直接影响控制性能甚至稳定性。4.1 预测矩阵的病态问题当 A_aug 包含单位特征值时增广状态型必然有预测矩阵 Sx 的条件数会随 N 增大而迅速上升。如果 N 过大比如 30 以上quadprog 求解时可能因条件数过大而收敛缓慢甚至报错。这时可以采取两个手段一是对状态矩阵做相似变换使矩阵尺度更均匀二是使用 QP 求解器的缩放选项或者在目标函数中增加小幅的正则项比如把 H 对角线加上 1e-6 的微小正数。这个小技巧在实测中非常有效能明显改善求解稳定性。4.2 增量约束与幅值约束的冲突当同时存在紧的幅值约束和紧的增量约束时问题可能无解。比如执行器只能允许很小的变化量但参考值突然跳变很大这时满足幅值约束的可行域里可能不存在可行增量序列。处理办法是引入约束软化的概念也就是在目标函数中加入约束违反量的惩罚项。具体做法是把约束不等式改为A_cons * ΔU b_cons eps其中 eps 也是优化变量并在目标函数中加入 ||eps||^2 的大权重惩罚。这样保证任何时刻都有可行解代价是约束可能被轻微违反但实际工程中这是可以接受的。我曾在某个位置伺服系统上遇到这种情况负载突变导致输出超调如果硬约束输出幅值QP直接无解控制量跳变。引入软约束后虽然输出短暂越过限制但整体响应平稳多了。从控制效果看温和地违反约束比剧烈抖动更安全。4.3 权重的标定从仿真到实物的映射Q 和 R 的选择没有绝对标准但增量MPC中 R 对应的惩罚对象是 Δu而不是 u因此它的物理含义是“控制动作变化的剧烈程度”。如果系统噪声大应该增大 R让控制动作更平滑如果系统需要快速响应则需要减小 R。我在调试中常用的做法是先把 Q 中对角线设为1R 慢慢从0.1开始往上加观察控制量和执行器寿命。一个经验公式是如果控制量变化率的均方根值超出执行器物理极限的30%说明 R 太小如果系统从阶跃响应开始出现明显的滞后感说明 R 太大。当然工程中也可以先从仿真里用Bode图或阶跃响应找一组初始值再放到实际设备微调。4.4 预测时域与控制时域的匹配增量MPC里预测时域 N 的选择直接决定决策变量数量。N 太小会导致预测信息不足约束处理能力弱N 太大则矩阵庞大实时性下降。通常根据系统开环响应时间来确定一般取系统调节时间的1/10到1/5作为预测步长再取30到50步作为预测时域。对于慢系统N 可以取小一些因为模型精度补贴不影响整体响应对于快系统N 需要适当加大否则约束容易失效。我实测过一个二阶系统采样周期 0.1s调节时间约 5s取 N20 时控制效果已经很好N30 时性能提升有限但计算时间翻倍。所以不需要一味追求大时域要综合实时性考虑。5. 常见问题与排查技巧实录这一节把我在实际调试中遇到的典型问题整理成一个速查表并按场景展开叙述希望能帮你缩短排查时间。5.1 问题一QP提示“无可行解”这是增量MPC最常见的报错。首先检查约束矩阵中是否出现了矛盾的不等式。比如同时约束 u 在 [-1,1] 且 Δu 在 [-0.01, 0.01]而初始 u 已经是 0.95那第一个步长就要求 Δu 至少为 0.05直接冲突。解决办法是初始时将 u 的上限从 0.95 扩大或者将 Δu 约束暂时放宽。另外还要检查参考轨迹是否在输出可行域内。如果参考值本身超出硬约束范围必然无解。此时需要把参考值投影到可行域或者用软约束替代输出硬约束。我习惯在代码里加一个可行性预检先用 linprog 检查约束可行域是否非空。虽然增加了一点计算量但可以避免 quadprog 返回错误信息让人摸不着头脑。5.2 问题二控制量振荡发散闭环中出现震荡常见原因有三个。第一是 Q/R 权重不合适R 太小导致增量变化过大。第二是预测模型与实际模型失配严重特别是对象增益方向相反时。第三是求解器在相邻时刻给出的最优解出现跳变这与约束活跃状态切换有关。此时可以尝试在目标函数中加入关于 Δu 二阶差分的惩罚也就是增加控制量变化率的平滑项。实际中我常用一个“控制量限幅滤波”来做最后一道防线求出 u_next 后与 u_prev 比较如果变化量超过允许值则按最大值截断。这个操作虽然理论上破坏了最优性但能有效保护执行器在很多工业场合反而是必须的。5.3 问题三稳态误差无法消除如果你用的不是增量形式遇到这个问题很常见。但增量形式不应该出现静态误差。如果仍存在检查增广状态更新时 u(k-1) 是否被正确反馈。我遇到过一种情况在计算当前增广状态时把 u_prev 误写成 u_opt即上一次优化得到但未实际执行的值导致模型输入与真实输入不一致最终形成偏差。正确做法是使用实际作用于执行器的上一时刻控制量而不是上一次求解器的建议值。此外如果被控对象本身包含积分器而模型里又没有准确描述增量形式也可能出现误差。这时候需要重新辨识模型把积分环节纳入。5.4 问题四计算时间过长决策变量数量为 nu*N当 nu 和 N 都偏大时quadprog 的内点法会明显变慢。解决办法有三个方向一是缩短预测时域 N二是使用活动集法active-set替代内点法它在约束较少时更快三是引入输入参数化比如将控制增量序列用少量的基函数表示降低决策变量维度。第三种方法在预测控制里叫“控制参数化”适合嵌入式环境。实测中一个 MIMO 系统nu3, N30决策变量90个quadprog 求解一次约5ms在Matlab实时仿真中完全够用。但如果放到 PLC 或单片机里需要仔细评估。6. 从代码到项目的最后一步接口设计与模块化写MPC代码最大的痛点不是算法本身而是代码结构混乱导致无法快速切换公式、调试约束。我强烈建议把MPC控制器封装成独立的类或结构体内部只暴露必要接口。6.1 定义参数结构体在Matlab中我习惯用一个 params 结构体保存所有模型参数和权重设置params.A A; params.B B; params.C C; params.N 20; % 预测时域 params.Q 1; % 输出权重 params.R 0.1; % 增量权重 params.u_min -1; params.u_max 1; params.du_min -0.05; params.du_max 0.05; params.y_min -inf; params.y_max inf;这样的好处是切换不同被控对象时只需修改 A,B,C切换不同MPC公式时只需修改控制器内部的核心函数其余代码完全复用。6.2 多公式切换的接口设计我在这套研究中设计了三个内部函数build_augmented_system、build_difference_system、build_tracking_system。它们接受同样的原始 A,B,C返回各自的增广矩阵。外层控制器则根据一个枚举参数选择调用哪个构造函数。这样对比不同公式时主仿真脚本完全不需要改动。这种模块化设计让我在对比增广状态型和差分模型型时省了大量时间。只要保证每个构造函数返回的接口矩阵一致后续的QP组装、约束构建、闭环更新全都可以复用。6.3 与仿真环境的对接最后建议把MPC控制器函数与仿真模型解耦。仿真模型可以是非线性的比如加入饱和、死区、时变参数而MPC内部始终使用名义线性模型。这样能真实检验MPC的鲁棒性。我在仿真中加入过20%的模型增益误差增广状态型依然能保持稳定并消除稳态误差差分模型型的响应虽然略慢但也没有发散。这说明增量MPC对中等程度的模型失配还是很有韧性的。写到这里关于增量状态空间MPC的实现方法基本讲完了。最后再分享一个我踩过好几次的坑不要忽略 u(k-1) 的初始值。在仿真或者上电启动时如果 u_prev 初始化为0而执行器实际偏置不为0MPC第一拍就会产生一个很大的 Δu 试图补偿。正确做法是将 u_prev 初始化为当前执行器的实际输出值或者在启动时先执行一个手动控制周期让状态稳定后再切入MPC。这个细节看起来不起眼但往往决定了系统能不能平稳上电。 另外如果你准备把这套代码从Matlab移植到其他环境记得提前测试求解器在不同数值尺度下的表现。增量MPC的QP矩阵中H 的元素可能跨越多个数量级必要时使用对角预处理来降低条件数。我一般在 H 的基础上左乘右乘一个缩放矩阵能让求解速度提升30%以上。 希望这篇文章能帮你把各种状态空间MPC公式从纸面落到代码里。如果只记住一句话那就是优先选增广状态型建模稳、约束方便、调试省心想要速度再考虑差分模型型参考轨迹已知时输出跟踪型是一个值得加进工具箱的选项。