Koopman算子与线性预测器：让MPC摆脱非线性优化困扰

1. 为什么我建议非线性控制先别急着上NMPC，先看看Koopman

这两年做受控非线性动力系统的控制，我踩过不少坑。最开始为了方便，所有控制器一律用非线性模型预测控制NMPC来写：建立非线性预测模型，在每个控制周期里调用fmincon或者sqp在线求解一个非凸优化问题。单看效果确实不差，但工程落地时很头疼——算力需求抖得厉害，收敛速度不可控，实时系统里动不动“这拍没解出来，上一拍的控制量先顶着”。

后来认真试了标题里的这条技术路线：Koopman算子配合线性预测器，再外接MPC。简单说就是先把非线性动力学通过可观测函数提升到一个高维空间，在高维空间里让状态转移关系尽量线性化，然后用一个线性预测模型去驱动模型预测控制。于是原来那个每周期都要做的非线性优化，被改造成了一个二次规划QP问题，直接在Matlab里用quadprog就能很快解出来。项目关键词里的“状态估计”、“线性预测器”、“Koopman”、“MPC”、“Matlab”，刚好对应了这套流程里最关键的五个环节。

这套方案尤其适合以下场景：不想依赖局部线性化模型、想保留非线性动态的大范围特性，同时又希望在线优化是凸的、实时可控的。做倒立摆、Duffing振荡器、机器人运动规划这类强非线性系统控制时，非常值得试一次。

2. Koopman算子为什么能把非线性系统变成线性预测器

2.1 从可观测函数空间看动力学推进

Koopman算子思想一开始并不是用来做控制的，它是动力系统分析里的一个概念。它的核心点是：与其观察状态向量本身怎么变，不如观察状态向量的函数怎么变。

假设原始离散时间系统是

code复制x(k+1) = f(x(k), u(k))

如果另取一组函数

code复制ψ(x) = [ψ1(x), ψ2(x), ..., ψD(x)]^T

那么Koopman算子研究的是这组函数的“转移关系”。理想情况下，存在一个线性算子K，使得

code复制ψ(x(k+1)) = K ψ(x(k))

这其实是对“沿轨线推进”这动作做了线性化，只是线性化发生在一个无限维函数空间里。理论上说，如果ψ取无穷多个函数，那这个等式可以精确成立。实际工程里当然不能取无穷维，只能取一组数量有限的字典函数，把无限维的算子截断成一个有限维矩阵A。这类方法用观测数据训练矩阵K的方式，一般被称为EDMD或DMD的扩展版，本质上是数据驱动最小二乘。

你不需要把这个问题想得太玄。当你面对一个非线性系统，原来的状态x往前进了一步后，x与x²之间会出现完全非线性的耦合；但如果你把x²也当成一个新的状态变量，让它跟x一起参与递推，那么某些非线性关系就可能被写进新的线性递推方程里。Koopman的提升思想就是系统化地做这件事：把非线性动力学藏进高维状态空间，让表面运动尽量线性。

2.2 加入控制输入，别把u也一起“提升”掉

上面的表达式只处理了无控制情形。对受控动力系统，最直觉的想法是把状态和控制输入合并成一个新的广义状态：

code复制χ = [x; u]

然后对χ做提升，把u也放进字典里一起拟合。这样做其实存在一个常见的工程坑：当你把控制量u也当成需要做线性转移的状态，后续的MPC问题里很可能出现“上一个时刻的控制输入变成了当前时刻模型里的一个初始条件”这种耦合，让问题变得别扭。

我实际项目里更喜欢显式地把u保留成线性输入项。也就是把提升后的状态记为

code复制z = ψ(x)

预测器写成

code复制z(k+1) ≈ A z(k) + B u(k)

其中A是提升空间里的动力学矩阵，B是输入矩阵。这样做的理由是：MPC约束通常直接加在u上，如果u还参与自身提升，那么约束条件很可能变成一个双线性项，在线优化的凸性就被破坏了。反过来，只要把输入项保留成线性通道，整个预测模型对z和u都是线性的，后面做MPC时只需要组装一个二次规划。

