Koopman算子结合MPC：非线性系统预测控制的Matlab实现

做控制的同行应该都有同感：非线性系统上MPC，理论上很漂亮，落地却很容易卡壳。每个采样周期都要在线解一个非线性优化问题，模型稍复杂一点，求解器就未必能在周期内收敛，更别说还要应付输入约束和状态约束。我前几个月在调一个强非线性对象时被这个问题折磨得够呛，后来把Koopman算子结合MPC的方案完整撸了一遍，才真正体会到这里面的门道。这篇文章就从Koopman线性预测器的原理讲起，给出可运行的Matlab实现框架，全程记录我在字典函数选择、EDMD训练、QP求解和闭环仿真中踩过的坑，适合正在做非线性预测控制或者对Koopman方法感兴趣的朋友参考。

1. 核心逻辑拆解：为什么Koopman能让MPC变简单

1.1 非线性MPC的真正瓶颈在哪里

模型预测控制的核心，说白了就是一个带约束的滚动优化问题：在当前时刻拿到被控对象的状态之后，基于预测模型算出未来N步的状态轨迹，同时解出一串最优输入序列，使跟踪误差和输入能量加权的目标函数最小，最后只执行第一个控制量，下一个采样周期重新来一遍。

这里最耗时的就是“基于预测模型”这一步。线性模型的预测轨迹可以直接写成初始状态和输入序列的线性组合，优化问题是一个标准的二次规划（QP），成熟求解器几毫秒内就能出结果。但换成非线性模型，预测轨迹是输入的非线性函数，目标函数和约束都非凸，每个采样周期都要做多轮迭代，迭代里还要反复评估非线性函数和雅可比矩阵，计算量成倍上升，求解时间变得完全不可控。

对于像Duffing振荡器这类带强非线性项的系统，还有一个更隐蔽的问题：预测时域一长，数值误差和非线性效应会把轨迹越推越偏，梯度信息也变得不可靠，控制效果明显劣化。所以我说非线性MPC的真正瓶颈不在控制理论本身，而在“在线优化的实时性”和“预测模型的可靠性”这两个工程矛盾上。Koopman算子方法恰好同时从这两头切入，才让非线性MPC有了真正能落地到实时系统上的可能。

1.2 Koopman算子的直觉：把非线性藏进高维线性空间

Koopman算子的思想初次接触会觉得有点绕，但直觉其实很直白。我们平时看到的非线性系统x_{k+1} = F(x_k)，状态x自己在低维空间里按非线性规则运动。Koopman的视角是：别直接盯着状态看，而是盯着状态上的“观测函数”看。定义一个标量函数g(x)，Koopman算子K的作用是把这个函数搬移一步，变成 (Kg)(x) = g(F(x))。

这个抽象算子有一个关键性质：因为函数空间是线性空间，搬移操作对函数加法是线性的，所以K是一个线性算子。换句话说，低维状态x的运动虽然是非线性的，但如果我们挑一批足够丰富的观测函数，把它们堆成一个高维向量 z = ψ(x)，那么在这组高维坐标下，系统的演化可以近似写成 z_{k+1} ≈ K z_k，K就是一个普通矩阵。

我经常用一个比喻：一只小船在水面上划出弯弯曲曲的轨迹，你在二维平面上描述它的运动非常复杂；但如果你坐上直升机升高视角，复杂轨迹可能变成一条近乎直线的航迹。Koopman算子就是那个“升高视角”的操作，代价是状态维度变大了。所以这个方案本质上是拿维数换线性，而且这个交换在工程上往往是划算的。

1.3 为什么这个思路和MPC是天生一对

Koopman线性预测器带给MPC最直接的好处，是把预测模型变成了线性时不变形式。有了这个线性模型，原来是NLP（非线性规划）的在线优化问题就退化成标准的QP问题。QP有全局最优解、有成熟算法、求解时间可控，可以放心丢进实时环境里跑。

更重要的是，Koopman模型不是传统的小扰动线性化。小扰动线性化只在平衡点附近一小块区域有效，离开工作点就废了。而Koopman模型是在指定的工作域上用数据拟合出来的“全局化的线性表示”，只要训练数据覆盖到位，它在整个工作域内都能保持较好的预测精度。做状态估计和预测控制时，最怕的就是模型在远离标称点的地方突然失效，Koopman思路给了我们一条实际可走的缓解路径。

2. 关键技术路线：从数据到线性预测器

2.1 完整流程的五步拆解

