简介面向无人机控制、数据驱动建模与控制方向研究者的Matlab实现方案聚焦四旋翼飞行器非线性系统的Koopman算子与扩展动态模式分解EDMD控制方法。方案将非线性动力学转化为近似线性表示配合MPC等控制策略适用于课程项目、综合实验及毕业课题。包内共90个文件、49.66MB以Matlab脚本.m与函数为主覆盖EDMD评估、MPC控制仿真、轨迹生成与状态绘图等模块附带fig/png结果图、mat实验数据、pptx示意说明及README文档结构清晰便于二次开发。已有110人学习关注适合需要理论结合实践的研究者。读者可基于参数化代码调整算子基函数、预测时域等设置复现四旋翼动态模式提取与控制效果实验数据可直接加载验证算法详尽注释与多版本兼容Matlab 2014/2019a/2024a降低上手门槛为深入理解Koopman方法及数据驱动控制提供可操作工具。1. 为什么用Koopman算子做四旋翼控制从非线性到线性表示我第一次看到Koopman算子是在翻线性控制论坛时当时手头正好有个四旋翼悬停控制项目被非线性动力学折磨得够呛。传统做法要么在平衡点附近做线性化要么硬上反步法、滑模控制但参数一改就要重新推导。后来用EDMD算法把无人机状态提升到高维空间居然直接得到一个线性状态方程后面再套MPC控制效果比想象中稳。这篇笔记把我从理论到Matlab的完整实现方案写下来包括可复现的代码和几个容易翻车的细节。适合正在做四旋翼数据驱动控制、又不想把时间耗在非线性模型重新推导上的从业者也适合刚开始接触Koopman算子、需要一个能跑通案例的初学者。我会尽量把参数、边界和坑都讲具体少说空话。2. 先把EDMD数学基础打牢字典函数与状态提升2.1 状态提升的核心思想为什么升维反而更简单Koopman算子的出发点是对非线性系统 x_{k1} f(x_k, u_k)如果能在无限维函数空间里找到一个线性算子 K使得对所有观测函数 g 都有 g∘f K g那么状态转移在“观测”层面就是线性的。EDMD 做的是把这个无限维算子在有限维子空间里近似出来。具体说取一组字典 Ψ(x) [ψ1(x); ψ2(x); ...; ψm(x)]然后找矩阵 A 和 B使 Ψ(x_{k1}) ≈ A Ψ(x_k) B u_k。这看起来是把状态 x 升到 m 维比原来的 n 维还大为什么反而更简单因为四旋翼的动力学里推力与加速度耦合、力矩与角加速度耦合再加上重力项直接建模是强非线性。但当我们把速度、角速度的平方项和交叉项放进字典后那些耦合项被“吸收”进线性矩阵里。以后每一步预测只需要一次矩阵乘法和加法不用每一步都重新算复杂的非线性方程。要提醒的是这只是一种近似。有限维字典不可能完全张成一个不变子空间所以 A、B 只是最小二乘意义下的最佳线性映射。实际操作里能不能用取决于字典函数选得是否贴近系统非线性。这也是为什么我反复强调不要一上来就整几十维的高斯基先把多项式调通再逐步加复杂度。以四旋翼为例常见的状态是位置 p、速度 v、欧拉角 Θ、角速度 ω维度 n12。连续动力学里包含旋转矩阵 R(Θ) 和力矩耦合项 ω × Jω这些在悬停点附近做线性化还行一旦做大机动就明显偏。EDMD的通俗理解是把 x 和 x 的二次项一起作为“新状态”让原来在原始坐标里弯曲的轨迹在高维空间被一段直线近似。一个极简的类比是标量系统 x_{k1} μ x_k^2。如果定义 z [x; x^2]理论上 z 的下一时刻是 [μ x^2; μ^2 x^4]需要更高阶项才能闭合。EDMD 不追求精确闭合它只是从大量数据里找到矩阵 A、B让 z_{k1} ≈ A z_k。这就是为什么数据覆盖范围、字典选取比理论推导更重要。2.2 字典函数怎么选多项式、高斯与傅里叶基的取舍字典函数是EDMD的“黑匣子”选得不好直接决定这是玄学还是工程。我整理了一个简单对照表方便你按场景挑。基函数类型表达形式参数个数外推能力适用场景多项式基1, x_i, x_i x_j低弱离训练点远会发散平衡点附近小幅机动高斯RBFexp(-|x-c_j|² / σ²)多中心点和宽度中局部响应悬停、大包线但训练充分傅里叶基sin(a x), cos(a x)中中周期性强周期性轨迹跟踪四旋翼在悬停和低速飞行时的非线性主要来自于重力分量在小角度下的正弦项、以及速度平方阻力项。二阶多项式可以近似这两种特征。所以我最常用的方案是对全部状态做标准化后用常数项、线性项、平方项和交叉项也就是二阶多项式字典。12维状态对应 m 1 12 12*13/2 91 维提升空间。这个维度在Matlab里做EDMD训练和MPC优化都不算大运行也快。构建字典的代码我放在下面function [Psi_x] build_poly_psi(x, order) % 构建多项式字典函数 % 输入: x 为 N x n_stateorder 为多项式阶数 % 输出: Psi_x 为 N x n_basis第一列是常数1 [N, n] size(x); % 先初始化效率比在循环里 append 高 col_count 1 n; if order 2 col_count col_count n*(n1)/2; end Psi_x zeros(N, col_count); Psi_x(:,1) 1; % 常数项 Psi_x(:,2:n1) x; % 线性项 if order 2 idx n 2; for i 1:n for j i:n Psi_x(:, idx) x(:,i) .* x(:,j); idx idx 1; end end end % 如果想加到三阶继续在下面扩展即可。 end这段代码的逻辑是先把常数和线性项写进去再用双重循环遍历所有 i≤j 的组合生成 x_i * x_j 项。注意我用zeros预分配了矩阵不是在循环里用[Psi_x, 新列]去拼因为后者在Matlab里会反复复制内存样本一多就非常慢这也是很多人说EDMD训练慢的常见原因。一个容易被忽略的参数是状态标准化。比如位置可能是几米角速度可能是每秒几弧度它们相乘后量级差异很大。如果不做标准化二次项会由量级最大的状态主导模型基本学不出角速度通道的动力学。我的习惯是用训练集的均值和方差做 z-score 标准化保存 mean_x、std_x控制时对实时状态也做同样的变换。这个步骤做好了模型验证阶段的 RMSE 往往能直接降一个数量级。选字典的流程我一般从一阶多项式开始跑通闭环验证再升到二阶。如果验证误差降幅明显继续升三阶如果升阶只让训练误差下降验证误差变差就停在前一阶这个信号说明字典已经过拟合了。高斯RBF我只在数据集非常干净、且训练覆盖范围明确时才用因为中心点和宽度需要调参。3. 在Matlab里搭EDMD离线训练流程从飞行数据到Koopman模型3.1 数据采集与预处理确保激发充分性EDMD是数据驱动方法数据质量决定模型上限。这里说的“充分激发”不是简单地来回飞几圈就行。我们要让每个状态通道和输入通道都被独立激励到尤其四旋翼的横滚、俯仰、偏航通道都要有持续变化的参考。常见做法是用叠加随机方波或扫频信号作为四个电机的虚拟控制量在仿真模型里跑一段足够长的轨迹。我个人习惯是采样时间固定为 0.02s总数据长度至少 2~3 分钟也就是几千个样本。样本太少时A、B 矩阵很容易过拟合尤其当字典维度升到几十上百以后。下面是一段用仿真模型生成训练数据的示意代码真实飞行记录也类似只是把模型仿真换成读飞控日志dt 0.02; N 9000; % 总样本数对应180秒 t (0:N-1) * dt; % 用随机方波激励四个输入通道 u_train zeros(N, 4); for k 1:4 u_train(:,k) 0.6 * sign(sin(2*pi*0.15*t k)) 0.4*randn(N,1); end % 实际中要保证电机推力在有效范围内这里只是示意 u_train min(max(u_train, 0), 1); % 归一化推力 0~1 % 然后用非线性四旋翼模型计算状态轨迹 X_train, X_next % 这里省略具体Euler积分代码核心是得到同步的 (X_k, X_{k1}, U_k) 三元组这里sign(sin(...))用来生成交替变化的方波randn再叠加随机小扰动目的是让系统既有大范围机动又在小波动。真正训练前还要检查是否有样本状态落在奇异点比如欧拉角接近 ±90° 时角度解算会跳变这种样本要直接剔除不然会把 A 矩阵学成一个畸形的映射。预处理阶段我还会做两个动作去除跳变点和低通滤波。飞控日志里偶发一个传感器毛刺对单步状态也许影响不大但多步预测会被 EDMD 放大。最简单的做法是检查相邻两帧状态变化量如果某个通道变化超过物理极限就删除该样本不要用插值去补。数据清洗后要重新做标准化因为均值方差变了。3.2 EDMD核心代码一步算出Koopman矩阵有了同步的三元组后EDMD的核心就是一步最小二乘。先把所有样本提升成字典矩阵然后求解一个线性回归。下面是完整的训练代码% X: N x n_state 当前状态 % X_next: N x n_state 下一时刻状态 % U: N x n_u 当前输入 % order 2 阶多项式 Psi_x build_poly_psi(X, 2); Psi_x_next build_poly_psi(X_next, 2); % 构造回归矩阵: M * K ≈ Psi_x_next M [Psi_x, U]; % N x (n_basis n_u) Z Psi_x_next; % N x n_basis lambda 1e-4; % 岭回归正则化系数 % 加入正则化防止过拟合 K (M * M lambda * eye(size(M,2))) \ (M * Z); % 拆分 A 和 B A K(1:n_basis, :); % n_basis x n_basis B K(n_basis 1:end, :); % n_basis x n_u逻辑说明M 的每一行由提升状态和输入拼接而成Z 的每一行是下一时刻的提升状态。最小二乘求解的是 A、B让A * Psi_x B * U在最小二乘意义上逼近Psi_x_next。代码里我用了正规方程加岭回归而不是直接M \ Z是因为字典矩阵的条件数往往很大直接反求会放大噪声加一个很小的lambda1e-4能明显改善稳定性又不至于把真实动力学抹掉。这里的n_basis由build_poly_psi自动决定12 维状态二阶多项式就是 91。如果你把order调成 3n_basis会迅速膨胀到 1 12 78 286 377 维。数据量不跟上就很容易翻车我通常只在数据量充足时才上三阶。关于拆分 A 和 B 的方向经常有人搞反。因为 K 的行对应 M 的列所以前 n_basis 行对应提升状态后 n_u 行对应输入。而 M * K 是样本行方向为了得到列方向的状态方程需要把拆出来的矩阵转置。建议你训练完后用一段验证数据检查一下z_next_pred A * z_current B * u_current如果维度报错或者预测结果等价于转置后的效果说明转置写反了。3.3 模型验证指标预测误差与特征值分布训练完别急着拿去控制。我的流程是先做两个验证单步预测误差和开环多步预测误差。单步误差只反映一步以内的拟合多步预测才能看出模型是否抓住了长时间动力学趋势。% 取一段训练时没见过的验证数据 x_val, u_val, x_val_next Psi_val build_poly_psi(x_val, 2); z_hat Psi_val * A u_val * B; % 预测状态线性部分 x_hat z_hat(:, 2:n_state1); rmse_step sqrt(mean((x_val - x_hat).^2, all)); % 多步预测: 迭代N步 z0 build_poly_psi(x_val(1,:), 2); H 50; % 预测50步也就是1秒dt0.02 z_pred zeros(n_basis, H); z_pred(:,1) z0; for k 1:H-1 z_pred(:,k1) A * z_pred(:,k) B * u_val(k,:); end x_pred z_pred(2:n_state1, :); % 取出线性状态这里的A * z_pred(:,k)是矩阵乘列向量注意 z 是列方向和训练时行方向一致。我一般看 3 秒预测窗口内的轨迹是否发散以及 RMSE 是否随预测步长线性增长。如果指数爆炸先检查 A 的特征值。EDMD 得到的 A 通常会有几个特征值接近单位圆代表旋翼的欠阻尼运动这本身不一定是错的但如果特征值实部明显大于 1模型基本不可用要么换字典要么检查数据是否覆盖了工作点。特征值检查代码eigA eig(A); % 画出复平面 figure; plot(real(eigA), imag(eigA), kx); hold on; rectangle(Position,[-1,-1,2,2],Curvature,[1 1],EdgeColor,r); axis equal; grid on;单位圆画出来一眼就能看出有没有发散模式。我还会单独打印按模长排序的前五个特征值看有没有接近 1 的积分模式这个模式对应位置漂移是四旋翼正常特性不需要处理。4. 基于Koopman模型的MPC控制轨迹跟踪与约束实现4.1 把Koopman模型转成线性MPC问题有了 A、B下一步是把它变成一个标准线性状态空间模型直接在滚动时域控制器里使用。由于 z 是高维提升状态我们通常只对原始状态感兴趣。输出矩阵 C 的作用就是从 z 里把线性项对应的状态取出来。因为我在build_poly_psi里把线性项放在第 2 到第 n_state1 列所以C zeros(n_state, n_basis); C(:, 2:n_state1) eye(n_state);这个 C 会用在 MPC 的成本函数里。成本函数我写成 0.5 倍的标准二次型方便直接喂给quadprogJ 0.5 * sum_{k0}^{N} (C z_k - r_k) Q (C z_k - r_k) 0.5 * sum_{k0}^{N-1} u_k R u_k其中 N 是预测时域r_k 是参考轨迹Q 是状态权重R 是输入权重。如果有人想用输入变化率约束可以在 QP 的约束里加成本项也可以加 S但初次调通建议先只惩罚 u把通道理顺再加。为了减少优化变量数我把状态序列用初始提升状态 z0 和输入序列 U 展开。定义 z_k A^k z0 sum_{j0}^{k-1} A^{k-1-j} B u_j把所有 z_k 拼成列向量 z_all那么 z_all S_z * z0 S_u * U。其中 U [u0; u1; ; u_{N-1}]。代码里用循环构建 S_z 和 S_uN 10; % 预测时域 nu size(B, 2); % 输入维度四旋翼通常是4 nz n_basis; S_z zeros(nz*(N1), nz); S_u zeros(nz*(N1), N*nu); S_z(1:nz, 1:nz) eye(nz); for k 1:N % 更新 S_z 中的第 k 块行: A^k S_z(k*nz1:(k1)*nz, :) A * S_z((k-1)*nz1:k*nz, :); % 更新 S_u 中的第 k 块行 for j 0:k-1 AkB A^(k-1-j) * B; S_u(k*nz1:(k1)*nz, j*nu1:(j1)*nu) AkB; end end逻辑说明S_z 每一块行是 A 的幂次乘 z0第一块是单位阵对应 z0 本身S_u 的第二块行第一列是 B对应 u0 对 z1 的影响第三块行第一列是 A*B第二列是 B分别对应 u0、u1 对 z2 的影响。这样循环生成的是标准可控性矩阵。有了 S_z 和 S_uMPC 的目标就变成关于 U 的二次函数。设C_aug kron(eye(N1), C)Qbar kron(eye(N1), Q)Rbar kron(eye(N), R)则C_aug kron(eye(N1), C); Qbar kron(eye(N1), Q); Rbar kron(eye(N), R); % quadprog 最小化 0.5*U*H*U f*U H S_u * C_aug * Qbar * C_aug * S_u Rbar;对于每个滚动时域步f 会随 z0 变化在循环里更新f -S_u * C_aug * Qbar * (C_aug * S_z * z0 - r_all);其中 r_all 是参考轨迹按时间展开的列向量。因为我把成本写成了 0.5 倍quadprog 的 H、f 不需要额外乘系数。4.2 四旋翼状态约束与输入约束的设置四旋翼的物理约束主要集中在电机推力饱和、姿态角限幅和角速度限幅。在压缩变量框架下U 本身就是优化变量输入幅值约束可以直接用 quadprog 的 lb、ub 参数% 输入上下限例如推力0~1 lb zeros(N*nu, 1); ub ones(N*nu, 1);姿态角约束需要转换成对 U 的线性不等式。例如滚转角的上下限是 ±45°对应输出 y_k C z_k 的第 7~9 个状态取决于你状态排列。把所有预测步的输出写成C_aug * (S_z*z0 S_u*U)那么% 假设 y_lb、y_ub 是 N1 步重复的约束向量 Aineq [C_aug * S_u; -C_aug * S_u]; bineq [y_ub - C_aug * S_z * z0; -y_lb C_aug * S_z * z0];注意 bineq 中包含 z0所以 Aineq 可以循环外预计算bineq 每个步长要更新。我一般先把 C_augS_u 和 C_augS_z 存下来避免重复大矩阵乘法。输入变化率约束也是工程里必不可少的否则电机指令会高频跳动。一种简化办法是把相邻时刻的输入差值写成约束。设 D 是 (N-1)nu × Nnu 的差分矩阵D U du_max表示正向跳变上限-D U du_max表示反向跳变上限。再加上当前时刻的 u0 与上一拍 u_prev 的差值约束。代码这里不展开实际操作时我会单独写一个函数去构造 D然后拼到 Aineq、bineq 里。约束太多时quadprog 的求解时间会上升所以要控制 N 和约束数量。4.3 仿真闭环控制效果与动态特性我用最朴素的办法直接调quadprog因为n_basis91、N10的规模并不大Matlab 的quadprog完全能实时算。下面是一个滚动时域控制循环核心options optimoptions(quadprog, Display, off); % 预计算与 z0 无关的矩阵 S_u_pre C_aug * S_u; S_z_pre C_aug * S_z; x x0; z0 build_poly_psi(x, 2); u_prev zeros(nu, 1); for t 1:Tsim % 生成参考轨迹 r_all这里是悬停参考 r_all repmat(r_ref, N1, 1); % 计算线性项 f f -S_u_pre * Qbar * (S_z_pre * z0 - r_all); % 更新 bineq 中与 z0 有关的部分 bineq [y_ub - S_z_pre * z0; -y_lb S_z_pre * z0]; Uopt quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options); % 取第一个输入 u Uopt(1:nu); u max(min(u, 1), 0); % 物理饱和 % 用非线性模型或真实动力学推进一步 x drone_dynamics(x, u, dt); z0 build_poly_psi(x, 2); end这里的drone_dynamics需要你自己提供仿真和半实物都行。跑完闭环后我最关心的指标是参考轨迹的跟踪误差 RMS以及四个电机的控制量是否有急变。Koopman 模型训练得好时悬停和小幅机动轨迹跟踪误差在厘米级如果出现低频振荡优先怀疑 Q 和 S 的权重比不对而不是去改字典。顺手放一组我常用的初始参数方便你起调参数数值说明N10预测时域约0.2秒Qdiag([1,1,1, 2,2,2, 0.5,0.5,0.5, 0.1,0.1,0.1])位置和速度权重高R0.01 * eye(4)推力权重lambda1e-4EDMD岭回归系数dt0.02s采样周期先让系统悬停再给一个 0.5m 位置阶跃观察超调量。超调大就加 Q 的位置项电机抖就加 R 或者加入输入变化率约束。不要同时调三个权重否则出了问题你都不知道是哪个参数引起的。5. 避坑与常见问题Koopman模型应用中的五个翻车点5.1 字典函数维度太高导致过拟合现象训练集上的单步预测 NMSE 低到 0.01但验证集的跟踪误差大得离谱甚至仿真发散。 原因二阶或三阶多项式在 12 维状态下会产生大量交叉项数据量不足时最小二乘解把噪声也学进去了。 解决第一先固定用二阶多项式把字典维度和训练样本量比例控制在 1:20 以上第二加岭回归正则化lambda从 1e-3 往上试第三如果仍然过拟合减少字典中的非必要交叉项比如只保留速度项和角速度项之间的交叉不保留位置项和姿态项的交叉。5.2 训练数据未覆盖飞行包线预测发散现象离线验证时只用了悬停附近的数据看起来很好。一旦给 MPC 一个较大的阶跃指令预测轨迹在几步内就飞到天上。 原因EDMD 本质是函数拟合对训练集覆盖范围以外的状态多项式项会按外推趋势放大可能比真实动力学还夸张。 解决训练数据里要混入大机动段比如 45° 滚转、俯仰的扫掠以及加减速过程。我一般把总数据的三分之一放在大机动区三分之一放在过渡区三分之一悬停。这样模型至少见过整个工作包线的边界。5.3 特征值实部大于1线性模型不稳定现象开环预测时即使不给控制z 也指数发散。 原因可能有两种。一种是符号约定问题A 矩阵的行列写反了另一种是字典函数不足以覆盖系统的耗散特征尤其是没有把阻力项写成负的平方项。 解决先用eig(A)排查如果特征值大于 1检查训练数据里有没有长时间的自由衰减段。没有的话补一段从大初速度滑翔衰减到悬停的数据。另外A 有一个特征值理论上是 1 对应积分作用和四旋翼位置积分对应这个不用慌。5.4 Matlab OOP与函数句柄的性能坑现象EDMD 训练时间几十秒到几分钟但其中构建字典占用 90% 的时间控制循环里每个步长都调用函数句柄慢到跑不动。 原因很多教学代码用arrayfun或者自定义类的feval一个点一个点计算字典Matlab 的循环和函数调用开销很大。 解决字典函数如build_poly_psi全部写成向量化操作只操作矩阵的行不逐元素循环控制循环里把 MPC 的权重矩阵、约束矩阵在循环外提前算好循环内只改参考轨迹和初始状态。我在一个项目中把 EDMD 训练从 80 秒压到 2 秒就是这个原因。5.5 与真实四旋翼模型接口的时序对齐问题现象仿真里 MPC 效果不错接到实物飞控后却出现明显相位滞后跟踪响应慢半拍。 原因真实系统有状态估计延迟、控制计算时间、执行器响应延迟而训练 EDMD 时用的是理想同步时序(x_k, u_k, x_{k1})。 解决在训练数据里主动把输入通道延迟 1~3 个采样周期让模型学到的是“当前状态历史输入→下一状态”的因果规律。具体延迟步数可以通过计算输入与状态之间的互相关确定。这是数据驱动控制上真机时最容易被忽略的一步也是我反复踩过的坑。6. 进阶技巧增量更新EDMD与在线Koopman控制6.1 用滑动窗口增量更新Koopman矩阵离线训练出的 A、B 只适用于固定工况。换一架起飞重量不同的四旋翼或者环境风场明显变化模型就会失效。一个轻量级的做法是用滑动窗口做增量更新每来一个新样本把它加入训练窗口同时丢一个旧样本重新计算 A、B。因为 M*M 和 M*Z 是累加结构可以用递推公式避免全部重算。Matlab 里可以直接维护 M*M 和 M*Z新样本来了做秩一更新速度快很多。要注意窗口太长会反应慢窗口太短则参数抖我一般取 200~500 个样本。6.2 参数自适应字典函数中心点随工况移动如果用的是高斯 RBF 字典中心点和宽度可以随状态均值漂移。每跑一段控制后计算当前工况附近的中心点密度把落在低密度区域的基函数中心移动到高频访问区域。这样不需要重新训练只需重算中心点并在下一次增量更新时调整字典矩阵。多项式字典没有中心点但可以做状态的平滑切换预先训练悬停模型和大机动模型根据当前姿态角实时线性插值两个模型的 A、B这个办法在工程里也很实用。在增量更新里要小心正则化项。每更新一次lambda都不变的情况下累积误差可能让 A 的特征值缓慢漂出单位圆。我每次更新后都会重新检查特征值和多步预测误差一旦超出阈值就回滚到上一版参数。这个动作比更新本身更重要否则模型会在不知不觉中退化等你发现时已经救不回来了。从那以后我每次换一个四旋翼平台都会强制走一遍“数据采集 → 标准化 → EDMD 训练 → 多步预测验证 → 延迟补偿确认”这五步再进入 MPC 调参。前两步偷懒后面调试时间至少翻一倍。这个流程救了我很多次希望帮到你。本文还有配套的精品资源点击获取