Koopman-MPC框架在四旋翼无人机控制中的应用

戴小青

1. 项目概述

四旋翼无人机的控制一直是机器人领域的热点研究方向。传统控制方法如PID在面对复杂非线性系统时往往捉襟见肘，而基于精确模型的非线性模型预测控制(NMPC)又面临计算复杂度高、实时性差等问题。本文介绍的Koopman-MPC框架提供了一种创新性的解决方案——通过数据驱动的方式将非线性系统"抬升"到高维线性空间，再利用成熟的线性MPC技术实现高效控制。

这个方法的独特之处在于它巧妙结合了两个关键思想：一是Koopman算子理论，可以将非线性动力学系统转化为无限维线性系统；二是扩展动态模态分解(EDMD)算法，能够从实际飞行数据中学习出有效的有限维近似。特别值得一提的是，我们采用了基于旋转矩阵的物理信息驱动可观测量，避免了传统欧拉角表示中的奇异性问题，使得模型在SE(3)流形上具有更好的全局性质。

2. 核心原理与技术路线

2.1 Koopman算子理论精要

Koopman算子的核心思想是将非线性系统的状态空间映射到一个函数空间，在这个新的空间中，系统的演化表现为线性动力学。具体来说：

对于离散时间非线性系统 xₖ₊₁ = f(xₖ)，Koopman算子 K 作用于可观测函数 g 满足：Kg(xₖ) = g(f(xₖ))
通过选择适当的可观测函数基（如多项式、径向基函数等），我们可以获得有限维的线性近似
这个线性表示保留了原始非线性系统的全局特性，而非常规方法中的局部线性化

在实际应用中，我们通常采用数据驱动的方式学习Koopman算子。EDMD算法通过收集系统轨迹的"快照"数据，构建如下优化问题：

min‖G₊ - AK G‖₂² + λ‖A‖₂²

其中G是观测数据矩阵，A是待求的Koopman矩阵，λ是正则化参数。这个最小二乘问题的解给出了Koopman算子的有限维近似。

2.2 四旋翼动力学建模创新

传统四旋翼建模通常采用欧拉角或四元数表示姿态，但这些方法存在固有缺陷：

欧拉角有万向节锁问题，在特定姿态下会失去一个自由度
四元数虽然避免了奇异性，但动力学方程仍然高度非线性

我们的创新点在于直接使用旋转矩阵R∈SO(3)表示姿态，结合角速度ω∈ℝ³，构建如下状态向量：

x = [R₁₁, R₁₂, ..., R₃₃, ω₁, ω₂, ω₃]ᵀ

这种表示方式具有几个关键优势：

完全避免了奇异性问题
自然地保持了SO(3)流形的拓扑结构
旋转矩阵与角速度的乘积项可以很好地捕捉姿态动力学中的非线性耦合效应

2.3 EDMD算法实现细节

在实际实现EDMD算法时，有几个技术细节需要特别注意：

数据预处理：
- 对原始传感器数据进行低通滤波，去除高频噪声
- 对姿态数据使用四元数插值确保平滑性
- 采用时间对齐技术处理不同传感器的延迟
可观测函数选择：
- 基础函数：包括状态变量本身（保真性）
- 非线性项：旋转矩阵元素的两两乘积（捕捉耦合效应）
- 物理启发项：角速度的叉积项（反映陀螺效应）
正则化处理：
- 采用Tikhonov正则化防止过拟合
- 通过交叉验证选择最优正则化参数
- 对不同的状态变量施加不同的正则化强度

3. 模型预测控制器设计

3.1 线性MPC问题构建

基于学习得到的Koopman线性模型，我们可以构建如下MPC优化问题：

min_{u} ∑(x̂ₖᵀQx̂ₖ + uₖᵀRuₖ) + x̂_NᵀPx̂_N
s.t. x̂ₖ₊₁ = Âx̂ₖ + B̂uₖ
u_min ≤ uₖ ≤ u_max
Δu_min ≤ uₖ - uₖ₋₁ ≤ Δu_max