2.3 提升空间的字典怎么选

这一步是整个流程里最影响模型质量的地方。字典函数选得好不好，直接决定这个“线性预测器”能覆盖多大范围。我从常用做法里整理了一张表，供实践时对照：

我自己用的频率最高的是“混合字典”：前几维固定保留原始状态x，后面接多项式项与少量径向基项，这样既能直接读取原状态做代价函数，又有提升项捕捉非线性耦合。需要提醒的是，初始状态要留在前几维，否则MPC的参考轨迹和约束处理很容易在提升空间里绕晕。

3. 从数据中拟合预测器：EDMD与Matlab实现要点

3.1 数据激励：不是随便采一段数据就能用

使用EDMD拟合矩阵A、B需要一个前提：实验数据要能覆盖你想让预测器工作的区域。这里有一个频繁出现的问题——直接在稳定平衡点附近采集数据，系统的运动范围太窄，非线性特质没有被充分激发，最后拟合出来的线性预测器虽然在那个局部点很准，但稍微远离一点就预测翻车。

我常用来激励系统的信号是叠加随机序列与多频正弦信号。对双输入的Duffing系统而言，状态上有硬弹簧一样的立方非线性，控制上也有耦合，我通常会在输入位置加一组“零均值随机数 + 多频正弦”，保证状态x能在几个势阱之间来回穿梭。这一段数据覆盖的范围，基本等同后续预测器能依赖的范围。

3.2 构造提升字典矩阵和输入矩阵

假设已经用固定采样周期h采集到一条轨迹：

code复制x(:,1), u(1), x(:,2), u(2), ..., x(:,T)

那么对于每个时刻k，我都有输入u(k)和下一时刻的状态x(k+1)作为监督目标。把所有x(:,1:T-1)做字典提升，得到矩阵：

code复制Ψ = [ψ(x(:,1)), ψ(x(:,2)), ..., ψ(x(:,T-1))]

同时把下一时刻状态全部提升：

code复制Ψ_next = [ψ(x(:,2)), ψ(x(:,3)), ..., ψ(x(:,T))]

把控制输入按行排在字典矩阵下方，组成扩展矩阵：

code复制W = [Ψ ; U]

其中U=[u(1), u(2), ..., u(T-1)]。然后只需要求如下形式的最小二乘问题：

code复制AB * W ≈ Ψ_next

其中AB=[A, B]是一个维度为D×(D+m)的矩阵。Matlab代码可以直接用右除符号实现：

matlab复制% 在数据采集后执行
nLift = size(psiX, 1);
uDim  = size(U, 1);

AB = PsiNext / [Psi; U];   % 最小二乘方式求解 AB * W = PsiNext
A  = AB(:, 1:nLift);
B  = AB(:, nLift+1:nLift+uDim);

PsiNext / [Psi; U]这行代码是最小二乘意义上的右除。求解得到A和B后，我建议做一步验证：随便选一段没有参与训练的轨迹，拿初始状态预测N步，看和真实非线性模型输出的差值曲线。这个差值如果发散得很快，大概率是个别提升项数值尺度太大，需要回头给数据做归一化或者正则化。

3.3 数值细节：归一化和正则化

EDMD在Matlab里看起来特别短，本质上就是一行矩阵运算。但隐藏的数值问题不少。如果多项式阶数偏高，Ψ矩阵里的元素很容易从1e-3到1e4跨越好几个数量级。这样直接求伪逆会让小数值项的作用被大数值项淹没，模型看起来误差很小，实际预测却经常抖动。

我的做法是先分别对字典行做z-score归一化，再用带脊回归的最小二乘形式：

matlab复制% 对Ψ和Ψ_next做行归一化，注意保存每一行的均值mu和标准差sd
% 假设已经得到归一化之后的 PsiNorm, PsiNextNorm
reg = 1e-6 * eye(size(W, 1));
AB = PsiNextNorm * W' / (W * W' + reg);

