Koopman算子和EDMD如何让四旋翼MPC更快更稳

1. 为什么四旋翼控制需要Koopman这套“数据升维”思路先聊一个背景问题四旋翼的动态耦合其实很“拧巴”。位置变化由姿态变化驱动姿态变化又由推力差驱动整个系统像一串咬在一起的非线性齿轮。传统家底是级联PID内环控姿态外环控位置设计直觉简单粗暴一旦你要求飞机做30度以上的大角度机动或者在地面风场里保持稳定姿态级联PID就要靠相当“狠”的参数调节才能压住震荡。我之前在项目里就吃过这样的亏外环PID调得过于激进内环跟着抖最后电池电压稍微一掉飞机直接来了个“点头式坠落”。非线性MPC理论上能解决这个问题——直接用非线性模型做滚动时域优化把输入饱和、姿态约束统统塞进代价函数里。但代价是计算量很大普通嵌入式处理器扛不太住一秒钟几十次的非线性规划求解。于是就想换个思路如果能把非线性系统“重写”成线性系统那么MPC就可以退化成二次规划求解速度大幅提升。Koopman算子的思路就在这个时候进入视野。它的核心直觉非常迷人非线性的动力学本身是连续作用在状态空间上的“变换”如果我们把这个变换扔到一个更高维的函数空间里去观察它反而可能表现出线性性质。换句话说Koopman把我们熟悉的非线性微分方程转换成了另一个空间里的线性矩阵方程。你不需要舍去非线性只需要把它“摊平”到更多维度里。对一个做控制的人来说这简直像把一堆乱七八糟的电路接线整理成一根根清清楚楚的排线——前提是你得找到那个合适的“排线箱”。我在这套项目里的技术选型是数据驱动部分用扩展动态模态分解EDMD估计Koopman矩阵控制部分用线性MPC公式求解位置跟踪问题全部在MATLAB里集成验证。硬件部署暂时没做但代码结构已经预留了C代码生成的接口方便后续搬到样机上去。文章后面讲到的所有参数、脚本、结论都是在MATLAB仿真环境里跑出来的有一部分是我反复折腾之后总结出来的经验希望对你有直接参考价值。2. 建模与数据采集训练数据决定了你能飞多稳2.1 仿真模型与状态定义写Koopman训练数据生成器之前必须先定好“真实系统”是什么。我这里说的“真实系统”不是无人机实物而是一个高置信度的六自由度仿真模型。你在MATLAB里做的所有数据采样、控制器验证最终都是拿这个模型当裁判。如果裁判模型本身简化过头学出来的Koopman模型再漂亮也没有意义。我采用的仿真模型状态是位置 p \in R3单位米速度 v \in R3单位米/秒姿态角 \Theta \in R3单位弧度顺序取滚动、俯仰、偏航角速度 \Omega \in R3单位弧度/秒。输入 u [T, \tau_r, \tau_p, \tau_y]分别是总推力以及绕三个机体轴的力矩。注意这里没有直接建模每个电机的转速而是把推力差到力矩的映射封装成了输入层。这样做的好处是训练数据生成时不需要关心底层混控细节直接激励四个虚拟通道即可。如果你打算做实物迁移再把这四个通道映射到电机转速也不晚。状态方程我只写最关键的几项。平动部分m * dv/dt T * R(\Theta) * e_z - m * g * e_z这里 R(\Theta) 是机体坐标到世界坐标的旋转矩阵。转动部分I * d\Omega/dt \tau - \Omega \times (I * \Omega)I 是惯性矩阵。仿真时我用的是四阶龙格库塔积分采样步长 0.02 秒控制周期也是 0.02 秒也就是 50Hz。如果你只是验证算法逻辑用欧拉积分也够但一旦涉及大角度机动欧拉积分容易引入能量漂移训练数据里就带有“假噪声”后续EDMD估计出来的矩阵会莫名地变得不稳定。2.2 激励信号怎么设计才能覆盖完整动力学这一步是整个项目最容易敷衍、也最容易翻车的地方。很多初学者会直接在平衡点附近给一组小的随机推力扰动然后采集数据结果训练出来的Koopman模型只在悬停点附近有效稍微给个大角度指令就发散。问题的根源在于Koopman模型学的是“提升状态空间里的局部线性化”它只在训练数据覆盖的状态区域内可靠。我推荐的激励方案是分段多幅值扫频加阶跃。具体做法如下第一阶段绕着悬停状态做小幅值激励幅值控制在最大力矩的 5% 以内主要是收集平衡点附近的线性动态特征第二阶段把幅值拉到 15% 到 30%让角度偏转达到 10 到 20 度覆盖中等机动的包线第三阶段在保证仿真不翻车的提前下加入随机脉冲来逼出系统的耦合项比如同时给俯仰力矩和偏航力矩让飞机产生滚转耦合。每个阶段持续约 20 到 30 秒三段数据拼接起来再随机抽取一部分做训练。需要特别注意的是开环激励容易让飞机迅速偏离安全状态尤其是姿态角变得过大时数据里会出现很多“必炸”轨迹这些轨迹对学习稳定动态毫无帮助反而会拉偏最小二乘解。我在项目里用的是“低增益PD稳定器加外部激励”的混合模式——PD只负责让飞机不至于翻掉真正驱动飞机探索状态空间的还是外部扫频信号。这样既能保持数据有效性又能保证训练集里没有大段的发散轨迹。2.3 数据归一化与训练/验证集划分归一化这个问题我在初版脚本里忽略了结果吃了一次大亏。提升字典里既包含原始位置数值量级可以达到几十也包含角速度量级是零点几到几如果直接扔进EDMD最小二乘会被量级大的维度主导角速度相关动态几乎学不到。后来我把所有状态维度分别减去均值、除以标准差把每个通道都压到 [0.1, 0.9] 的范围模型预测精度立刻上了一个台阶。数据集的划分也要讲究。不能简单随机打乱因为轨迹是时间序列相邻样本高度相关打乱会让训练集和验证集“互通信息”。正确做法是按轨迹段划分比如采集了 20 段数据用 16 段训练、4 段验证。验证段要包含训练段没有的激励幅值组合这样才看得出模型有没有泛化能力而不是单纯记样本。3. 用EDMD估计Koopman算子从快照到线性矩阵3.1 提升字典怎么选Koopman理论本身不限制你选什么观测函数工程上常用的有三类多项式、高斯径向基、三角函数。我做了一个很朴素的对比纯多项式字典在状态偏离较大的时候预测误差会快速膨胀因为多项式对边界外的外插能力很差径向基函数在中心点附近精度好但需要铺很多中心点计算量随着维度上升得很快我最后采用的是“二次多项式 少量状态交叉项 原始状态”的混合字典实际效果最稳。一个参考性的字典配置是原始状态 12 维所有状态的两两二次项也就是 C(12,2)12 78 个额外加了三项积分量用于消除位置跟踪的稳态误差。总共提升到 93 维。这个规模在MATLAB里做EDMD非常轻松在线计算也只是几十乘几十的矩阵乘法。如果你追求更高精度可以再加三次项但95维和150维的区别在我的仿真里并不明显反而是矩阵条件数变差在线求解MPC时偶尔会出现数值抖动。所以我建议从 60 到 100 维起步而不是一开始就堆一个巨大的字典。3.2 EDMD最小二乘的核心操作EDMD的输入是快照对序列(x_k, u_k) - x_{k1}。对于每个采样时刻 k构造\psi_k \psi(x_k) \psi_{k1} \psi(x_{k1})然后把所有时刻堆成两个大矩阵\Psi_{state} [\psi_1, \psi_2, \ldots, \psi_{N-1}] \Psi_{next} [\psi_2, \psi_3, \ldots, \psi_N]还要把控制输入拼进来U [u_1, u_2, \ldots, u_{N-1}]我们希望找到一个分块矩阵 [A, B, c]使得\psi_{k1} \approx A \psi_k B u_k c把所有样本整合成一个超定线性方程组。MATLAB里我最常用的写法是Phi [Psi_state; U; ones(1, size(Psi_state,2))]; M Psi_next / Phi; A M(:, 1:n_psi); B M(:, n_psi1 : n_psin_u); c M(:, end);这里的Psi_next / Phi是矩阵右除本质上就是在解最小二乘问题 M Psi_next * pinv(Phi)。从数据结构上理解就是把“当前提升状态和控制输入”当作解释变量“下一时刻提升状态”当作目标变量拟合一个线性回归。这是整个项目里最朴实也最关键的一步直接决定后续MPC预测是否靠谱。还有一个小细节值得提建议对 \psi 做主成分截断。如果字典里某些维度和其它维度高度线性相关求解 M 时会产生数值退化。我通常先对 \Psi_{state} 做一次奇异值分解把奇异值小于最大奇异值千分之一的成分砍掉再做最小二乘。这样牺牲一点拟合残差换来的是MPC在线求解时的数值稳定性。3.3 模型验证别只看开环预测误差我见过一些论文里的Koopman模型验证只画一张“开环预测轨迹和真实轨迹的对比”看起来误差很小但放到闭环控制里却一塌糊涂。原因很简单开环预测误差会随时间累积但只要误差增长率不大开环轨迹也会慢慢偏而闭环控制里每步都会用当前真实状态重新构造 \psi模型只需要在单步预测或几步预测上准确就能维持稳定。所以我的验证分成三层第一层单步预测误差检查模型在一步更新里的精度第二层多步预测误差每 20 步重置一次状态考察在MPC滚动时域内的预测质量第三层闭环仿真直接把Koopman模型嵌入MPC跑姿态和位置跟踪看是否发散。第一层只是及格线第三层才是真正的裁判。我自己的经验是如果单步预测误差在 5% 以内但闭环还是震荡不要急着怀疑控制器先检查是不是提升状态的观测函数在设计时无法充分区分原始状态比如两个不同状态映射成了几乎相同的 \psi。这种情况下缩小字典里的低方差项或者增加高阶项往往比调MPC权重更有效。4. 把Koopman模型嵌入MPC约束、目标与在线求解4.1 Koopman-MPC控制器架构控制器整体结构并不复杂。每个控制周期开始时拿到当前状态 x_t提升成 z_t \psi(x_t)基于线性递推 z_{k1} A z_k B u_k c求解一个有限时域最优控制问题把最优控制序列的第一个控制量 u_t 送去执行。这种结构的最大优点是预测模型是线性定常的所以不需要在每步重新线性化。非线性MPC需要在每个采样周期重新计算雅可比矩阵然后重新搭建非线性规划问题Koopman-MPC把这部分流程完全省略了只靠一组离线学习好的矩阵做在线递推。效果就是在仿真中MPC的求解时间被压缩到了几乎可以忽略的程度。4.2 代价函数与约束设计我的代价函数设计成J Σ_{k1}^{N} || z_k - z_ref ||^2_Q Σ_{k0}^{N-1} || u_k - u_hover ||^2_R || z_N - z_ref ||^2_P其中 z_ref 是对应参考位置的提升状态。不要忘记把输入代价里的 u_hover 减掉。我见过很多实现在代价函数里只写 u_k结果控制器为了省小推力会偷偷让无人机往下掉因为输入为零比悬停推力更“便宜”。这个错误非常隐蔽仿真里只有当你观察飞行高度时才会发现。约束方面输入约束 u_min u_k u_max这是物理限制姿态角约束直接从 \psi 里提取原始角度维度来施加位置约束同理提取位置维度加边界。为什么可以这样提约束因为提升字典里保留了原始状态维度。如果你的字典是纯多项式基没有把原始状态直接包含进来那么你无法从 z 里恢复物理状态约束就只能加在输入上控制效果会大打折扣。所以再次强调字典设计必须包含原始状态分量这是我的血泪教训。4.3 用MATLAB求解预测序列的QP形式把线性递推展开z_k 可以写成初始状态 z_0 和输入序列 U 的线性组合。这一步的推导不复杂但写得干净很重要。我习惯先构造预测矩阵定义 Z [z_1; z_2; \ldots; z_N]U [u_0; u_1; \ldots; u_{N-1}]那么Z F * z_0 G * U H其中 F 是块状矩阵每一块是 A 的幂次G 是由 A、B 组合成的下三角块矩阵H 是由常数项 c 递推产生的偏移向量。MATLAB里可以循环填充也可以用 blkdiag 和 toeplitz 结构快速生成但规模不大的时候循环反而更清晰。代价函数整理成标准二次型J(U) U^T * Q_bar * U q^T * U const然后用 quadprog 调用。我的一个典型配置opts optimoptions(quadprog, Algorithm, interior-point-convex, Display, off); U_opt quadprog(H_qp, f_qp, Aineq, bineq, [], [], lb, ub, [], opts);预测时域建议取 10 到 15 步控制时域取 5 步。太短的时域看不出预测控制的好处太长的话求解时间和保守性都会上升。在我那台普通的笔记本上50Hz 控制周期内完成一次 quadprog 求解耗时大约在 10 到 25 毫秒之间完全能实时跑。另一个需要注意的坑是quadprog 默认求解的是凸问题如果你发现它报了“无法找到可行解”不要急着调 solver先看看是不是 z_ref 离当前状态太远导致输入约束无法在预测时域内拉回来。可以给参考轨迹加一阶滤波让目标点平滑过渡这也是工程里降低MPC保守性的通用技巧。5. 仿真对比Koopman-MPC vs 级联PID vs 非线性MPC5.1 三个测试场景的设计我为了看清这几种方案的实际差距设计了三个场景。第一个场景是悬停扰动恢复让飞机先稳定悬停在第 5 秒时给一个 \pm5 度的姿态脉冲扰动观察姿态恢复时间和超调。第二个场景是大角度姿态跟踪给定 30 度的俯仰角阶跃参考飞机需要快速抬起机头并保持同时要求滚转通道零交叉。这个场景非常考验控制器的非线性处理能力因为 30 度已经明显超出小角度近似范围。第三个场景是带常值风力干扰的位置保持在水平方向上叠加一个 0.6 m/s 的等效风速影响观察位置漂移后的恢复速度。每个场景都用同一套仿真模型、同样的采样时间只是控制器不同这样比较起来才有说服力。5.2 结果表格与我的判读我把我这组模型参数下的典型结果整理成表注意这是仿真里的表现更换模型参数后数值会变但趋势是可参考的。指标级联PID非线性MPCKoopman-MPC场景1姿态恢复时间1.6秒超调约1.9度1.1秒基本无超调1.2秒超调约0.4度场景2稳态误差0.7度有轻微耦合0.2度滚转交叉很小0.3度滚转交叉较小场景3位置漂移恢复受扰后2.8秒回到误差带2.0秒2.2秒单周期求解耗时无优化环节约420毫秒约15毫秒非线性MPC在控制精度上确实是最好的这一点不意外但它单周期求解动辄几百毫秒意味着在50Hz控制频率下几乎无法直接实时运行除非用编译级优化或者专用求解器。Koopman-MPC的精度比非线性MPC略差一点但远好于级联PID并且带约束处理能力足以应对大部分飞控场景。我在项目里最看重的是它的“性能冗余”。当我把这个控制器迁移到一架参数完全不同的四旋翼上只需要重新采集数据、重新拟合Koopman矩阵代价函数里的权重甚至都不用调太多就能重新获得稳定的控制效果。这种“换机不换控制框架”的体验在手工调PID参数时是难以想象的。5.3 计算开销这个方案最关键的实践价值如果比较单次预测模型更新的计算量Koopman-MPC比级联PID多了两步在线提升 \psi以及求解QP。在线提升本质就是一个矩阵乘法和一个非线性函数求值开销极低QP求解虽然比PID循环重但在普通配置下依然能跑在100Hz以上。而传统非线性MPC如果想要跑同样的频率就需要非常精细的代码级优化。Koopman-MPC某种意义上站在了“经典线性MPC简单高效”和“非线性MPC精准可控”的交界点上。它不完美但胜在付出的计算代价和获得的控制质量之间性价比极高。这也是为什么我最后会把项目重点落在Koopman方案上——它适合在真实机载硬件上落地。6. 调试路上最值得记住的五个教训6.1 归一化不是可选项而是必选项前面已经提过一次归一化但我想再强调它的具体表现。最初我跳过归一化时EDMD出来的矩阵里位置维度的行内元素普遍比角速度维度大两个数量级导致MPC在调节位置时“反应神速”在调节角速度时“反应迟钝”整个控制器像个偏瘫患者。把归一化加上之后矩阵元素量级变得均匀控制品质直接翻倍。所以无论是训练数据还是在线递推的在线状态都必须用同一套均值和标准差做变换。6.2 小心“局部线性化”的幻觉Koopman矩阵看着像线性系统但它的有效域受到训练数据分布的限制。如果你只采集了悬停附近的数据控制器就只能在悬停附近可靠你用它去处理大机动预测误差会让你误以为MPC被“bug”卡住了。实际排查时先看一眼当前状态是否超出训练数据覆盖范围。我在代码里直接加入了状态范围检查一旦超出就闪警告帮助我避免把“模型失配”误判成“控制器失效”。6.3 提升维度过量会带来什么有一段时间我迷信“维度越高表达越强”把字典加到了 200 维结果两个现象出现了一是训练时拟合误差确实下降二是在线QP求解偶尔出现病态矩阵甚至有一次控制量剧烈跳动差点把仿真飞控判为发散。后来查资料才知道高维提升空间里 \psi 的线性相关性会变得复杂最小二乘矩阵的条件数急剧增大微小数值扰动被矩阵乘法放大。我现在遵循一个原则字典维度在精度提升进入平台期后就不再增加转而通过数据多样性和归一化来优化模型。6.4 仿真通过不代表换场景还能用仿真环境可控但真实飞行有传感器噪声、通信时延、桨叶老化、风场突变等等这些都不会体现在你生成的训练数据里。我在仿真里测试各种方案时都能拿到好看的结果一旦考虑更实际的因素就必须在训练数据里加入噪声和时延模拟否则Koopman模型会被“清洁数据”宠坏。我并不是说要一开始就上复杂噪声模型但至少要做几组“噪声扰动下的鲁棒性测试”把训练数据加上测量噪声重新拟合看控制器会不会劣化得不可接受。6.5 数据采集要考虑闭环稳定性最后一个教训来自一次失败的试验。我为了追求“纯开环激励”用多段大幅值扫频信号直接驱动飞机模型结果不到几秒钟模型就进入不可恢复的翻转状态数据全报废。后来改成“低增益PD稳定器加外部激励”的方案立刻顺了很多。做数据驱动控制时数据本身的行为必须来自一个“至少不会崩溃的系统”。如果你想探索更大范围的状态空间就得多层递进地加大激励幅值而不是一把梭哈。这个思路不只是针对四旋翼凡是做学习型控制器的数据采集都适用。最后再分享一个实用小技巧训练完成的Koopman矩阵在MPC里投入使用前最好先做一次离线“闭环遍历测试”。把参考输入设置成一系列快速切换的方波观察仿真里是否有异常超调或稳态振荡。我在这种测试里遇到过不止一次矩阵拟合残差不大但闭环失稳的情况排查后发现都是因为部分观测函数在特定状态下存在激活死区导致模型预测突然“失灵”。这种问题在单步预测误差指标里很难暴露但闭环测试能很快逼出来。等你经历过这些坑再回头看Koopman-MPC这套流程就会明白它从前期的数据设计到后期的闭环验证每一个环节都是在跟“建模误差”和“泛化能力”较劲而这正是做数据驱动控制系统最有意思的部分。