做四旋翼控制的人估计都经历过这种纠结PID调到头还是觉得不够鲁棒想上模型预测控制MPC结果一看非线性MPC的求解时间就头皮发麻。我之前在课题组那会儿为了把姿态控制从PID方案换成NMPC光是调那个非线性求解器的初值就折腾了快一个月动不动就无解或者超时最后老实退回工作点线性化。但线性模型在大角度机动下又明显拉胯增益调度写起来也是一地鸡毛。后来我接触到这种基于数据驱动的“Koopman-MPC”方案才算是找到一个折中的路子。它的想法挺绕但内核很直接通过若干观测函数把非线性系统“提升”到高维空间在高维空间里系统可以被近似看作线性的于是你就可以在这个提升空间里直接用最成熟、最好写的线性MPC而不是去啃非线性优化的硬骨头。整个过程在Matlab里跑通闭环仿真验证我用跟踪大角度轨迹、加随机风扰的场景实测下来鲁棒性比我预想的要好。这篇文章就围绕这个思路把从原理、建模、数据采集、MPC设计到代码实现和踩坑记录的完整流程都拆出来适合对四旋翼控制、数据驱动控制或者MPC有基础、想落地这套方案的开发者参考。1. 项目核心思路为什么四旋翼控制需要Koopman-MPC1.1 两种传统控制方案各自的瓶颈在哪先说一条老路就是经典PID加上各种改进。四旋翼的状态维度有12个位置3维、速度3维、姿态3维、角速度3维各个通道之间本身高度耦合尤其是姿态矩和加速度之间还叠加着重力和空气动力耦合。PID在设计时通常按单通道解耦处理一旦飞机做大摆幅机动或者遭遇侧风耦合效应就不能忽视了控制器参数可能直接失效。另一条老路就是用精确的非线性模型做MPC也就是NMPC。这个方案在原理上最干净给定当前状态和参考轨迹在每个控制周期求解一个非线性规划得到未来若干个采样时刻的最优控制输入。四旋翼模型只有12维预测时域取10到20步时这个优化问题规模其实不算特别大但问题是非线性优化本质上是迭代的每一步都涉及雅可比矩阵的实时计算强约束下还可能出现初值敏感性。在我自己的仿真里用标准内点法求解NMPC单个周期平均耗时在20毫秒以上这还不算某些迭代发散后的重试时间放到实际飞控周期里基本没法用。1.2 Koopman-MPC在这两条路之间找到一个折中Koopman-MPC的思想最早是学生时期一个师兄给我安利的我当时第一反应是“这玩意儿是不是又搞什么高深的数学包装”。但真去推了一遍才发现它的逻辑其实是换个坐标系看问题而且这个坐标系是数据学出来的。具体来说假设有个离散系统 x(k1) f(x(k), u(k))这是四旋翼的真实非线性动力学。Koopman算子做了一件事不对状态本身做递推而是对“状态的函数”做递推。如果定义一个观测向量 z Φ(x)理论上存在一个线性算子使得 Φ(f(x, u)) 可以用 AΦ(x) Bu 来逼近而且随着观测函数集不断扩大这个逼近可以做到任意精确。说白了线性模型没有消失消失的是“必须用原来那个非线性状态当坐标”这个限制。真正的价值在于工程落地。一旦我们在高维空间获得了线性动力学后面的事情就完全回到了经典多变量线性控制的地盘写一个二次规划QP形式的线性MPC套标准求解器几十毫秒的在线优化能压到几毫秒以内约束可以明确写进去稳定性也有一些线性系统理论可以支撑分析。这就是为什么我觉得这条路值得认真研究一把它不是要替代NMPC而是用数据的钱换在线求解的时间钱在很多场景下这笔交易非常划算。2. Koopman算子的数据驱动实现原理2.1 从函数空间角度理解“非线性变线性”如果正儿八经讲数学Koopman算子定义在无穷维函数空间上。一个动力学系统 x_{k1} f(x_k)会对任何一个观测函数 g(x) 产生一个“拖拽”原本在 x 位置的 g 值被映射到 f(x) 位置去。这个拖拽过程是线性的因为函数空间本身满足叠加原理。用生活类比可能更容易理解一间屋子里摆了很多传感器只测温度和湿度两个标量观察整个屋子的演化会觉得规律很复杂、非线性很强。但如果你把屋里每一个分子的状态都当作观测变量铺到无穷维空间里系统演化反而是简单的线性扩散。Koopman算子要干的就是尽可能多地铺“观测变量”让复杂非线性在更高维视角下显露出线性结构。不过现实工程里没有人能用无穷维函数只能用有限个观测函数组成一个有限维子空间。这时所谓“Koopman算子”实际是原算子在子空间上的投影也就是一个有限维矩阵。这个矩阵通常用EDMDExtended Dynamic Mode Decomposition从数据里直接拟合出来它是整个数据驱动流程的核心。2.2 EDMD用快照数据拟合提升矩阵EDMD的思路非常直接收集一堆“现在时刻的提升状态输入”和“下一时刻的提升状态”快照然后最小二乘拟合一个线性映射。假设把系统的提升向量记为 z_k Φ(x_k) ∈ R^N输入为 u_k ∈ R^m我们希望逼近等式z_{k1} ≈ A z_k B u_k把同一时刻的 z_k 和 u_k 拼接成列向量 w_k [z_k; u_k]那么对所有时刻的数据可以写成矩阵形式。最小二乘解是标准的线性回归构造块矩阵 G Σ w_k w_k^T 和 E Σ z_{k1} w_k^T然后 K EG^{-1}再把 K 按维度切回 A 和 B。我在Matlab里实现这一步时发现一个容易踩的细节G 矩阵经常条件数非常大因为提升向量的各个分量往往高度相关比如状态平方项和状态本身之间就有较强的统计相关性。一个简单可靠的缓解办法是加Tikhonov正则化把求解目标变成min ||Z_next - K W||_F^2 λ ||K||_F^2这样做之后矩阵求逆稳定性会好很多训练出来的模型在预测长时域轨迹时也更稳。2.3 编码器-解码器结构与端到端训练EDMD的好处是快、简单、线性最小二乘一把梭但它的局限是提升函数要预先人为指定。通常大家都用多项式、正弦、余弦、径向基之类的字典字典选得好不好对最终模型性能影响极大。如果不想花心思凑字典可以用神经网络结构来代替EDMD设一个编码器 φ_w(x) 把原始状态提升到高维空间中间放一个线性动力学矩阵 A、B再加一个解码器 ψ_w(z) 把提升状态还原成原始状态整个结构端到端训练。损失函数通常写成L Σ ||z_{k1} - A z_k - B u_k||² λ1 Σ ||x_k - ψ(z_k)||²这个方案对四旋翼这种多变量强耦合系统尤其友好因为编码器可以自己学习到更贴合动力学特性的观测函数而不是依赖人工试出来的多项式组合。用神经网络版本的优势在训练结束后的预测精度上很明显但训练稳定性需要多花心思这一点我在第五节还会具体展开。3. 四旋翼动力学建模与仿真数据生产3.1 四旋翼的12维非线性模型长什么样数据驱动不代表不需要模型恰恰相反你需要用一套相对可信的仿真模型来生产数据和验证闭环效果。四旋翼的动力学通常写成如下形式状态取 x [px, py, pz, vx, vy, vz, φ, θ, ψ, p, q, r]^T其中 p 是位置v 是速度η 是欧拉角ω 是机体角速度。位置方程ṗ v速度方程v̇ R(η) [0, 0, T/m]^T - [0, 0, g]^T其中 R 是旋转矩阵T 是总拉力姿态运动学η̇ J(η) ωJ 是欧拉角速率到体轴角速度的变换矩阵角速度动力学J_b ω̇ τ - ω × (J_b ω)其中 J_b 是惯量矩阵τ 是力矩这四个方程组合起来就把“油门和三个姿态力矩”这四个控制输入映射到状态变化上了。仿真时我用离散时间更新采样周期设为 0.01 秒也就是100Hz这个频率也对应后面控制器的工作频率。值得注意的一点是这里用欧拉角表示姿态在大机动下有万向节锁的风险。如果训练数据里有太多接近 ±90° 的俯仰或滚转用欧拉角描述会出现奇异性。我实际处理时限制训练机动幅度把俯仰和滚转都压在 ±45°以内这样既覆盖了大多数实际飞行场景又绕开了奇异问题。如果真要支持全姿态机动状态表示就得换旋转矩阵或四元数复杂度会高一个档次。3.2 激励轨迹设计把系统的“脾气”逼出来Koopman模型是数据驱动的训练数据质量基本决定了模型上限。我一开始犯过的错误是图省事只在悬停点附近加了几组小幅正弦激励结果训练出来的Koopman模型预测小扰动还行一放到大幅机动跟踪场景立刻发散。后来我把激励数据分为三类组合采集第一类是随机脉冲激励在悬停状态给每个控制通道叠加不同幅值的随机阶跃信号相当于把系统在各个方向“捅”一下用于覆盖模型在不同工作点的局部动态。第二类是扫频信号从0.5Hz扫到10Hz左右重点激励系统的中低频飞行模态这些模态在跟踪典型轨迹时最容易被触发。第三类是组合机动直接飞“8字”、“螺旋爬升”这种大幅轨迹让系统在真实的长时域机动中产生联合激发。这三类数据加起来我一般采200秒以上总样本数超过2万组足够训练一个中等规模的EDMD模型或者神经网络编码器。3.3 数据预处理的三个细节采完数据不要直接扔给训练函数预处理这一步往往决定了模型能不能用。第一个细节是时间对齐。输入输出数据必须严格按采样时刻一一对应任何一拍的偏移都会给训练模型引入虚假的纯延迟闭环控制时表现为相位裕度下降、抖动加剧。我检查对齐的方法是直接算输入与速度响应之间的互相关函数正常应该在零延迟附近出现峰值。第二个细节是去异常。执行器饱和阶段的样本、传感器跳变造成的野值这些数据如果不剔除会让最小二乘解被少数离群点严重拉偏。我的做法是先用一个试训练的Koopman模型算出每个样本的预测残差残差超过3倍标准差的样本直接丢弃再重新训练一次。第三个细节是归一化。对于EDMD线性回归归一化与否对结果的影响相对小一些但对神经网络版本影响很明显。我把位置、速度、角度的量纲统一归一化到 [-1, 1] 区间角速度按实际最大幅值缩放这样编码器的训练收敛速度会快很多。4. 基于Koopman模型的线性MPC设计4.1 提升空间里的线性MPC长什么样既然我们已经有了 z_{k1} Az_k Bu_k 这个线性模型MPC的设计就完全复用了教科书上的线性预测控制套路。记 z_k 为当前时刻的提升状态N 为预测时域MPC在每个控制周期求解如下优化问题min Σ_{i0}^{N-1} ( (z_i - z_ref,i)^T Q (z_i - z_ref,i) u_i^T R u_i )约束为z_{i1} A z_i B u_ii 0, 1, ..., N-1z_0 Φ(x_current)u_min ≤ u_i ≤ u_maxz_min ≤ z_i ≤ z_max可选末端惩罚项可选用于提升稳定性由于动力学约束是线性的目标函数是二次的约束是线性不等式把决策变量全部组合成一个长向量 U [u_0, u_1, ..., u_{N-1}]^T优化问题可以直接转成标准QPmin 1/2 U^T H U f^T U s.t. G U ≤ h这个形式非常成熟Matlab里直接调 quadprog 就行实测下来单步求解时间通常在3~8毫秒之间完全能放到100Hz的控制回路里跑。4.2 参考轨迹怎么映射到提升空间这里有个容易犯迷糊的地方参考轨迹通常定义在原始状态空间比如目标位置 p_ref [1, 2, 3]^T但我们的模型是在提升空间里预测的。所以必须把参考路径也映射到提升空间也就是 z_ref Φ(x_ref)。不过这里要注意一个实用细节我们不需要对所有状态分量都施加跟踪权重一般只对位置、速度等物理量感兴趣姿态角只需要约束在一定范围内即可。如果直接在提升空间里给所有提升维度都加上大权重会把模型的内部表示也“锁死”在参考上反而影响控制效果。我的做法是保留参考提升向量用于目标项但Q矩阵只在物理对应的提升分量上取得较大值其余分量权重设得很小这样控制器既知道要往哪飞又不会跟模型内部表示较劲。解码器的作用这时候就体现出来了预测完之后用解码器 ψ(z) 把提升状态还原回原始状态用来显示轨迹、计算误差也可以用于反转控制量验证。4.3 约束、松弛变量与求解器选择MPC的好处是约束可以显式写进去。四旋翼最关键的约束有两个推力方向上的总拉力绝对值上限以及姿态角范围限制后者本质上是保护飞机不进入奇异姿态。这些不等式约束加到QP的G U ≤ h 里很简单但实操时我强烈建议在所有状态约束上额外加一个松弛变量。为什么因为真实情况下模型预测误差、外界扰动都可能导致QP在某个瞬间找不到可行解。加了松弛变量后约束变成“软约束”优化器宁可轻微越界也不会直接无解崩溃代价是惩罚项会把松弛量拉回来。这个设计对飞控这种实时系统几乎是必须的不然一个极端扰动就能让控制器直接无输出。求解器方面Matlab自带的 quadprog 分够用但到仿真循环里反复调用还是有开销。如果想把整个流程加速可以考虑用OSQP或者qpOASES这类专用QP求解器接口都很成熟。我自己用OSQP替换quadprog后平均求解时间降了差不多一半算是性价比很高的一项优化。5. Matlab实现细节与参数调优实战5.1 整体代码架构与流程组织这套项目的Matlab代码我按职责拆成了四个文件互相之间接口很清晰plant_model.m四旋翼离散动力学输入状态和控制量输出下一时刻状态collect_data.m生成激励信号调用 plant_model 采集训练数据并保存为mat文件train_koopman.m读取数据构造提升函数矩阵训练A、B矩阵并保存setup_mpc.m根据训练好的A、B构造QP的H、f、G、h矩阵run_simulation.m主仿真循环在每个周期调用quadprog求解再把最优控制量给 plant_model 更新状态这么组织的好处是训练和在线控制完全解耦以后换一组数据重新训练只要模型格式不变MPC部分一行都不用改。下面这段是训练EDMD模型的核心代码骨架用最小二乘直接拟合A和B% train_koopman.m 核心片段 Z []; W []; for k 1:size(Data, 1) - 1 x_k Data.states(k, :); u_k Data.inputs(k, :); x_next Data.states(k1, :); Z [Z, lift_function(x_next)]; W [W, [lift_function(x_k); u_k]]; end % 最小二乘加正则 lambda 0.01; K Z * W / (W * W lambda * eye(size(W,1))); A K(:, 1:dimZ); B K(:, dimZ1:end);5.2 提升函数的选择与参数影响提升函数选择是整个方案里最“靠经验”的部分。对四旋翼这种12维状态我试过三组字典第一组是纯线性字典就是 z [x; u中必须保留的输入]这时Koopman模型退化成普通线性系统效果约等于工作点线性化。第二组是加上状态平方项和交叉项的二次多项式字典维度大概涨到90多模型预测精度有一个明显跳升尤其对速度变化和姿态耦合的刻画变好。第三组是再往上加正弦余弦项、部分三次项维度超过200后精度提升开始变平但训练数据的需求量变得很大容易在小数据集上过拟合。我最终实用选择是二次多项式字典加少量关键的三次项比如俯仰角速度与推力的交叉项。这类选取并不需要很严格的数学推导而是靠验证集预测误差来挑的。做法是留出20%的数据做验证集比较不同字典配置下多步开环预测的归一化均方误差。用这个指标选字典比凭感觉快得多也更有说服力。5.3 MPC求解矩阵的构造技巧MPC部分最容易写错的就是QP矩阵构造维度错一格都要排查半天。我提供一个可以照抄的思路先把预测时域内所有状态用决策变量 U 线性表示。把当前提升状态记为 z0那么预测 i 步后的状态为z_i A^i z0 Σ_{j0}^{i-1} A^{i-1-j} B u_j把所有 i 的表达式展开把 u_0 到 u_{N-1} 提取出来形成一个块矩阵 S那么状态序列向量 Z_vec S_u * U S_z * z0。目标函数里的二次项系数矩阵就是 H S_u^T Q_bar S_u R_bar线性项系数就是 f (S_z * z0 - Z_ref)^T Q_bar S_u。这里 Q_bar 是把 Q 按预测时域重复成的块对角矩阵R_bar 同理。把这个过程封装成一个函数 setup_mpc_matrices(A, B, Q, R, N)之后不管预测时域怎么改只要重新调一次函数就能拿到新的H、f、G、h调试效率会提高很多。5.4 关键参数的经验取值与调整顺序把参数调好的顺序我总结为“先模型、后MPC、再约束”。也就是说先保证Koopman模型开环预测足够准确再调Q和R最后再慢慢加约束否则几个问题混在一起特别难排查。下面的表是我在Matlab仿真里比较稳定的起点参数可以作为初值再微调参数含义推荐起点调整方向采样周期控制器/仿真步长0.01s过大则性能下降过小则求解时间紧张预测时域N预测步数20增大提高稳定性但增加计算量提升维度字典数量90二次多项式先小后大观察验证集误差正则化λ训练正则0.01模型发散时增大Q位置权重位置跟踪权重对角阵diag([10,10,10,...])先调位置分量再调速度R输入权重控制力度权重0.1 * eye(4)过大则响应变慢过小则抖动松弛变量惩罚软约束系数100以上越大约束越硬但过大可能引起数值问题从我调下来的体感看Q和R的比例是最重要的。位置权重与输入权重的比值一般在100左右比较合适如果出现控制量高频抖振优先增大R而不是强行减小Q因为输入权重直接惩罚了控制量的大幅跳变。6. 调试中常见问题与排查技巧实录6.1 模型训练不收敛或者预测误差很大这是最常碰到的问题。如果你发现Koopman模型在验证集上的多步预测偏差越来越大先不要急着调网络结构或字典先检查数据本身。我遇上过的情况是训练数据里悬停附近的样本占了90%大机动样本渺渺无几导致模型把悬停附近的动态学得很好但大机动时完全跑偏。解决办法就是按我在第三节说的把随机激励、扫频、机动三类数据混合好保证样本在各工作点分布相对均匀。另外一个隐蔽的坑是数据里包含执行器饱和段四旋翼的总推力有上限饱和段输入不等于实际产生的推力这时候样本本身信息就是错的一定要剔除。6.2 开环预测发散模型一跑长就爆炸Koopman模型是无限维精确、有限维近似的当提升空间维数不够时模型预测可能在长时域内发散。比如只用线性字典预测5秒之后就明显飞了。解决手段有三种按成本从低到高排列。第一种是增大字典维数尤其增加非线性项这个方法通常能把发散时间点往后推不少。第二种是训练时对A阵做一点点收缩处理把A的特征值模最大值稍微调小于1这相当于给模型加了稳定性偏移长时预测会稳一些但代价是快速动态会被轻微压扁。第三种是上神经网络编码器让网络自动学习更合适的提升函数这是效果最好的方案但训练难度也最大需要调整网络结构和学习率。6.3 闭环控制器抖动、控制量来回切换如果开环模型预测已经不错了但闭环仿真里控制量抖得厉害大概率问题出在MPC本身。第一个要检查的是预测时域N是否过短N太短时控制器相当于短视对远期状态没有约束力工作点附近容易出现抖振。我实际从N10调到N20抖动明显改善。第二个要检查的是Q、R比例控制量抖振时优先加大R或者对相邻时刻控制量差也加上惩罚项后一种办法对抑制高频抖振特别有效。还有一个容易被忽略的原因状态约束太紧。如果你让MPC在提升空间里强约束某些内部表示分量可能会产生“刚凑出一个可行解要立马修正”的尴尬局面表现为控制序列频繁跳变。我建议初期先把所有状态约束关掉只保留输入约束等调稳定了再逐步加。6.4 从仿真到实机模型迁移的经验最后说一下从仿真往实机迁移的注意点这一步是很多同学最后卡住的地方。仿真里模型是确定的控制周期也是稳定的但实机上会有传感器噪声、执行器延迟、状态估计误差等一堆问题。我在仿真验证时留了一手在plant_model里人为加入了随机噪声和一拍延迟来模拟“坏”条件再用Koopman-MPC去控制效果仍然稳定。但真要上实机我建议状态估计环节一定要做准最好用扩展卡尔曼滤波或互补滤波把姿态速度估准否则Koopman模型输入的是带偏状态预测再准也无济于事。实机上训练的另一个现实问题是数据采集不像仿真那么随意需要手动遥控飞机飞各种机动来采集数据。建议先在悬停点附近采小激励数据做第一批模型用这个模型跑通全套闭环再慢慢扩大激励范围分批次迭代更新Koopman模型这样风险是可控的。7. 调试心得与后续扩展最后再分享一个我个人在实际操作中总结出来的小技巧Koopman-MPC这套方案别一上来就追求“全姿态、超大机动”的完美效果。我最初就是想着一步到位支持各种花式机动结果数据和模型训了无数版实际还是先老老实实把悬停和中等机动跑通更务实。先把悬停点附近的模型训准、将MPC从5步预测做到20步、把软约束调好这些基础工作打好后再逐步增加数据的机动范围你会发现模型和控制器是能跟着迭代一起“成长”的。整个过程虽然不算特别快但每前进一步的结果都是可预测、可验证的。后续如果想扩展我觉得两个方向最值得试。一个是在Koopman模型基础上加入扰动估计器把风扰和模型失配当作扩展状态抗扰能力能进一步提升。另一个是考虑把控制输入也作为提升函数的参数让Koopman模型能适应更宽范围的工作点变化这基本就是我目前在做的事情。这套基于数据驱动的学习和控制方案入口很窄但延展面很宽希望这篇文章能帮你少踩几个我踩过的坑。