这里的1e-6不是固定值，我实际调参时会从1e-8试到1e-3，看验证集上的开环预测误差哪里最小。正则项本质是防止A矩阵的特征值跑得过大。A矩阵代表了提升空间里的动态本质，它的特征值如果过于夸张，在实际闭环仿真里会出现“预测器自激振荡”这类隐患。

4. Koopman MPC的完整工作链：状态估计与二次规划组装

4.1 状态估计如何接入Koopman框架

可能有人会问：非线性MPC的目标是把非线性系统管住，Koopman MPC的提升状态维数更高，为什么题目里还要提状态估计？

答案在于：Koopman MPC虽然预测器是线性的，但它依然是一个基于模型的控制器，模型需要从当前状态出发往下推。如果系统状态不可直接全部测量，你必须先拿到一个可信的状态估计值x̂，再用它去算提升状态：

code复制z0 = ψ(x̂)

之后才进入MPC优化。也就是说，状态估计和Koopman预测器不是互斥的，而是串联关系。我在仿真里常用的结构是：

code复制传感器测量 y
    ↓
非线性状态估计器（EKF/UKF/卡尔曼滤波）
    ↓
给出当前估计 x̂(k)
    ↓
升维得到 z0=ψ(x̂(k))
    ↓
线性预测模型推进 + MPC求u(k)
    ↓
作用到真实非线性系统

如果状态全部可测，那这一步就简化成x̂=x，直接把测量结果送进字典。但工程上传感器信息通常有限，比如只有位移传感器，没有速度传感器，那必须有一个状态估计环节垫底。这时候可以给EKF提供真实非线性模型，也可以只在回归数据上建立近似的局部模型用于状态估计。两种方式我都试过，如果想要稳定可靠的系统，建议状态估计器使用尽可能准确的非线性模型，因为它在闭环回路中离真实系统最近。

4.2 设计参考轨迹与提升字典的代价函数

MPC需要一个代价函数，并且这个代价函数必须能表达成凸二次型。由于控制器内部使用的是提升状态z，不是原始状态x，所以这里要小心一件事：你希望x靠近参考轨迹，但优化器操作的变量是z。

最顺手的处理方式是让字典的前几个函数就是原始状态本身，也就是：

code复制z(1:2) = x
z(3:D) = 其他提升项

这样代价函数可以写成：

code复制J = Σ (z_part(k) - z_ref_part(k))^T Q (z_part(k) - z_ref_part(k))
    + Σ u(k)^T R u(k)

这里z_part只取z中前两维，对应真实状态x，“其他提升项”不参与参考误差，主要依靠动力系统中的A矩阵自然约束它们。如果你硬要强迫多项式提升项也归零或趋向某个固定参考，往往会导致控制量变得很奇怪，因为那些提升项本身与真实状态之间存在代数关系，它们不是免费可设的设计变量。

4.3 MPC问题的离散化表达

预测器已经确定：

code复制z(k+1) = A z(k) + B u(k)

这是一个线性时不变模型。假设预测时域长度是N，当前提升状态是z0，那么k步后的预测是：

code复制z(k) = A^k z0 + Σ_{j=0}^{k-1} A^{k-1-j} B u(j)

代价函数里如果只用x误差项，可以定义一个投影矩阵：

code复制Ex = [eye(2), zeros(2, D-2)]   % 提取真实状态

然后代价函数写成：

code复制V = Σ_{k=0}^{N-1} (Ex z(k) - r(k))^T Qx (Ex z(k) - r(k))
    + u(k)^T R u(k)
    + (Ex z(N) - r(N))^T P (Ex z(N) - r(N))

其中Qx是第一段预测代价权重，P是终端代价权重，R是控制代价权重。把每个z(k)表达式代入代价后，优化变量只剩下u(0),...,u(N-1)。于是问题就变成：

code复制min_U   0.5 U^T H U + g^T U
subject  u_min ≤ U ≤ u_max

