✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、引言
在低空飞行器的控制领域，精确的轨迹跟踪至关重要。然而，实际飞行过程中，飞行器面临参数不确定性（如质量和惯性的变化）以及外部干扰（如风力影响）的挑战。模型预测控制（MPC）及其扩展自适应非线性模型预测控制（Adaptive NMPC）为解决这些问题提供了有效途径。本文将深入探讨如何基于自适应 NMPC 与 MPC 实现针对参数不确定性和外部干扰的低空飞行器 3D 轨迹跟踪，并进行鲁棒性分析。
二、模型预测控制（MPC）基础
（一）MPC 原理
MPC 基于飞行器的动态模型，通过预测系统未来的状态，在每个采样时刻求解一个有限时域的优化问题。优化目标通常是最小化飞行器实际轨迹与期望 3D 轨迹之间的偏差，同时考虑控制输入的约束，如最大推力、最大转角等。在每个采样时刻，只将优化问题得到的第一个控制输入作用于飞行器，然后在下一个采样时刻，基于新的测量状态重新求解优化问题，从而实现滚动时域控制。
三、自适应非线性模型预测控制（Adaptive NMPC）
（一）自适应机制
参数估计：为应对飞行器质量和惯性等参数的不确定性，Adaptive NMPC 引入在线参数估计方法。例如，利用扩展卡尔曼滤波器（EKF）或无迹卡尔曼滤波器（UKF），根据飞行器的测量状态（如加速度、角速度等）实时估计质量和惯性参数。以 EKF 为例，通过将参数作为增广状态，在预测和更新步骤中同时估计系统状态和参数。
模型更新：根据估计得到的参数，实时更新飞行器的动力学模型。这样，Adaptive NMPC 能够适应参数的变化，提高轨迹跟踪的准确性。
（二）Adaptive NMPC 算法流程
初始化：设定初始参数估计值、滤波器参数、预测时域、控制时域等。同时，获取飞行器的初始状态和期望轨迹。
参数估计：利用测量得到的飞行器状态信息，通过 EKF 或 UKF 估计质量和惯性等不确定参数。
模型更新：根据估计得到的参数，更新飞行器的动力学模型。
优化求解：基于更新后的模型，求解与 MPC 类似的优化问题，得到当前时刻的最优控制输入。
控制执行：将最优控制输入作用于飞行器，获取新的测量状态。
循环迭代：重复步骤 2 - 5，实现对飞行器的实时控制和轨迹跟踪。
四、应对外部干扰（风）
⛳️ 运行结果
📣 部分代码
function Xref = spiralTrajectory(T, Ts, radius, height, turns, p0, q0)t = 0:Ts:T; % Vector of timesN = numel(t);tau = t/T; % Normalization of time% arc lengths = 10*tau.^3-15*tau.^4+6*tau.^5;s_dot = (30*tau.^2-60*tau.^3+30*tau.^4)/T;% angle referencetheta = 2*pi*turns*s;theta_dot = 2*pi*turns*s_dot;% position reference ...x_local = radius*cos(theta)-radius;y_local = radius*sin(theta);z_local = height*s;% ... respect to the initial positionx = p0(1) + x_local;y = p0(2) + y_local;z = p0(3) + z_local;pos = [x; y; z];% linear velocity referencevx_local = -radius*sin(theta).*theta_dot;vy_local = radius*cos(theta).*theta_dot;vz_local = height*s_dot;vel = [vx_local; vy_local; vz_local];% yaw reference ...yaw_local = theta + pi/2;% ... respect to inital orientationeul0 = quat2eul(q0.', "ZYX");yaw0 = eul0(1);delta = yaw0 - yaw_local(1);yaw = yaw_local + delta;% pitch and roll referencespitch = zeros(1, N);roll = zeros(1, N);% quaternion constructioncy = cos(yaw/2); sy = sin(yaw/2);cp = cos(pitch/2); sp = sin(pitch/2);cr = cos(roll/2); sr = sin(roll/2);qw = cr.*cp.*cy + sr.*sp.*sy;qx = sr.*cp.*cy - cr.*sp.*sy;qy = cr.*sp.*cy + sr.*cp.*sy;qz = cr.*cp.*sy - sr.*sp.*cy;q = [qw; qx; qy; qz];% angular velocity referenceyaw_dot = theta_dot;angvel = [zeros(1,N); zeros(1,N); yaw_dot];% Final trajectoryXref = [pos; q; vel; angvel];end
