✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、引言
在无人机应用领域，当无人机携带悬挂载荷飞行时，其动力学行为变得复杂，精确控制面临诸多挑战。Koopman 模型预测控制（MPC）作为一种先进的控制策略，为解决这一问题提供了有效途径。它通过构建 Koopman 模型来捕捉系统的动态特性，并利用模型预测控制的优势，实现对 3D 无人机携带悬挂载荷的高效控制。
二、无人机 - 悬挂载荷系统动力学
三、Koopman 模型与训练
-
状态提升：
-
Koopman 理论基于对系统状态的提升操作，将原始低维状态空间映射到高维空间，使得非线性系统在高维空间中呈现出线性的动力学特性。在本系统中，对 10 维的原始状态向量 x 进行提升，除了包含原始状态变量外，还添加了悬挂载荷摆动角度的正弦和余弦值（sin(θx),cos(θx),sin(θy),cos(θy)），形成更高维的状态向量 z。
-
-
训练数据生成：
-
为了学习 Koopman 模型，需要生成大量的训练数据。在悬停状态附近生成有界的随机扰动作为控制输入，与悬停控制输入叠加后作用于系统。
-
对每次迭代得到的系统状态进行检查，若出现异常（如状态变量过大、包含非数或无穷大），则重新生成初始状态，以确保生成的训练数据有效。通过多次迭代，记录系统的当前状态 Xtrain、下一时刻状态 Ytrain 以及对应的控制输入 Utrain。
-
-
Koopman 模型学习：
-
将提升后的状态数据 Z 和控制输入数据 Utrain 组合成矩阵 W，利用正则化的最小二乘法求解 Koopman 矩阵 Akoop 和控制输入矩阵 Bkoop。正则化参数 λ=1e−6 用于防止过拟合，确保模型的泛化能力。
-
四、模型预测控制（MPC）
-
预测矩阵构建：
-
根据学习得到的 Koopman 矩阵 Akoop 和控制输入矩阵 Bkoop，以及设定的预测时域 N，构建状态预测矩阵 Sz 和控制输入预测矩阵 Su。这些矩阵用于预测系统在未来 N 个时间步内的状态变化，是 MPC 实现的基础。
-
-
代价函数设计：
-
MPC 的目标是在满足系统动力学约束的前提下，最小化代价函数。代价函数由两部分组成：状态跟踪误差项和控制输入变化项。
-
状态跟踪误差项通过状态权重矩阵 Qx 对不同状态变量的跟踪误差进行加权，强调对重要状态的精确跟踪；控制输入变化项通过控制输入权重矩阵 R 对控制输入的变化进行加权，避免控制输入的剧烈变化，确保系统的平稳运行。
-
通过克罗内克积等操作，将状态权重矩阵和控制输入权重矩阵扩展到预测时域上，构建完整的代价函数矩阵 H 和梯度向量 f。
-
-
求解最优控制输入：
-
将构建好的代价函数矩阵 H 和梯度向量 f，以及控制输入的上下限 Umin 和 Umax 作为输入，利用投影梯度法求解二次规划（QP）问题，得到最优控制输入 Uopt。
-
对最优控制输入进行处理，提取前三个元素作为实际的控制输入 u，并加上悬停所需的垂直力，同时将控制输入限制在允许的范围内。
-
⛳️ 运行结果
📣 部分代码
% ============================================================% COMPARISON PLOTS: LQR vs KOOPMAN STUDY (EXP SET)% ============================================================clear; clc; close all;%% ============================================================% Load experiments% ============================================================exp1 = load('exp_lqr_mpc_nominal.mat').data;exp2 = load('exp_koopman_nominal.mat').data;exp3 = load('exp_lqr_wind.mat').data;exp4 = load('exp_koopman_wind.mat').data;exp5 = load('exp_koopman_mismatch.mat').data;%% ============================================================% Extract% ============================================================T1 = exp1.T; X1 = exp1.X;T2 = exp2.T; X2 = exp2.X;T3 = exp3.T; X3 = exp3.X;T4 = exp4.T; X4 = exp4.X;T5 = exp5.T; X5 = exp5.X;%% ============================================================% Reference trajectory% ============================================================T_ref = linspace(0, 60, 1000)';A_horiz = 1.5;f_horiz = 0.1;z0 = 1.0;slope = 0.02;vx = 0.5;Xref = vx * T_ref;Yref = A_horiz * sin(2*pi*f_horiz*T_ref);Zref = z0 + slope * T_ref;%% ============================================================% ============================================================% EXP 1: NOMINAL LQR vs KOOPMAN% ============================================================%% ============================================================% ---------------- 3D TRAJECTORY ----------------figure('Name','Nominal 3D','Color','w');plot3(X1(:,1),X1(:,3),X1(:,5),'b','LineWidth',1.2); hold on;plot3(X2(:,1),X2(:,3),X2(:,5),'r','LineWidth',1.2);plot3(Xref,Yref,Zref,'k--');grid on; xlabel('X'); ylabel('Y'); zlabel('Z');title('Nominal 3D Trajectory');legend('LQR-MPC','Koopman','Reference');% ---------------- STATES ----------------figure('Name','Nominal States','Color','w');subplot(3,1,1);plot(T1,X1(:,1),'b',T2,X2(:,1),'r',T_ref,Xref,'k--'); grid on;ylabel('X');subplot(3,1,2);plot(T1,X1(:,3),'b',T2,X2(:,3),'r',T_ref,Yref,'k--'); grid on;ylabel('Y');subplot(3,1,3);plot(T1,X1(:,5),'b',T2,X2(:,5),'r',T_ref,Zref,'k--'); grid on;ylabel('Z'); xlabel('t');% ---------------- SWING ----------------figure('Name','Nominal Swing','Color','w');subplot(2,1,1);plot(T1,X1(:,7),'b',T2,X2(:,7),'r'); grid on;ylabel('\theta_x');subplot(2,1,2);plot(T1,X1(:,9),'b',T2,X2(:,9),'r'); grid on;ylabel('\theta_y'); xlabel('t');%% ============================================================% EXP 2: WIND LQR vs KOOPMAN% ============================================================figure('Name','Wind 3D','Color','w');plot3(X3(:,1),X3(:,3),X3(:,5),'b','LineWidth',1.2); hold on;plot3(X4(:,1),X4(:,3),X4(:,5),'r','LineWidth',1.2);plot3(Xref,Yref,Zref,'k--');grid on;title('Wind Disturbance 3D');legend('LQR Wind','Koopman Wind','Reference');figure('Name','Wind States','Color','w');subplot(3,1,1);plot(T3,X3(:,1),'b',T4,X4(:,1),'r',T_ref,Xref,'k--'); grid on;ylabel('X');subplot(3,1,2);plot(T3,X3(:,3),'b',T4,X4(:,3),'r',T_ref,Yref,'k--'); grid on;ylabel('Y');subplot(3,1,3);plot(T3,X3(:,5),'b',T4,X4(:,5),'r',T_ref,Zref,'k--'); grid on;ylabel('Z'); xlabel('t');figure('Name','Wind Swing','Color','w');subplot(2,1,1);plot(T3,X3(:,7),'b',T4,X4(:,7),'r'); grid on;ylabel('\theta_x');subplot(2,1,2);plot(T3,X3(:,9),'b',T4,X4(:,9),'r'); grid on;ylabel('\theta_y'); xlabel('t');%% ============================================================% EXP 3: KOOPMAN NOMINAL vs MISMATCH% ============================================================figure('Name','Koopman Robust 3D','Color','w');plot3(X2(:,1),X2(:,3),X2(:,5),'b','LineWidth',1.2); hold on;plot3(X5(:,1),X5(:,3),X5(:,5),'r','LineWidth',1.2);plot3(Xref,Yref,Zref,'k--');grid on;title('Koopman Robustness (Nominal vs Mismatch)');legend('Nominal','Mismatch','Reference');figure('Name','Koopman Robust States','Color','w');subplot(3,1,1);plot(T2,X2(:,1),'b',T5,X5(:,1),'r',T_ref,Xref,'k--'); grid on;ylabel('X');subplot(3,1,2);plot(T2,X2(:,3),'b',T5,X5(:,3),'r',T_ref,Yref,'k--'); grid on;ylabel('Y');subplot(3,1,3);plot(T2,X2(:,5),'b',T5,X5(:,5),'r',T_ref,Zref,'k--'); grid on;ylabel('Z'); xlabel('t');figure('Name','Koopman Robust Swing','Color','w');subplot(2,1,1);plot(T2,X2(:,7),'b',T5,X5(:,7),'r'); grid on;ylabel('\theta_x');subplot(2,1,2);plot(T2,X2(:,9),'b',T5,X5(:,9),'r'); grid on;ylabel('\theta_y'); xlabel('t');%% ============================================================% ERROR METRICS% ============================================================rmse = @(X,T) sqrt(mean((X(:,1) - interp1(T_ref, Xref, T)).^2));fprintf('\n===== RMSE (X tracking) =====\n');fprintf('LQR nominal : %.4f\n', rmse(X1,T1));fprintf('Koopman nominal : %.4f\n', rmse(X2,T2));fprintf('LQR wind : %.4f\n', rmse(X3,T3));fprintf('Koopman wind : %.4f\n', rmse(X4,T4));fprintf('Koopman mismatch : %.4f\n', rmse(X5,T5));
扫一扫
356
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