H和g可以用几层循环在Matlab里直接拼出来。这个阶段代码写起来有些繁琐，但在线执行速度非常快，因为后面每次调用只需要重新组装一次g，H完全可以在离线阶段预计算出来。

4.4 使用quadprog求解滚动优化

我先把H和g的组装代码骨架简单列一下。假定u的维度是m，预测时域为N，则决策变量U的长度是mN。离线可以预先算出多步转移矩阵：

matlab复制Npred = 30;
nu = size(B, 2);
nstate = size(A, 1);

% 决策变量长度
nvars = Npred * nu;

% 离线预计算每个预测步对U的灵敏度矩阵S
% S是一个 nstate x (Npred*nu) 的矩阵
S = zeros(nstate, Npred*nu);
Ak_prev = A;
for k = 1:Npred
    % 从第j个控制量对第k+1步状态的贡献
    for j = 1:k
        S(:, (j-1)*nu+1:j*nu) = S(:, (j-1)*nu+1:j*nu) ...
            + Ak_prev^(k-j) * B;  % 严格表达可用循环，这里略写
    end
end

这层代码写起来容易犯糊涂，更推荐用递推方式攒矩阵。对于真实项目，可以直接用下面的时间循环来组装Q矩阵：

matlab复制H = zeros(nvars, nvars);
g = zeros(nvars, 1);

Ak = eye(nstate);
for k = 0:Npred-1
    z_free = A^k * z0;
    if k == 0
        S_k = zeros(nstate, nvars);
    else
        S_k = zeros(nstate, nvars);
        for j = 1:k
            S_k(:, (j-1)*nu+1:j*nu) = A^(k-j) * B;
        end
    end
    Qz = Ex' * Qx * Ex;
    H = H + S_k' * Qz * S_k;
    g = g + S_k' * Qz * (z_free - ref_k);
end

% 加入控制项的R矩阵
Ru = kron(eye(Npred), R);
H = H + Ru;

这里的ref_k是参考状态。如果没有参考，可以设成零。最终调用：

matlab复制options = optimoptions('quadprog', 'Display', 'none', 'Algorithm', 'interior-point-convex');
Usol = quadprog(H, g, [], [], [], [], -u_max*ones(nvars,1), u_max*ones(nvars,1), [], options);
u_apply = Usol(1:nu);

取出第一个控制量施加给真实非线性系统，下一拍重新估计状态、重新升维、再求解，就是标准的滚动时域MPC。

5. 用Duffing例子验证方案：从双阱跃迁看预测器价值

5.1 为什么选Duffing系统做验证

为了让人直观体会这套方法的价值，我建议你用一个双阱Duffing系统试：

code复制x1' = x2
x2' = x1 - x1^3 - c*x2 + u

这个系统的特点是存在两个稳定平衡点x1=±1和中间一个不稳定的鞍点x1=0。如果目标是从左阱到右阱，传统的局部线性化模型会在出发点附近失效，因为局部线性模型会认为系统只是在左阱附近震荡，不具备“穿越势垒”的信息。但你若用Koopman字典把x1³这类强非线性项提升进模型，预测器就有机会学到完整势能面的特征。

在该模型里，如果只有x1这一路输出测量，加一个扩展卡尔曼滤波器估计x2，然后再用前面提到的Koopman MPC，就可以很清晰地看到控制效果：系统被激励后从左侧平衡点跳出，越过中点，最终稳定在右侧平衡点附近。这个过程中，内部MPC每次只解一个凸二次规划，不需要为非线性求雅可比矩阵再反复迭代。

5.2 预测器验证到底看什么指标

我在做验证时最关心的指标有三个：开环预测误差、闭环稳定性和控制成本。开环预测误差是最容易被忽视的，因为闭环情况下即使预测模型存在偏差，反馈也可以把误差压住。好的Koopman预测器应该在验证轨迹上做到若干步之内误差不爆炸。如果误差在20步以内就发散，说明字典或A矩阵拟合出了问题，先不要急着调MPC。