要把Koopman MPC跑通，在我看下来核心链路是五步，缺一环都容易出问题。第一步是设计激励实验，让系统在目标工作域内充分运动，采集状态和输入数据；第二步是挑选字典函数，也就是确定高维映射ψ；第三步用EDMD算法从数据里拟合出Koopman矩阵K和输入矩阵B；第四步确定状态恢复矩阵C，因为MPC最终还是要拿真实状态去做代价函数和约束；第五步把Koopman模型写成标准线性模型，嵌进MPC的QP求解框架。

很多人上手时容易直接在第三步猛调算法，忽略了第一步和第二步。实际上对方案成败影响最大的反而是数据覆盖度和字典函数选取。数据没覆盖到的地方，Koopman模型就是瞎猜；字典函数选得不匹配系统非线性结构，拟合精度会一直卡在某个水平上不去。我把这几个环节的顺序固定下来，每次换新对象都按这个次序走，至少不会出现方向性错误。

2.2 字典函数怎么选：实操中的取舍

字典函数是Koopman方法里最“艺术”的部分。理论上，Koopman算子要在一个无穷维的函数空间里才是精确线性的，实际做只能截取有限维，所以字典函数其实就是一堆观测函数的集合。最稳妥的做法是把状态本身放进字典，这样状态就能直接从高维空间里恢复出来，再叠加多项式、三角函数、径向基函数或它们的组合。

我个人实测下来，多项式类字典在小到中等非线性强度下最省心。以受控Duffing系统为例，状态是x1、x2，非线性主要是x1的三次方项，那么字典可以取1、x1、x2、x1²、x1x2、x2²、x1³、x1²x2、x1x2²、x2³。其中包含常数项1，是为了给模型一个偏置项的自由度，特别是当系统存在非零平衡点或输入偏移时，效果会更稳。如果系统还有明显的周期性或振荡特性，再加正弦余弦项会有帮助。

这里有个坑：字典并不是越多越好。维度涨上去，Gram矩阵的条件数会恶化，训练数据量不变的情况下，拟合方差会被放大。我一般从“状态本身加最低次多项式”起步，先看状态恢复误差和单步预测误差，再逐步加项。若加项后测试误差没有明显下降，就果断停手。另外，字典函数最好做归一化处理，尤其是x1、x2量纲差异大的时候，不做归一化，数值优化很容易吃亏。

2.3 EDMD算法：最小二乘拟合背后的细节

EDMD（Extended Dynamic Mode Decomposition）是目前估计Koopman矩阵最主流的方法。它的思路很朴素：把数据组织成矩阵，然后用最小二乘去拟合线性关系。设收集的样本对为(x_i, u_i, x_i^+)，其中x_i^+是施加输入u_i之后的下一时刻状态。令Ψ_X为字典函数作用在状态样本上得到的矩阵，Ψ_X^+为对应的下一步字典函数值，U为输入样本矩阵。

受控系统的目标就是找矩阵K和B，使得 Ψ_X^+ ≈ K Ψ_X + B U。把所有数据拼成一个大矩阵W = [Ψ_X; U]，那么待求的就是 M = [K B]，满足 Ψ_X^+ ≈ M W。最小二乘解是 M = Ψ_X^+ W^T (W W^T)^{-1}，在Matlab里可以直接用右除运算符实现：M = Psi_next / W。这一步数值上通常要进行正则化，否则数据共线性一严重，Gram矩阵接近奇异，解出来的K矩阵特征值经常乱飞，后面MPC预测几大步就爆了。

状态恢复矩阵C同样用最小二乘求，满足X ≈ C Ψ_X，即C = X / Psi。有个常见误解是C等于字典函数中状态项对应的那几行，只有当字典里第一个块就是原始状态时，这部分是精确成立的；如果字典里状态经过了非线性变换，C就必须通过拟合得到。我习惯不管哪种情况都显式求一遍C，省得后续恢复状态时出现系统偏差。

3. Matlab实现全流程：可直接复用的框架

3.1 数据采集：激励信号的设计与处理

先定义被控对象。我用一个带三次项的非线性振荡器作为示例：x1' = x1 + dt·x2，x2' = x2 + dt·(-0.5x2 - x1 - 2x1³ + u)。这个系统在原点附近是稳定的，但x1³项在x1偏离0.5以后贡献已经和线性项相当，可以代表一类中等强度的非线性对象。采样周期dt取0.05，离散递推用简单的欧拉法，数据采集阶段可以直接用同一套递推式生成。

