用数据把非线性无人机变成线性系统Koopman-MPC这套思路最近在控制圈讨论度很高。这个项目把Koopman算子理论、数据驱动建模和模型预测控制结合在一起做了一套完整的四旋翼无人机控制方案全部基于Matlab实现。简单说它的核心价值在于以前处理四旋翼这种强非线性系统要么得费劲推导线性化模型要么得靠PID这种不太在意模型精度的办法硬扛。而现在我们可以直接从飞行数据里学出一个高维线性模型再在这个模型上跑MPC既能保留MPC的前瞻优化能力又不需要面对非线性优化的求解负担。这篇分享从原理到代码实现、从参数调到坑实录完整过一遍这套方案的落地过程适合研究无人机控制方向的研究生也适合想给传统控制方案换一种新思路的工程师。1. 项目核心思路为什么非要用Koopman算子把系统“拉直”先说清楚这项目到底在折腾什么。四旋翼无人机在空中的运动本质上是刚体动力学加上空气动力学的耦合姿态和位置之间还存在着强非线性关系。你油门推大一点机体倾斜升力方向跟着变水平方向就开始加速这中间还夹着重力、陀螺力矩、空气阻力这些因素。传统的做法是拿一个小扰动线性化模型去近似比如悬停点附近的线性化方程然后在这个近似模型上设计控制器。这个方法不是不能用但你想做大机动、大角度姿态变化时小扰动假设就撑不住了模型误差一大控制器性能就明显掉下来。那Koopman算子是干什么的呢。它提供了一种很巧妙的视角一个非线性动力系统可以在一个无限维的函数空间里被表示成一个线性系统。好比你在原来的状态空间里看到的是各种弯弯绕绕的轨迹但如果找到一个合适的观测函数lifting function把这些状态映射到一个更高维的空间在这个新空间里动力学的演化竟然变成了线性映射。这就是把系统拉直的意思。实际的工程做法当然不可能真用无限维空间但我们可以用数据驱动的方式在有限维空间里近似这个Koopman算子。最常用的方法是EDMDExtended Dynamic Mode Decomposition扩展动态模态分解。你把飞行数据分成快照对每个快照对是一个状态点和他下一步的状态点然后在这些数据上做回归拟合一个线性矩阵 K 使得这些快照对的演化关系在提升后的空间里尽量精确。K 矩阵就是Koopman算子在这个有限维子空间里的近似。有了这个线性模型之后MPC就变得特别好用了。标准的MPC是个在线优化问题每一控制周期都要解一个带约束的优化问题。如果系统模型是线性的那这个优化问题就变成了一个标准的二次规划QP求解速度快还有成熟求解器可以用稳定性和实时性都好处理。非线性 MPC 虽然也能做但每一步都要处理非线性优化问题求解代价高而且非线性求解器的收敛性有时候让你头疼。Koopman-MPC 的本质就是用数据把非线性系统伪装成线性系统吃下MPC的优化框架两头的好处都占到。这里要插一句我对这套思路的总体评价Koopman-MPC 这个方向很吸引人关键在于它代表了一种模型来源的范式变化。以前我们的模型要么靠物理推导要么靠系统辨识在固定模型结构里拟合参数。Koopman 方法跳出了固定的模型结构假设直接从数据里长出一个线性推进器某种程度上是数据驱动和模型预测控制之间的一座很自然的桥梁。2. Koopman算子与EDMD算法核心原理怎么落地成代码很多看这个项目的人卡在理解 Koopman 算子的数学概念上。这里我换个说法来试着说明白。你可以把动力系统想象成一台复杂的机器从外面看这台机器是个黑箱输入一个状态输出下一个状态非线性系统意味着输入变化和输出变化之间还存在着各种各样的拐弯关系。Koopman 算子做的事情相当于你把这台机器放到一个更大的展厅里换了一组探针去观测它而这些探针的读数组合在一起后这台机器的行为突然变成了线性的。关键问题来了这些探针也就是提升函数到底应该怎么选EDMD 的做法是选一组基函数库比如多项式函数、正弦余弦函数、径向基函数等。把这些基函数作用到原始状态上生成一个更高维的提升状态。假设原始状态是 x∈R^n选了 N 个基函数提升状态就是 z Φ(x) ∈ R^N一般 N 远大于 n。我们可以把这个过程理解成原来12维的无人机状态位置3维、速度3维、姿态四元数4维、角速度3维去掉冗余约束之后实际动力学用12维状态提升到50维甚至100维的空间里去。EDMD 的具体计算过程是这样走从仿真或者实验中采集数据得到一系列快照对 (x_k, x_{k1})。对每一个快照对计算提升状态 z_k Φ(x_k)z_{k1} Φ(x_{k1})。构造两个矩阵 Z1 和 Z2Z1 的每一列是 z_kZ2 的每一列是 z_{k1}。求解最小二乘问题K Z2 * Z1^这里的 Z1^ 是 Z1 的伪逆。这个过程在Matlab里实现起来非常直接核心代码就几行。我自己写的时候会用 [U, S, V] svd(Z1, econ) 然后做截断SVD来求伪逆比直接用 pinv 更快也更稳特别是在数据量大或者矩阵条件数不太好的时候。% 数据矩阵: snapshots 是原始状态快照对 Z liftFunction(snapshots); % (N, numSnapshots) Z_next liftFunction(snapshotsNext); % (N, numSnapshots) % 截断SVD求伪逆 [U, S, V] svd(Z, econ); s diag(S); r sum(s 1e-8); % 截断阈值按实际数据调节 K Z_next * V(:, 1:r) * diag(1 ./ s(1:r)) * U(:, 1:r);这个代码里最重要的是 liftFunction 函数的设计它直接决定了Koopman模型的近似精度。我前几次跑数据时随便选了一组基函数结果模型验证阶段误差大得离谱后来才发现问题出在基函数选择上。提升函数库的设计有一些经验性的原则基础状态本身必须包含在内保证线性反馈的完整表达空间。加入状态之间的耦合项比如位置乘速度、速度乘角速度这种交叉项能显著提升对非线性气动力项的逼近能力。正弦余弦项对姿态相关的周期变化有帮助四旋翼的升力和姿态角是三角函数关系这组基函数不能漏。基函数数量要够但也不是越多越好。数量太少拟合精度不够数量太多会遇到维数灾难而且观测矩阵会变得病态。我在这个项目里压测过不同维度30维时误差明显50维时效果不错到80维之后提升很有限反而不如50维稳定。关于EDMD的收敛性还有一个值得提的点。EDMD 本质上是在一个有限维子空间里对无限维Koopman算子做投影理论上随着数据量增大和子空间维度提升这个近似是收敛的。但在实际使用中你要保证数据覆盖了系统的工作区域。如果你训练数据只覆盖了悬停附近的小范围那学到得Koopman模型在大姿态角区域就是外推误差会急剧增大。所有采集数据时要有意识地做激励信号设计比如用不同幅度的扫频信号让无人机充分经历各个工作点。3. 四旋翼的Koopman-MPC控制器设计与Matlab实现细节3.1 整体框架和状态定义在Matlab里落地这套方案我把系统拆成四个模块数据生成模块、Koopman模型学习模块、MPC控制器模块、仿真验证模块。数据生成模块负责跑一组开环激励把飞行状态记录成数据集。Koopman模型学习模块用EDMD从数据集里拟合出线性推进器。MPC控制器模块基于这个线性模型做滚动优化输出控制量。仿真验证模块把控制器放回原始非线性仿真模型看看实际表现。状态定义这部分我直接用的是四旋翼的标准12维状态位置 (p_x, p_y, p_z)、速度 (v_x, v_y, v_z)、姿态角和角速度。姿态表示我用的是欧拉角而不是四元数原因是这个项目偏教学和演示性质欧拉角更直观好调试。如果你要飞大机动甚至倒飞那还是得换四元数避免万向锁问题。控制输入是四个电机的转速指令为了和MPC的线性框架匹配我把输入定义成相对悬停点的转速增量这样线性模型更准一些。具体而言悬停时四个电机有一个基准转速 ω0控制量 u 表示各个电机相对基准值的偏移量。3.2 提升函数的实操选型提升函数这块我试过好几版方案最后用的是一个混合组合。原始状态12维我在这个基础上加了18个提升基函数总维度是30。先看一眼效果还不够又加了角速度的二次项和速度-角速度交叉项总维度提到40多效果明显改善。后面再想加更高阶的项效果就不明显了。一个实战经验是提升函数的构造尽量利用你已经知道的物理知识。完全当黑箱来凑基函数虽然也可以但没必要。比如四旋翼的升力和电机转速的平方大致成正比总升力方向和姿态角之间有三角函数关系。你可以把这些认识放到基函数设计里比纯多项式拟合效率高很多。匹配物理直觉的基函数还有一个额外的好处数据需求会下降。我测试过如果基函数选得贴合物理结构5000个训练快照对就够用了而纯通用基函数可能要好几万条数据才能达到类似精度这在真实平台上意味着多飞好几个架次成本差别很大。3.3 MPC问题的建立与标准化求解Koopman 模型学出来之后就可以把它嵌进MPC框架。我用的MPC公式是标准的有限时域二次规划预测模型z_{k1} K * z_k B * u_k状态约束把原始状态约束映射到提升空间控制量约束电机转速限制、变化率限制目标函数跟踪误差加权项 控制量惩罚项注意一下Koopman 算子模型通常给出的是 z_{k1} K * z_k 加上输入项这里需要额外处理 B 矩阵。我通常的做法是在EDMD回归时把输入也纳入快照对构造把系统看成增广状态或者采用一种带输入EDMD的变体在回归时显式计算 B。这个细节容易踩坑后面在问题排查里会细讲。MPC的目标函数我写成这样% 二次规划标准形式: min 1/2 * U * H * U f * U % H 矩阵包含预测模型推导出的罚矩阵, f 和当前状态相关 H G * Q * G R; f (x_ref - Kp * z0) * Q * G; % 约束用线性不等式 A * U b 表示这里 G 是提升状态对控制序列的脉冲响应矩阵Q 和 R 是对应的权重矩阵。在Matlab里我直接调 quadprog 求解用 interior-point-convex 算法控制周期设在50毫秒。这个时间窗口在Matlab环境下够用因为提升空间的维度不高几十维预测时域取15步决策变量也就一百来个quadprog 求解非常快。3.4 轨迹跟踪的完整闭环流程把整个闭环串起来跑通我花了最长的时间。最开始我单独验证Koopman模型时效果还行一到闭环就出问题无人机位置控制慢半拍姿态还有小幅震荡。后来追查原因发现是预测模型在闭环工况下和开环辨识时的数据分布不一致典型的distribution shift问题。解决方案是在训练数据里加入一些闭环数据也就是先用一个保守的PID控制器让无人机飞一段轨迹把PID闭环数据也放进训练集里Koopman模型对闭环控制区域的覆盖能力就强了很多。这个闭环迭代训练的流程在文献里叫 iterative learning control 或者> Koopman-MPC：用数据把四旋翼无人机变成线性系统
用数据把非线性无人机变成线性系统Koopman-MPC这套思路最近在控制圈讨论度很高。这个项目把Koopman算子理论、数据驱动建模和模型预测控制结合在一起做了一套完整的四旋翼无人机控制方案全部基于Matlab实现。简单说它的核心价值在于以前处理四旋翼这种强非线性系统要么得费劲推导线性化模型要么得靠PID这种不太在意模型精度的办法硬扛。而现在我们可以直接从飞行数据里学出一个高维线性模型再在这个模型上跑MPC既能保留MPC的前瞻优化能力又不需要面对非线性优化的求解负担。这篇分享从原理到代码实现、从参数调到坑实录完整过一遍这套方案的落地过程适合研究无人机控制方向的研究生也适合想给传统控制方案换一种新思路的工程师。1. 项目核心思路为什么非要用Koopman算子把系统“拉直”先说清楚这项目到底在折腾什么。四旋翼无人机在空中的运动本质上是刚体动力学加上空气动力学的耦合姿态和位置之间还存在着强非线性关系。你油门推大一点机体倾斜升力方向跟着变水平方向就开始加速这中间还夹着重力、陀螺力矩、空气阻力这些因素。传统的做法是拿一个小扰动线性化模型去近似比如悬停点附近的线性化方程然后在这个近似模型上设计控制器。这个方法不是不能用但你想做大机动、大角度姿态变化时小扰动假设就撑不住了模型误差一大控制器性能就明显掉下来。那Koopman算子是干什么的呢。它提供了一种很巧妙的视角一个非线性动力系统可以在一个无限维的函数空间里被表示成一个线性系统。好比你在原来的状态空间里看到的是各种弯弯绕绕的轨迹但如果找到一个合适的观测函数lifting function把这些状态映射到一个更高维的空间在这个新空间里动力学的演化竟然变成了线性映射。这就是把系统拉直的意思。实际的工程做法当然不可能真用无限维空间但我们可以用数据驱动的方式在有限维空间里近似这个Koopman算子。最常用的方法是EDMDExtended Dynamic Mode Decomposition扩展动态模态分解。你把飞行数据分成快照对每个快照对是一个状态点和他下一步的状态点然后在这些数据上做回归拟合一个线性矩阵 K 使得这些快照对的演化关系在提升后的空间里尽量精确。K 矩阵就是Koopman算子在这个有限维子空间里的近似。有了这个线性模型之后MPC就变得特别好用了。标准的MPC是个在线优化问题每一控制周期都要解一个带约束的优化问题。如果系统模型是线性的那这个优化问题就变成了一个标准的二次规划QP求解速度快还有成熟求解器可以用稳定性和实时性都好处理。非线性 MPC 虽然也能做但每一步都要处理非线性优化问题求解代价高而且非线性求解器的收敛性有时候让你头疼。Koopman-MPC 的本质就是用数据把非线性系统伪装成线性系统吃下MPC的优化框架两头的好处都占到。这里要插一句我对这套思路的总体评价Koopman-MPC 这个方向很吸引人关键在于它代表了一种模型来源的范式变化。以前我们的模型要么靠物理推导要么靠系统辨识在固定模型结构里拟合参数。Koopman 方法跳出了固定的模型结构假设直接从数据里长出一个线性推进器某种程度上是数据驱动和模型预测控制之间的一座很自然的桥梁。2. Koopman算子与EDMD算法核心原理怎么落地成代码很多看这个项目的人卡在理解 Koopman 算子的数学概念上。这里我换个说法来试着说明白。你可以把动力系统想象成一台复杂的机器从外面看这台机器是个黑箱输入一个状态输出下一个状态非线性系统意味着输入变化和输出变化之间还存在着各种各样的拐弯关系。Koopman 算子做的事情相当于你把这台机器放到一个更大的展厅里换了一组探针去观测它而这些探针的读数组合在一起后这台机器的行为突然变成了线性的。关键问题来了这些探针也就是提升函数到底应该怎么选EDMD 的做法是选一组基函数库比如多项式函数、正弦余弦函数、径向基函数等。把这些基函数作用到原始状态上生成一个更高维的提升状态。假设原始状态是 x∈R^n选了 N 个基函数提升状态就是 z Φ(x) ∈ R^N一般 N 远大于 n。我们可以把这个过程理解成原来12维的无人机状态位置3维、速度3维、姿态四元数4维、角速度3维去掉冗余约束之后实际动力学用12维状态提升到50维甚至100维的空间里去。EDMD 的具体计算过程是这样走从仿真或者实验中采集数据得到一系列快照对 (x_k, x_{k1})。对每一个快照对计算提升状态 z_k Φ(x_k)z_{k1} Φ(x_{k1})。构造两个矩阵 Z1 和 Z2Z1 的每一列是 z_kZ2 的每一列是 z_{k1}。求解最小二乘问题K Z2 * Z1^这里的 Z1^ 是 Z1 的伪逆。这个过程在Matlab里实现起来非常直接核心代码就几行。我自己写的时候会用 [U, S, V] svd(Z1, econ) 然后做截断SVD来求伪逆比直接用 pinv 更快也更稳特别是在数据量大或者矩阵条件数不太好的时候。% 数据矩阵: snapshots 是原始状态快照对 Z liftFunction(snapshots); % (N, numSnapshots) Z_next liftFunction(snapshotsNext); % (N, numSnapshots) % 截断SVD求伪逆 [U, S, V] svd(Z, econ); s diag(S); r sum(s 1e-8); % 截断阈值按实际数据调节 K Z_next * V(:, 1:r) * diag(1 ./ s(1:r)) * U(:, 1:r);这个代码里最重要的是 liftFunction 函数的设计它直接决定了Koopman模型的近似精度。我前几次跑数据时随便选了一组基函数结果模型验证阶段误差大得离谱后来才发现问题出在基函数选择上。提升函数库的设计有一些经验性的原则基础状态本身必须包含在内保证线性反馈的完整表达空间。加入状态之间的耦合项比如位置乘速度、速度乘角速度这种交叉项能显著提升对非线性气动力项的逼近能力。正弦余弦项对姿态相关的周期变化有帮助四旋翼的升力和姿态角是三角函数关系这组基函数不能漏。基函数数量要够但也不是越多越好。数量太少拟合精度不够数量太多会遇到维数灾难而且观测矩阵会变得病态。我在这个项目里压测过不同维度30维时误差明显50维时效果不错到80维之后提升很有限反而不如50维稳定。关于EDMD的收敛性还有一个值得提的点。EDMD 本质上是在一个有限维子空间里对无限维Koopman算子做投影理论上随着数据量增大和子空间维度提升这个近似是收敛的。但在实际使用中你要保证数据覆盖了系统的工作区域。如果你训练数据只覆盖了悬停附近的小范围那学到得Koopman模型在大姿态角区域就是外推误差会急剧增大。所有采集数据时要有意识地做激励信号设计比如用不同幅度的扫频信号让无人机充分经历各个工作点。3. 四旋翼的Koopman-MPC控制器设计与Matlab实现细节3.1 整体框架和状态定义在Matlab里落地这套方案我把系统拆成四个模块数据生成模块、Koopman模型学习模块、MPC控制器模块、仿真验证模块。数据生成模块负责跑一组开环激励把飞行状态记录成数据集。Koopman模型学习模块用EDMD从数据集里拟合出线性推进器。MPC控制器模块基于这个线性模型做滚动优化输出控制量。仿真验证模块把控制器放回原始非线性仿真模型看看实际表现。状态定义这部分我直接用的是四旋翼的标准12维状态位置 (p_x, p_y, p_z)、速度 (v_x, v_y, v_z)、姿态角和角速度。姿态表示我用的是欧拉角而不是四元数原因是这个项目偏教学和演示性质欧拉角更直观好调试。如果你要飞大机动甚至倒飞那还是得换四元数避免万向锁问题。控制输入是四个电机的转速指令为了和MPC的线性框架匹配我把输入定义成相对悬停点的转速增量这样线性模型更准一些。具体而言悬停时四个电机有一个基准转速 ω0控制量 u 表示各个电机相对基准值的偏移量。3.2 提升函数的实操选型提升函数这块我试过好几版方案最后用的是一个混合组合。原始状态12维我在这个基础上加了18个提升基函数总维度是30。先看一眼效果还不够又加了角速度的二次项和速度-角速度交叉项总维度提到40多效果明显改善。后面再想加更高阶的项效果就不明显了。一个实战经验是提升函数的构造尽量利用你已经知道的物理知识。完全当黑箱来凑基函数虽然也可以但没必要。比如四旋翼的升力和电机转速的平方大致成正比总升力方向和姿态角之间有三角函数关系。你可以把这些认识放到基函数设计里比纯多项式拟合效率高很多。匹配物理直觉的基函数还有一个额外的好处数据需求会下降。我测试过如果基函数选得贴合物理结构5000个训练快照对就够用了而纯通用基函数可能要好几万条数据才能达到类似精度这在真实平台上意味着多飞好几个架次成本差别很大。3.3 MPC问题的建立与标准化求解Koopman 模型学出来之后就可以把它嵌进MPC框架。我用的MPC公式是标准的有限时域二次规划预测模型z_{k1} K * z_k B * u_k状态约束把原始状态约束映射到提升空间控制量约束电机转速限制、变化率限制目标函数跟踪误差加权项 控制量惩罚项注意一下Koopman 算子模型通常给出的是 z_{k1} K * z_k 加上输入项这里需要额外处理 B 矩阵。我通常的做法是在EDMD回归时把输入也纳入快照对构造把系统看成增广状态或者采用一种带输入EDMD的变体在回归时显式计算 B。这个细节容易踩坑后面在问题排查里会细讲。MPC的目标函数我写成这样% 二次规划标准形式: min 1/2 * U * H * U f * U % H 矩阵包含预测模型推导出的罚矩阵, f 和当前状态相关 H G * Q * G R; f (x_ref - Kp * z0) * Q * G; % 约束用线性不等式 A * U b 表示这里 G 是提升状态对控制序列的脉冲响应矩阵Q 和 R 是对应的权重矩阵。在Matlab里我直接调 quadprog 求解用 interior-point-convex 算法控制周期设在50毫秒。这个时间窗口在Matlab环境下够用因为提升空间的维度不高几十维预测时域取15步决策变量也就一百来个quadprog 求解非常快。3.4 轨迹跟踪的完整闭环流程把整个闭环串起来跑通我花了最长的时间。最开始我单独验证Koopman模型时效果还行一到闭环就出问题无人机位置控制慢半拍姿态还有小幅震荡。后来追查原因发现是预测模型在闭环工况下和开环辨识时的数据分布不一致典型的distribution shift问题。解决方案是在训练数据里加入一些闭环数据也就是先用一个保守的PID控制器让无人机飞一段轨迹把PID闭环数据也放进训练集里Koopman模型对闭环控制区域的覆盖能力就强了很多。这个闭环迭代训练的流程在文献里叫 iterative learning control 或者>