✅作者简介热爱科研的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));​ 参考文献往期回顾扫扫下方二维码