激励信号我建议用“多级随机阶跃”而不是白噪声。白噪声的高频成分容易被系统低通特性滤掉，导致数据在相空间里挤成一团，覆盖度很差。多级随机阶跃的做法是：每200个采样步内保持输入的幅值不变，幅值从{-3, -2, -1, 0, 1, 2, 3}里随机抽取，这样系统会在不同输入水平下经历充分过渡过程，相空间轨迹覆盖度明显更好。总数据长度我取3000到5000步，单位置采样后做一下去均值处理，会让后面的回归更稳定。

数据采集的核心代码如下，关键是把输入序列保存下来，和状态轨迹对齐：

matlab复制dt = 0.05;
T = 4000;
u = zeros(1, T);
seg_len = 200;
amp = [-3 -2 -1 0 1 2 3];
for i = 1:ceil(T/seg_len)
    idx_start = (i-1)*seg_len + 1;
    idx_end = min(i*seg_len, T);
    u(idx_start:idx_end) = amp(randi(length(amp)));
end

x = zeros(2, T);
x(:,1) = [0.3; 0.0];
for k = 1:T-1
    x1 = x(1,k); x2 = x(2,k);
    x(1,k+1) = x1 + dt * x2;
    x(2,k+1) = x2 + dt * (-0.5*x2 - x1 - 2*x1^3 + u(k));
end

训练前一定要检查数据的相空间轨迹覆盖范围。我通常在训练前先画一下x1和x2的散点图，确认两个维度上都跑开了。如果发现x1只在0附近小幅波动，说明激励力度不够，要调大输入幅值或加大段长。数据覆盖不到位是后面模型准确性差的头号原因，这个检查步骤不能省。

3.2 Koopman训练函数：EDMD的Matlab实现

字典函数我用匿名函数构造。这里取状态本身、常数项以及到三次的多项式项：

matlab复制basis_func = @(X) [ ones(1, size(X,2));
                    X;
                    X(1,:).^2;
                    X(1,:).*X(2,:);
                    X(2,:).^2;
                    X(1,:).^3;
                    X(1,:).^2 .* X(2,:);
                    X(1,:).*X(2,:).^2;
                    X(2,:).^3 ];

这样字典维度p = 1 + 2 + 3 + 4 = 10。训练函数如下：

matlab复制function [K, B, C] = train_koopman_edmd(X, U, X_next, basis_func, lambda)
    Psi = basis_func(X);
    Psi_next = basis_func(X_next);
    W = [Psi; U];
    G = W * W' + lambda * eye(size(W,1));
    M = Psi_next * W';
    KB = M / G;
    K = KB(:, 1:size(Psi,1));
    B = KB(:, size(Psi,1)+1:end);
    C = X / Psi;
end

lambda是正则化系数，我一般取1e-6到1e-4之间。训练完成后，用一组合成的正弦输入做open-loop预测验证，比较Koopman模型预测轨迹和真实非线性模型轨迹。单步预测误差在1e-2量级、10步内不发散，就说明模型基本合格。这里一定要用训练数据之外的输入序列验证，否则很容易出现“训练集上很漂亮，一换输入就崩”的假象。

3.3 将Koopman线性模型嵌入MPC

有了K、B、C之后，MPC就变成标准的带约束线性二次型问题。预测模型是 z_{k+1} = K z_k + B u_k，其中z0 = psi(x_current)，实际状态恢复为 x_k = C z_k。目标函数取基于真实状态的加权：J = Σ (x_k - x_ref)ᵀ Q (x_k - x_ref) + u_kᵀ R u_k，代入 x_k = C z_k 后可写成关于优化变量（输入序列）的二次型。

我用quadprog来求解，所以需要显式构造Hessian矩阵H和线性项f。思路是：令优化变量y = [u_0; u_1; ...; u_{N-1}]，对预测时域内的每一步k，把z_k表达为初始状态z0的线性响应与输入序列的线性组合，即 z_k = P_k y + q_k，其中q_k是零输入响应，P_k是输入作用矩阵。然后在循环里累加H和f：

matlab复制Np = 30;
p = size(K,1);
m = size(B,2);

Q = diag([10, 1]);
R = 0.1;

H = zeros(m*Np, m*Np);
f = zeros(m*Np, 1);

Pz = zeros(p, m*Np);
qz = z0;

for k = 1:Np
    CtQC = C' * Q * C;
    H = H + Pz' * CtQC * Pz;
    f = f + 2 * Pz' * CtQC * qz;

    % 输入代价
    idx = (k-1)*m + 1 : k*m;
    H(idx, idx) = H(idx, idx) + R * eye(m);

    % 递推下一步 z_{k+1} = K z_k + B u_k
    Pz = K * Pz;
    Pz(:, idx) = Pz(:, idx) + B;
    qz = K * qz;
end

