Koopman-MPC：数据驱动四旋翼无人机非线性控制新方法

1. 先聊清楚Koopman-MPC 到底解决了一个什么痛点四旋翼无人机的控制难在哪大家都清楚这不是一个线性系统。悬停状态附近小扰动线性化模型还能凑合一旦做大机动、大角度倾斜、快速穿越障碍动力学里的非线性项就开始显著起作用。传统的控制方案往往是两条路要么依赖精确建模然后走非线性控制设计过程繁琐对模型误差敏感要么在平衡点附近做线性化再用线性MPC问题是一旦状态偏离平衡点太远预测模型就失真控制性能断崖式下跌。那数据驱动的方法这几年被反复提起很多做控制的人第一反应是端到端深度学习神经网络直接拟合状态转移函数或者干脆端到端输出控制量。这种思路理论上没问题但落地的时候坑很多一是需要大量优质数据二是一旦网络过拟合或者推理延迟高实时控制就出问题三是完全没有可解释性控制器出了异常行为你很难定位原因。Koopman-MPC 走了一条在我看来更聪明的中间路线。它的核心思想是用Koopman算子理论把非线性系统提升到一个高维空间里在这个空间里动力学变成线性的。你没听错非线性系统在一个经过精心构造的“观测空间”里可以呈现线性演化。正因为这个空间里的模型是线性的经典MPC那套凸优化框架可以直接搬过来用实时性、稳定性都有保障。而且模型是从数据里直接学出来的不需要你手推拉格朗日方程、不需要精确测量转动惯量、气动阻尼系数。这就是“数据驱动、模型线性化、滚动优化”三者合流的一个非常漂亮的技术方案。我最初接触这个方向的时候最打动我的不是那些皮毛的“智能控制”宣传而是它的数学结构足够干净Koopman算子给了一个线性提升模型MPC解决约束下的最优控制问题MATLAB 代码实现路径非常短。本文就基于这个项目标题把我自己对 Koopman-MPC 用于四旋翼无人机的理解、数据准备、算法流程、代码架子、踩坑经验全部整理出来。2. Koopman 算子为什么非线性系统可以被线性化2.1 从状态转移到“观测函数转移”我们先把数学基础铺一下。考虑一个离散非线性系统x_{k1} F(x_k)其中 x 是系统状态F 是非线性映射。经典的做法是直接在状态空间里研究 F 的性质麻烦就麻烦在F的本质非线性。Koopman理论换了个视角不直接看状态而是看状态空间上的观测函数 g(x)。定义一个算子 U作用在观测函数上(U g)(x) g(F(x))这个算子是线性的——不管F多么非线性U作用于函数的操作是线性的因为线性组合的观测函数作用在同一个点上。代价是什么呢代价是 U 作用的空间是无穷维的通常我们取一个有限维的子空间来近似。这个思想可以和生活里的事情做个类比。你在三维空间里看一个翻滚的纸飞机轨迹乱得一塌糊涂但如果把一个滑动的窗口相机放在更高的维度去记录它可能呈现出一个非常规律的螺旋上升结构。Koopman 算子做的事情就是找到那个“更高的维度”把非线性动力学塞进一个线性框架里。四旋翼的强耦合、大角度非线性在这个提升空间里慢慢被消解。2.2 怎么从数据里学到 Koopman 矩阵理论再好最终要落地我们必须把无穷维算子截断成一个有限维矩阵 K。现在学术界和工业界最常用的方法是 EDMD扩展动力学模式分解。步骤非常清晰第一步构造快照对。在你的四旋翼仿真或者实机上施加激励信号记录状态序列。激励信号要覆盖你关心的飞行区域不要只在悬停点附近打转。典型做法是叠加PRBS伪随机信号、扫频信号、或者幅值递减的方波序列。每一条轨迹产生一串状态x_1, x_2, ..., x_{N1}那么快照对就是 (x_i, x_{i1})也就是 (X_i, Y_i)。第二步选定字典函数。所谓字典就是一组观测函数集合Ψ(x) [ψ_1(x), ψ_2(x), ..., ψ_n(x)]^T这些函数把你关心的物理量提升到高维空间。四旋翼场景下我常用的基础字典包括状态本身位置、速度、欧拉角、角速度、它们的平方项、交叉项再加上一组径向基函数RBF比如高斯核形式的 Ψ(x) exp(-||x - c_j||² / σ²)。中心 c_j 可以选在训练数据点的子采样上或者用 k-means 聚类得到。第三步最小二乘求解。我们希望找到一个矩阵 K 使得Ψ(x_{k1}) ≈ K Ψ(x_k)两边乘以 Ψ(x_k)^T 并做聚合问题转化为标准最小二乘。定义两个矩阵G (1/N) Σ Ψ(x_k) Ψ(x_k)^TA (1/N) Σ Ψ(x_k) Ψ(x_{k1})^T那么 K G⁻¹ A。这是一个非常标准的线性代数过程。我第一次跑通这个流程的时候最大的感受是原来“学习一个非线性动力学”也可以如此优雅数据 字典 最小二乘问题就解决了完全不需要反向传播。当然细节上的坑后面再说。2.3 延迟坐标四旋翼轨迹预测里容易被忽视的环节这里要特别提一个四旋翼场景下的实际问题。我们拿到的状态 x [px py pz vx vy vz φ θ ψ p q r] 共12维。问题是在纯数据驱动的框架下你并不知道系统的阶数到底是多少——建模时省略了电机动态、空气阻力、机架弹性这些都会让系统的高阶信息丢失。解决这个问题的一个实用技巧是延迟坐标嵌入。把历史状态拼进来比如定义扩展状态s_k [x_k, x_{k-1}, ..., x_{k-d}]这里 d 是延迟步数。相当于用“记忆”补充隐藏状态。我在仿真里测过加入 d 2~3 的延迟扩展对大幅机动的预测精度能提升不少。延迟坐标本质上是把不可观测或未建模的动态通过历史轨迹呈现出来。Koopman理论里这类方法也有对应结论延迟嵌入可以构造更完整的观测空间不必追求理论严谨性工程上的增益是立竿见影的。3. 为什么选 MPC 而不选别的控制策略3.1 MPC 的“预测-滚动优化-反馈”闭环现在我们已经有了一个线性动力学模型z_{k1} K z_k H u_k其中 z Ψ(s)是提升空间的观测状态u 是控制输入四旋翼场景下通常是期望姿态角或者归一化电机推力指令。为什么这个框架和 MPC 是绝配因为 MPC 本来就是靠线性模型起家的。它的思路是当前时刻 k拿到状态估计 z_k然后求解一个有限时域的最优控制问题min Σ_{j0}^{N-1} (z_{kj} - z_ref)^T Q (z_{kj} - z_ref) u_{kj}^T R u_{kj}约束条件包括动力学约束z_{kj1} K z_{kj} H u_{kj}源自 Koopman 模型输入约束电机推力有上下限Umin ≤ u ≤ Umax甚至考虑相邻控制量之间的变化率约束状态约束如有需要位置有界、姿态角有界防止飞行器翻出合理范围求解后取第一段控制量 u_k 作用于系统下一时刻重新观测状态、重新求解。这个“滚动优化”赋予了系统非常强的鲁棒性即使模型存在一定误差或者出现了未建模扰动通过每步重解误差会被持续纠正。3.2 Koopman-MPC 和“端到端学习的控制”关键差异现在做数据驱动控制的人很多常常被问到Koopman-MPC和用神经网络训练一个控制器本质区别在哪在我看来至少有三点关键差异。第一是可靠性Kooped MPC 每一步在求解一个凸优化问题只要问题有解就一定满足约束神经网络策略再训练得再好你难以保证它在没见过的状态下不会输出一个过大的控制量。第二是采样效率EDMD 只需要几千到几万组数据点就能得到不错模型需要的数据比深度强化学习百万级样本少两个数量级第三是调试成本K 矩阵和字典函数是可检视的模型预测不准时能用工具分析哪里有问题而一个深度网络出问题时几乎没法解释。我的体感是它们在适用场景上各有分工——数据量充足、系统极度复杂难以建模、安全性要求略宽松的场景可以考虑端到端而对安全要求高、需要硬约束保证、调试人员需要理解模型行为的场景Koopman-MPC 有很强的吸引力。3.3 目标函数和约束怎么写更稳定目标函数的调节是MPC落地最考验经验的地方。提升空间的 z 是字典函数的值不同维度之间的量级差异可能非常悬殊——多项式项可能是10的4次方RBF基函数稳定在0到1之间如果不做归一化Q矩阵根本没法调。我的做法是在构造提升空间之前对原始状态先做归一化处理。每个物理量缩放到[-1, 1]区间再算字典函数。这样 z 的各维度量级相对均匀Q 矩阵就可以按照“你对哪个物理量更在意”来设权重。具体到四旋翼位置权重通常比姿态角权重调高因为任务层次上位置跟踪的优先级更高。Q 设成对角矩阵R 设成一个比较小的对角阵——注意 R 不能设成0否则高频抖振会非常剧烈。另外说一句输入约束的表达。四旋翼的内环如果接收的是期望姿态角那约束应该加在期望角的极限上比如 ±30°同时为了防止控制信号变化太快我还会在约束里加一条 |u_{k1} - u_k| ≤ Δu_max这对抑制执行器磨损和位置振荡效果很好代价只是优化问题里多一组线性约束求解时间几乎没增加。4. MATLAB 代码实现从模型构建到仿真闭环4.1 整体架构设计很多同学一上来就写代码写到一半发现不知道该怎么拼装。我建议先画清楚模块图再动手。整套实现分成四层第一层是数据生成层用四旋翼仿真模型可以是简单的牛顿-欧拉模型也可以直接用Simulink里现成的四旋翼模型产生激励轨迹 第二层是离线学习层负责归一化、字典构造、EDMD计算得到K和H矩阵 第三层是MPC求解层实现滚动优化求解器 第四层是仿真验证层把MPC控制器接回四旋翼模型做闭环跟踪测试。对初学者我建议把这四层严格分开。很多人为了图省事把数据生成和学习杂糅在一个脚本里最后排查问题时逻辑混乱、浪费时间。分层的代码结构可维护性高得多更重要的是每层的调试手段完全不同。4.2 数据生成激励设计比想象中重要数据质量决定模型上限。这一步做得不好后面算法再精巧也白搭。四旋翼的激励我一般这么设计对于位置通道给位置参考信号叠加扫频分量。频率从0.1Hz扫到1.5Hz幅值5米以内持续60秒。这样能够在多个频带上激励系统的动态特征。对于姿态通道给定滚转和俯仰角参考信号做幅值递减的方波和正弦混合激励。典型参数幅值从15度开始每10秒衰减到12度、9度、6度、3度。这样覆盖大角度和小角度两种工况。带宽覆盖的原则是你仿真中关心什么频率范围激励信号就要覆盖哪个范围。如果任务是多航点跟踪位置变化比较平滑那激励里低频分量充足就行但如果要做快速避障中高频的响应特性必须被激发出来否则模型学不到那些动态特征。采集数据时需要同步考虑采样率。四旋翼控制周期一般选择50Hz到100HzKoopman模型预测步长和控制周期保持一致这样MPC的预测步长才对应真实时间。不要用1000Hz仿真步去训练Koopman模型再以50Hz控制周期去做MPC——这种不匹配会在预测里带来系统性误差。4.3 核心代码EDMD 训练 Koopman 矩阵下面这段代码是我实际项目里的简化版本。关键结构都在照抄基本能跑通。前提是X_data和U_data是数据生成阶段保存好的训练数据格式为快照对序列。function [K, H, DictParams] trainKoopmanMPC(X_data, U_data, delay, dictOpts) % X_data: 训练状态快照每一列是一个时刻的状态 % U_data: 对应输入快照 % delay: 延迟坐标嵌入的步数 % 1. 状态归一化参数 x_mean mean(X_data, 2); x_std std(X_data, 0, 2) 1e-6; X_norm (X_data - x_mean) ./ x_std; % 2. 构造延迟坐标 Z_data []; for i 1:size(X_norm, 2) - delay z_i []; for d 0:delay z_i [z_i; X_norm(:, id)]; end Z_data(:, end1) z_i; end % 3. 构造字典函数矩阵 Psi(Z) Psi computeDictPhi(Z_data, dictOpts); % 4. EDMD 求解 G (Psi * Psi) / size(Z_data, 2); A (Psi * Psi_next) / size(Z_data, 2); % 这里的 Psi_next 是 Z_data 往后平移一步对应的字典矩阵 K_lin (G 1e-6 * eye(size(G))) \ A; % 5. 扩展得到带输入的 Koopman 矩阵 Z_full [Psi; U_data(:, 1:size(Z_data,2))]; Psi_plus computeDictPhi(Z_data(:, 2:end), dictOpts); M Z_full * Z_full; N Psi_plus * Z_full; K N / (M 1e-6 * eye(size(M))); % 返回 K以及归一化参数、字典参数 DictParams.x_mean x_mean; DictParams.x_std x_std; end有几个细节我要提醒。一是正则化项 1e-6 不是随便加的——快照矩阵如果秩亏比如某些状态通道几乎常值直接求逆会数值崩溃加上一个小的对角扰动能显著提高数值稳定性。二是字典函数computeDictPhi必须保持一致训练和在线预测时用完全相同的参数否则 Koopman 矩阵等于白学。4.4 MPC 在线求解关键代码MPC 的在线求解我推荐优先用 MATLAB 的quadprog因为 Koopman 模型是线性的目标函数是二次型整个优化问题是一个标准的凸二次规划QP。quadprog求解非常快两三毫秒就能完成一个有限时域的解算完全能满足四旋翼的控制周期。function u_mpc solveKoopmanMPC(z_current, K, H, Q, R, N, u_ub, u_lb) % z_current: 当前提升空间的观测状态 % K, H: Koopman 矩阵对应提升空间 输入 % N: 预测时域长度 % 构造标准 QP 形式min 0.5*u*Hqp*u f*u n_z size(K, 1); % 提升空间维度 n_u size(H, 2); % 输入维度 % 批量构造预测矩阵 A_aug zeros(n_z*N, n_u*N); for k 1:N for j 1:k A_aug((k-1)*n_z1:k*n_z, (j-1)*n_u1:j*n_u) K^(k-j) * H; end end % 预测的零输入响应 z_free zeros(n_z*N, 1); z_temp z_current; for k 1:N z_temp K * z_temp; z_free((k-1)*n_z1:k*n_z) z_temp; end % 目标函数矩阵 Q_blk kron(eye(N), Q); R_blk kron(eye(N), R); Hqp A_aug * Q_blk * A_aug R_blk; fqp (z_free) * Q_blk * A_aug; % 输入约束 Ain [eye(n_u*N); -eye(n_u*N)]; bin [repmat(u_ub, N, 1); -repmat(u_lb, N, 1)]; % 求解 options optimoptions(quadprog, Display, off, Algorithm, interior-point-convex); u_seq quadprog(Hqp 1e-6*eye(size(Hqp)), fqp, Ain, bin, [], [], [], [], [], options); % 只取第一步 u_mpc u_seq(1:n_u); end这个实现是最直观的稠密MPC形式预测矩阵 A_aug 的维度是 Nn_z × Nn_u。当 n_z 50、N 20 时矩阵规模是 1000×80quadprog 完全吃得消。如果基函数选得比较多比如 n_z 超过 100则需要切换到稀疏的线性时变系统形式或者用快速梯度法做内点迭代这里不展开。4.5 从提升空间回到控制输入的完整一步在线控制循环里每一个控制周期你需要完成这样几个动作先获取当前四旋翼的12维状态 x_k。这里假设仿真中可以完美测量。接着归一化、构造延迟坐标 s_k然后计算字典函数 Psi(s_k)得到 z_k。随后调用solveKoopmanMPC得到最优控制输入 u_k即期望姿态角指令送入底层姿态控制器。最后等待下一个控制节拍。我把这一步的代码放在了仿真主循环里你可以把控制器替换成P-PID组做对比直观看到Koopman-MPC的轨迹跟踪误差显著更小尤其是在大角度转弯段PID会明显滞后而MPC因为提前规划了轨迹整个运动像“看着公路拐弯开车”一样平滑。5. 关键参数怎么选字典规模、时域长度和正则化5.1 字典函数的维度和种类字典选择是Koopman建模最核心的自由度。维度太低提升空间不够、线性化近似能力不足维度太高过拟合和计算量双重增长、K矩阵病态程度加剧。我的经验四旋翼12维状态加上延迟 d 3扩展状态就是48维。字典包含这些元素基本够用扩展状态的线性项48维每个状态量的二次项48维左右关键交叉项比如位置与速度的交叉项约24维8~12个RBF基函数中心点在训练数据覆盖空间里均匀采样算下来提升空间差不多 120~140 维。这个量级Quadprog毫无压力EDMD计算也就毫秒级。如果你看到训练误差和验证误差差异很大说明过拟合减少RBF个数或者增加正则化强度。这里有一个常被忽略的细节字典函数必须包含原始状态本身。这不是可选项而是必须项。因为MPC的目标函数通常要惩罚位置误差、速度误差如果这些物理量不能直接从 z 里提取出来你无法构造有意义的代价函数。有一种处理是把原始状态单独放在 z 的前12维字典其余部分作为“辅助观测”。我在目标函数里就只惩罚这些原始状态对应的维度辅助维度不参与惩罚只负责演化。5.2 预测时域 N 的选择MPC预测时域是经典权衡问题。N 太小控制器短视无法有效规避未来的障碍或实现提前减速N 太大预测矩阵规模增大优化耗时上升更关键的是 Koopman 线性近似在长时域预测中误差累积。四旋翼场景下我做过的实验控制周期20msN 取 15~25 表现最好。换算成预测时长就是0.3~0.5秒。这个时长刚好覆盖了四旋翼姿态响应的自然时间尺度——需要提前规划但又不会长到模型误差主导。开始调试时建议从 N 10 起步先保证闭环稳定性再逐步加长 N。5.3 正则化强度与K矩阵谱半径Koopman矩阵的特征值分布直接反映模型稳定性。理想情况下K矩阵的谱半径最大特征值模应该在 1 附近。明显大于1表示模型不稳定训练数据里的瞬态增长被当成了系统固有特性明显小于1表示预测快速衰减模型可能丢失真实系统的高频特征。训练时有一步值得做计算K的特征值并画出谱图。如果某些特征值模大于1.1可以通过在 EDMD 目标里加谱罚项抑制也可以在求解阶段对K的特征值做投影。我在实际项目里用的一个简便方案是训练时给G矩阵加特征值软约束的正则化具体做法是把 (G λL)⁻¹A 中的 λ 从 1e-4 逐步往上调观察K的谱变化。经验上 λ1e-3 左右能够在拟合精度和谱稳定之间取得较好平衡数据量越大λ可以越小。6. 实操中遇到的坑和排查方法6.1 状态归一化没做好K矩阵病态这是我第一次跑通整个流程时踩的第一个大坑。当时位置量是米数值到几十角速度是弧度每秒数值零点几如果不归一化RBF函数的带宽参数完全没法统一设计——一个维度的尺度差异会直接导致部分字典函数失效。排查方法很简单训练完成后画出字典函数所有输出的均值和标准差。理想情况是大部分输出在0~1量级。如果发现某个输出长期是10的3次方以上或者恒为0说明归一化或者字典选择有问题。解决方案对所有输入状态先做z-score归一化后再进字典函数所有字典输出再做一个单位化处理除以训练集最大值。6.2 训练误差很小验证误差爆炸这个问题九成是过拟合。我在一次实验中把RBF数量从10加到40训练误差确实下降了但验证集预测误差反而涨了3倍。根因是RBF的中心分布过于稀疏某些区域只有一两个数据中心K矩阵在这些局部区域拼命拟合噪声。解决方案三选一减少RBF个数让字典的表达能力低于数据量能支撑的复杂度增加训练数据让每个RBF中心附近都有足够样本增加正则化系数限制K矩阵自由度。工程上最优先尝试的是减少RBF个数立竿见影。很多时候“少即是多”保留原始状态、二次项加上少量RBF就已经足够优秀。6.3 MPC求解变得很慢降维和QP预处理当字典维度超过200、预测时域N超过30稠密Q P的求解时间会突破30ms超过控制周期。这时候就不能再指望quadprog走默认流程了。两个发展方向。第一是模型降维对训练数据构造的 K 矩阵做主成分分析PCA或者平衡截断只保留对可控性贡献最大的前30~40个模式压缩后问题规模大幅缩小。第二是求解预处理设定quadprog的初始点用上一步的解作为热启动、严格设置约束矩阵的稀疏格式用 sparse 声明 Ain能显著加速迭代收敛。我测试过的一个数据点维度140、N20的MPC问题热启动之后单步求解约1.6ms完全够用。如果到了这一步还是不满足性能下一步考虑换 C 语言实现、用 OSQP 求解器而不是在 MATLAB 里无限堆算力。6.4 延迟坐标的维度怎么定延迟步数 d 的选取没有绝对标准但有判断方法。做法是做一个简单的预测实验用不同的 d 分别训练模型在验证集上做10步预测对比预测误差。误差随 d 增加先下降后上升拐点处就是合理的 d。四旋翼场景我的经验是 d 在2到4之间。超过4之后延迟维度给模型带来的信息量趋于饱和但过拟合风险和计算量反而上去了。另外延迟坐标给维度带来的膨胀是很吓人的12维状态延迟3步就是48维再加上字典扩展直接推到近150维。所以不要盲目加大延迟。7. 系统辨识之外关于实机部署的思考7.1 仿真到实机的差距怎么处理我承认文章到目前为止的内容主要在仿真框架内讨论。仿真模型获取数据非常干净无延迟、噪声很小Koopman学习和MPC的自然配合可以说如鱼得水。一旦放到现实无人机上传感器噪声、执行器延迟、模型不确定性都会冒出来。我了解的实用做法是在训练数据生成阶段就注入与实际传感器匹配的噪声。比如位置测量加入高斯噪声标准差0.05m、姿态角加入噪声标准差0.02rad。让EDMD在含噪数据上学习得到的K天然包含对噪声的鲁棒性。控制器反馈时用的状态先过卡尔曼滤波器让进入到MPC预测的z更平滑。7.2 失败模式和安全兜底任何数据驱动控制器都必须考虑“模型失效”时的表现。我的思路是给Koopman-MPC设计两层保护第一层在MPC内部对所有约束严格保留尤其对输入做饱和限制不允许控制器输出超出执行器能力的指令第二层在控制器外部当预测误差超过预设阈值比如连续5步预测残差超过正常范围的3倍立刻切换回底层PID控制器保证无人机至少不掉落。这个“模型可信度监控”模块在实机部署时是刚需也是我觉得很多论文作者不会提的细节。仿真里你永远不会遇到模型完全失效的时刻但实机上一阵侧风、一次装机误差就可能让K矩阵面目全非。8. 我的实操心得从零到一搭建 Koopman-MPC 的路径建议最后分享一点我调试这个系统的顺序和方法希望对刚开始接触的朋友有帮助。第一步不要急着练MPC。先把数据生成和EDMD跑通做开环预测验证。输入一段训练集之外的轨迹让模型多步预测比如20步和真实轨迹画在一起肉眼看贴合度。这一步如果不过关后面全白搭。记下这个准则开环预测20步不散是参加仿真的门票。第二步在仿真里用固定增益控制器比如姿态内环PID做稳定化把Koopman-MPC只放在外环位置控制层。这样即使MPC行为异常内环的稳定器可以兜底调试过程不至于发散。很多人第一次就把MPC放在全环一旦参数不对无人机仿真直接炸飞很难定位问题出在哪一层。第三步一点点增加MPC的权限。先只控制高度通道再单独控制水平位置最后把横滚俯仰也交给MPC。每一步都对比PID基线的跟踪误差、控制量抖动程度。编辑完这些内容回头再看Koopman-MPC确实是一个“理论和工程握手”很成功的方向理论上它用Koopman算子给出了非线性到线性的漂亮转化工程上它把MPC的成熟体系直接嫁接过来用数据代替人工建模。对我个人而言这套方法最大的价值在于它把“非线性控制”从偏微分方程和Lyapunov函数的纸面推导里解放出来变成了一个“数据优化”的组合工程问题而这类问题正是我们这些做应用的人最擅长的。希望在读这篇文章的朋友也能早日跑通自己的第一版Koopman-MPC四旋翼控制器。