其中：

x̂是提升后的状态（包含原始状态和可观测函数）
Q,R,P是权重矩阵，平衡状态误差和控制代价
输入约束反映了电机物理限制
输入变化率约束确保平滑性

3.2 实时优化技巧

为了实现100Hz的实时控制，我们采用了以下优化技巧：

热启动：使用上一时刻的解作为当前优化的初始猜测
稀疏化：利用Koopman矩阵的稀疏结构加速QP求解
代码生成：使用CVXGEN工具生成定制化的QP求解器
并行计算：将预测时域内的矩阵运算分配到多核CPU

3.3 抗干扰策略

针对风扰等外部干扰，我们在基础框架上增加了两项增强：

扰动观测器：设计Luenberger观测器估计持续干扰
自适应权重：根据跟踪误差动态调整MPC代价函数权重

4. MATLAB实现关键代码解析

4.1 数据收集与预处理

matlab复制% 数据收集参数设置
dataParams = struct(...
    'samplingTime', 0.01, ...    % 10ms采样周期
    'duration', 60, ...          % 60秒记录时长
    'inputRange', [0.7, 1.3], ... % 输入激励范围
    'frequency', [0.1, 5]);      % 激励信号频率范围

% 生成扫频激励信号
t = 0:dataParams.samplingTime:dataParams.duration;
u = chirp(t, dataParams.frequency(1), dataParams.duration, ...
          dataParams.frequency(2)) * diff(dataParams.inputRange)/2 + ...
          mean(dataParams.inputRange);

% 数据预处理函数
function [cleanData] = preprocessData(rawData)
    % 低通滤波
    [b,a] = butter(4, 10/(100/2)); 
    cleanData = filtfilt(b, a, rawData);
    
    % 异常值处理
    medianData = movmedian(cleanData, 5);
    diffData = abs(cleanData - medianData);
    cleanData(diffData > 3*std(diffData)) = medianData(diffData > 3*std(diffData));
end

4.2 EDMD核心算法实现

matlab复制function [A, B, C, obsFunc] = edmd(X, Y, U, obsFuncType)
    % 构建可观测函数矩阵
    switch obsFuncType
        case 'poly2'
            Psi_X = [X; kron(X, X)]; 
            Psi_Y = [Y; kron(Y, Y)];
        case 'so3'
            % SO(3)特定的可观测函数
            R = reshape(X(1:9,:), [3,3,size(X,2)]);
            omega = X(10:12,:);
            Psi_X = [X; 
                    reshape(pagemtimes(R, reshape(omega,[3,1,size(X,2)])), [3,size(X,2)])];
            Psi_Y = [Y;
                    reshape(pagemtimes(reshape(Y(1:9,:),[3,3,size(Y,2)]), ...
                    reshape(Y(10:12,:),[3,1,size(Y,2)])), [3,size(Y,2)])];
    end
    
    % 添加常值项
    Psi_X = [ones(1,size(Psi_X,2)); Psi_X];
    Psi_Y = [ones(1,size(Psi_Y,2)); Psi_Y];
    
    % 构建增广矩阵
    Z = [Psi_X; U];
    
    % 最小二乘求解
    AB = Psi_Y * pinv(Z);
    A = AB(:,1:size(Psi_X,1));
    B = AB(:,size(Psi_X,1)+1:end);
    
    % 输出矩阵C (选择原始状态)
    C = [zeros(size(X,1),1), eye(size(X,1)), zeros(size(X,1),size(Psi_X,1)-size(X,1)-1)];
    
    % 返回可观测函数句柄
    obsFunc = @(x) evalObsFunc(x, obsFuncType);
end

function psi = evalObsFunc(x, type)
    % 可观测函数求值
    switch type
        case 'poly2'
            psi = [1; x; kron(x,x)];
        case 'so3'
            R = reshape(x(1:9),3,3);
            omega = x(10:12);
            psi = [1; x; R*omega];
    end
end

4.3 MPC控制器实现