如果目标不是原点而是某个参考点，把qz换成参考响应即可，代码结构不用变。输入约束很简单，直接写成lb和ub向量：lb = -3ones(mNp,1)，ub = 3ones(mNp,1)。如果还要加状态约束，比如限制x1不超过±1.5，就额外构造Aineq和bineq，放入quadprog的约束参数里。

当前时刻解完QP后，取y(1:m)作为当前控制量输出，丢弃其余分量，进入下一个采样周期。这就是MPC最基本的滚动时域策略。

3.4 闭环仿真与性能对比

闭环仿真的框架是：每一拍用真实非线性模型递推一步，再调用MPC求解器。注意这里MPC内部用的是Koopman线性模型，而被控对象是真实非线性模型，这种“模型失配”恰恰是工程上最有价值的验证场景。

matlab复制x_hist = zeros(2, Tsim);
u_hist = zeros(1, Tsim-1);
x = [0.8; 0.2];
for k = 1:Tsim-1
    z0 = basis_func(x);
    u_k = solve_mpc_koopman(z0, K, B, C, Np, Q, R);
    u_hist(k) = u_k;
    x1 = x(1); x2 = x(2);
    x(1) = x1 + dt * x2;
    x(2) = x2 + dt * (-0.5*x2 - x1 - 2*x1^3 + u_k);
    x_hist(:,k+1) = x;
end

我在同一套参数下分别用线性MPC（在工作点线性化得到的状态空间模型）和Koopman MPC做对比，差距一眼就能看出来。线性MPC在初始状态离原点较远时，预测模型偏差大，控制量给得保守，状态回到原点的速度明显更慢，甚至在特定初始状态下会出现小幅发散。Koopman MPC则能在整个预测时域内保持相对准确的预测，控制器敢于在前期给出较大的控制量把状态快速拉回原点附近，再平稳收敛。

这个对比并不说明Koopman方法万能，但它确实证明了“全局近似线性化”比“局部线性化”在MPC里更有工程价值。

4. 参数调优与工程细节

4.1 字典维度与正则化的平衡

字典维度直接决定模型容量。维度太小，模型表达不了非线性动力学，单步预测误差就下不去；维度太大，需要更多数据来抑制过拟合，计算量也涨。我的经验是从简单字典开始，观察单步预测误差和闭环性能，再逐步加容量的策略最省时间。

正则化lambda的调节也是一个细节。我常用的办法是看G矩阵的条件数，如果cond(G)大于1e10，就明显有共线性问题，需要在1e-6到1e-2之间尝试几个量级，挑一个让验证集误差最小的值。正则化过大会引入偏差，K矩阵的特征值会整体向单位圆内收缩，导致预测偏保守；过小则数值噪声被放大，模型对训练集外的工况很敏感。

4.2 状态恢复精度对控制效果的影响

Koopman模型的预测是在提升空间里做的，但MPC代价函数和状态约束都建立在真实状态上，所以状态恢复矩阵C的精度会直接影响控制品质。如果C的拟合误差大，最优控制量就会偏向错误的“目标状态”，整个闭环出现静差甚至失稳的现象都可能出现。

我建议在训练完K和B之后，专门拿一组测试数据算一下状态恢复误差，指标上看相对误差均值是否小于3%。如果恢复误差超标，多半是字典里缺了关键状态分量。最保险的做法是把原始状态作为字典的前几项，这样理论上C的前几行就是单位阵的一部分，恢复精度天然有保障。实际用EDMD做最小二乘时我还是会重新求C，让它在数据上做一次最优补偿，能吸收一部分数值误差。

4.3 采样时间与模型有效域的把握

采样时间dt对Koopman模型的影响非常隐蔽。dt取得太大，离散化误差会直接把非线性模型的结构扭曲，EDMD拟合出来的K矩阵特征值偏离真实连续系统；dt取得太小，相邻状态之间的差异过小，信噪比下降，EDMD对噪声的敏感度上升。我一般以被控对象主导时间常数的1/10到1/20为起始值，再用单步预测误差去微调。

模型有效域本质上是训练数据的覆盖范围，Koopman模型只在数据覆盖的区域内可信。在MPC闭环里，如果控制量把状态推到训练域之外，预测会迅速失真。所以我在训练数据生成时，会有意识地把激励信号幅度开到MPC允许的输入上限附近，让数据覆盖整个可能到达的相空间区域。如果事后发现闭环轨迹超出训练域，就得重新生成数据、扩大激励范围，而不是硬调控制器参数。

5. 常见问题与排查技巧实录

5.1 模型发散或预测不准怎么办