开环预测的验证代码其实就是一行仿真：

matlab复制xPred = zeros(2, Nval);
zPred = zeros(nLift, Nval);
zPred(:,1) = dip(x_true(:,1));
for k = 1:Nval-1
    uCur = u_val(:,k);
    zPred(:,k+1) = A * zPred(:,k) + B * uCur;
    xPred(:,k+1) = zPred(1:2, k+1);
end

然后和真实非线性系统的输出x_true画在一起，就能快速判断模型精度。需要留意的是，这个验证选的数据不能和训练数据重合，否则模型只学会了背诵训练数据，无法体现泛化能力。

6. Koopman MPC调参时我踩过最多的几个坑

6.1 提升维度并非越高越好

字典函数加得太多，会让拟合矩阵A的维度涨到几百，MPC里的H矩阵也会变成几百乘几百。虽然仍然是凸优化问题，但计算代价上去了。更麻烦的是，过度冗余的提升项不一定带来预测精度提升，反而会让A矩阵接近病态。我自己的经验是从“基础状态 + 一到两阶多项式”开始，先看预测误差，只有误差明显受非线性限制时再加RBF或更高阶项。

一个实用技巧是在EDMD拟合完成后，检查A矩阵特征值分布。如果特征值在单位圆附近密集聚堆，说明系统在提升空间里的模态很多，后续MPC对某些不重要的模态也赋予了权重，控制效果可能会表现得非常敏感。此时建议裁掉部分字典，或者用截断SVD降低有效维度。

6.2 采样周期和离散一致性不能想当然

Koopman模型是对离散时间系统做数据驱动拟合的。这里的离散时间步长必须与真实控制周期一致。我见过有人先连续时间仿真数据，再用子采样拼凑轨迹，拟合出的A矩阵和真实动力学在采样相位上对不上，结果开环验证怎么也过不了。

实际项目中务必要弄清楚：数据对(x(k),u(k),x(k+1))里的k+1到底对应真实物理时间的多久之后。如果跨了多个仿真步再做线性提升，非线性项在这些步之间的积分效果就很难被简单回归准确表达。

6.3 MPC约束只能约束那些提升空间里“受控”的量

前面说过，我把字典前几维设置为原始状态，所以我可以在MPC里直接约束原始状态，比如角度边界、速度上限等。但如果你想让某些提升项也满足范围约束，必须仔细检查它们是否能被输入驱动到任意位置。高维提升项往往不是完全独立可控的，直接对它们加上下界会让QP无解。

更好的做法是把约束重新拉回到原始状态层面。如果需要更严格的安全约束，建议直接在代价函数里做软约束惩罚，避免因为一两个不可达的提升项导致全盘无解。

6.4 quadprog的初始点设置和热启动

模型预测控制是滚动的，每一拍之间只有很小的状态变化。用上一拍的控制序列做这一拍的初始化，可以有效减少求解迭代次数。这个方法在NMPC里也有用，但在Koopman MPC里效果更明显，因为QP问题本身非常平滑，初始点好一点，收敛速度立刻提升。

在Matlab里设置初始点的方式是：

matlab复制Usol = quadprog(H, g, [], [], [], [], lb, ub, U_prev_shift, options);

其中U_prev_shift是把上一拍得到的最优控制序列整体左移一位，末尾补零。实测下来，大部分场景迭代次数可以减少三分之一到一半。

一点随身建议

我个人的体会是，Koopman模型预测控制最大的价值不是替代所有非线性控制方法，而是在你需要“大范围非线性能力”和“实时凸优化求解”之间找到一个平衡点。它其实还是在用数据替你做力学建模，所以数据质量直接决定上限。建议第一次跑通流程时，不要急着套复杂系统，用Duffing或倒立摆这类动力学清晰且非线性足够强的对象练手，把字典、EDMD拟合、QP组装、状态估计这四段链路分别调明白，再迁移到更复杂的实际设备上。这样出了问题也容易定位。