matlab复制classdef KoopmanMPC < handle
    properties
        A, B, C           % Koopman模型矩阵
        Q, R, P           % 代价权重
        N                 % 预测时域
        umin, umax        % 输入约束
        dumin, dumax      % 输入变化率约束
        solver            % QP求解器
        prev_u            % 上一时刻输入
    end
    
    methods
        function obj = KoopmanMPC(A, B, C, Q, R, P, N, umin, umax, dumin, dumax)
            % 初始化控制器参数
            obj.A = A; obj.B = B; obj.C = C;
            obj.Q = Q; obj.R = R; obj.P = P;
            obj.N = N;
            obj.umin = umin; obj.umax = umax;
            obj.dumin = dumin; obj.dumax = dumax;
            obj.prev_u = zeros(size(B,2),1);
            
            % 初始化QP求解器
            obj.initQP();
        end
        
        function initQP(obj)
            % 构建QP问题的矩阵形式
            nx = size(obj.A,1); nu = size(obj.B,2);
            
            % 构建预测矩阵
            Sx = zeros(nx*obj.N, nx);
            Su = zeros(nx*obj.N, nu*obj.N);
            for k = 1:obj.N
                Sx((k-1)*nx+1:k*nx,:) = obj.A^k;
                for j = 1:k
                    Su((k-1)*nx+1:k*nx,(j-1)*nu+1:j*nu) = obj.A^(k-j)*obj.B;
                end
            end
            
            % 构建QP矩阵
            Qbar = blkdiag(kron(eye(obj.N-1),obj.Q), obj.P);
            Rbar = kron(eye(obj.N),obj.R);
            H = Su'*Qbar*Su + Rbar;
            f = @(x,r) Sx'*Qbar*Su*(x - repmat(r,obj.N,1));
            
            % 构建约束矩阵
            Aineq = [eye(nu*obj.N); -eye(nu*obj.N); 
                    tril(ones(nu*obj.N)); -tril(ones(nu*obj.N))];
            bineq = [repmat(obj.umax,obj.N,1); -repmat(obj.umin,obj.N,1);
                    repmat(obj.dumax,obj.N,1) + [obj.prev_u; zeros((obj.N-1)*nu,1)];
                    -repmat(obj.dumin,obj.N,1) - [obj.prev_u; zeros((obj.N-1)*nu,1)]];
            
            % 保存QP参数
            obj.solver = struct('H',H, 'f',f, 'Aineq',Aineq, 'bineq',bineq);
        end
        
        function [u, x_pred] = solve(obj, x, r)
            % 求解MPC问题
            H = obj.solver.H;
            f = obj.solver.f(x,r);
            Aineq = obj.solver.Aineq;
            bineq = obj.solver.bineq;
            
            % 调用QP求解
            options = optimoptions('quadprog', 'Display', 'off');
            U = quadprog(H, f, Aineq, bineq, [], [], [], [], [], options);
            
            % 提取控制输入
            nu = size(obj.B,2);
            u = U(1:nu);
            obj.prev_u = u;
            
            % 预测状态轨迹
            x_pred = zeros(size(obj.A,1), obj.N);
            x_pred(:,1) = obj.A*x + obj.B*u;
            for k = 2:obj.N
                x_pred(:,k) = obj.A*x_pred(:,k-1) + obj.B*U((k-1)*nu+1:k*nu);
            end
        end
    end
end

5. 实验验证与结果分析

5.1 测试环境配置

我们搭建了完整的仿真测试平台，主要配置如下：

硬件环境：
- 处理器：Intel i7-11800H @ 4.6GHz
- 内存：32GB DDR4
- 操作系统：Ubuntu 20.04 LTS
软件环境：
- MATLAB 2021b
- ROS Noetic (用于硬件在环测试)
- Gazebo 11 (物理仿真)
无人机参数：
- 质量：1.2 kg
- 轴距：0.45 m
- 最大推力：25 N
- 惯性矩阵：diag([0.03, 0.03, 0.06]) kg·m²