我遇到最多的现象是：训练完的Koopman模型在open-loop验证时，前几步匹配很好，到后面轨迹越来越偏，最后直接发散。排查顺序一般这样：先看K矩阵的特征值是否全部在单位圆内。如果数据是稳定的被控对象，而K存在模大于1.05的特征值，大概率是数据覆盖不足或正则化不够，导致过拟合。可以试着增大lambda，或者增加训练数据的覆盖度。

还有一种常见情况是字典里没有常数项。系统如果存在非零平衡点，而字典没给常数项，零输入预测就会漂移。加入常数项后往往立竿见影。另外，数据里的输入序列如果含有明显的均值分量，EDMD拟合出来的B矩阵会吸收这部分偏置，导致开环验证时换一组均值为零的输入就出偏差。处理办法是让训练输入范围尽量覆盖你关心的工况，不只是在某个均值附近做小幅扰动。

如果上述都排除了，那就要怀疑离散化步长。有个案例我印象很深：把欧拉法换成四阶龙格库塔法之后，同样的训练流程，预测误差直接降了一个量级。所以被控对象仿真精度不够，EDMD再厉害也补不回来。

5.2 MPC求解失败或约束违反

quadprog解QP时偶尔报出“Hessian不是正定”的提示，这是最常见的问题。原因通常是Hessian构建时把输入代价R的块重复加了，或者初始状态距离目标太远导致数值动态范围过大。我一般会检查H矩阵的特征值，确保最小特征值大于一个小的正数。如果发现某步Pz矩阵中有大量接近0的列，说明预测时域和系统可控性可能不匹配。

输入约束被违反的情况也有，但多数不是求解器的问题，而是模型预测误差导致优化器误判了约束边界。一个典型的例子是：QP允许的最大控制量是3，但真实非线性系统在状态x1=0.8时需要的补偿力已经接近2.8，如果预测模型把非线性项低估了0.3，优化器给出的控制量就会不足，实际轨迹冲出约束范围。这种问题要从提高模型精度入手，光靠调QP的约束余量不解决根本问题。

5.3 Koopman MPC与传统非线性MPC的取舍

有工程师问我，既然非线性MPC可以用CasADi这类工具求解，为什么还要费劲做Koopman。我的回答是看场景。如果采样周期在几百毫秒以上、系统维度低、而且已经有很准的非线性机理模型，那直接上非线性MPC更省事，不需要训练数据，也不会有数据覆盖的顾虑。但如果你面对的是采样周期只有几十毫秒的嵌入式控制器，或者模型本身只有输入输出数据、没有精确机理，Koopman MPC的优势就非常明显了。

我在实际项目里用过Koopman MPC之后，最大的感受是它的调试周期短。机理模型需要慢慢校核参数，而Koopman模型只需要一套激励数据和一段不大长的训练代码，迭代起来很快。缺点是对数据质量要求高，而且模型的可解释性弱。遇到安全攸关的场合，我会在Koopman模型外面加一层“预测误差保护”，比如计算单步预测残差，残差超过一个阈值就切换到保守控制模式。这个保护机制成本很低，但能避免很多意外风险。

6. 还需要注意的工程问题

严格说Koopman MPC不是银弹，它更适合那些“非线性中等强度、工作域明确、数据易采集”的对象。如果你的系统本身存在分叉、突变、迟滞这类强非光滑特性，单纯靠多项式字典的Koopman模型很难准确描述，这时候需要更复杂的字典结构，甚至要考虑分段Koopman模型。

另外，把Koopman模型用于状态估计时，要充分利用零输入响应qz这一块。MPC的预测初始状态来自观测器或直接测量，如果观测器输出有噪声，z0的误差会被K矩阵放大，控制量也会抖动。我在处理这类问题时，会在z0送入QP前做一次滑动平均滤波，或者在目标函数里加入输入变化率的惩罚项，效果都非常明显。

经验之谈：刚开始搭建整个流程时，千万不要一上来就追求复杂字典和深度Koopman网络。先用最朴素的EDMD加多项式字典把闭环跑通，搞明白每一步数据流向和维度匹配，再逐步增加复杂度。我自己第一次踩坑就是花了一周调一个很花哨的深度网络，最后发现数据预处理没做好，回头用简版EDMD两天就解决问题了。

如果你准备在自己的项目里用这套方案，我建议先选一个简单的仿真对象完整走一遍本文的流程，记录每个环节的时间消耗和误差指标，形成自己的baseline。后续无论换什么被控对象，都有一个可对比的参照。这套方法真正落地之后，你会发现非线性MPC的实时性难题在大多数场景下没有想象中那么可怕。