Matlab仿真——基于NMPC的四旋翼无人机轨迹跟踪

本文链接：https://blog.csdn.net/weixin_55188463/article/details/132598205

本文参考Matlab官方例程实现。

【问题描述】

两台四旋翼无人机在不同初始位置，实现B无人机跟踪A无人机的轨迹规划。

【解决思路】

建立四旋翼无人机的动力学模型，使用NMPC进行控制。

【代码实现】

1.demo：MPC控制器与相关函数调用

%% 四旋翼无人机
% clear;clc;
% 创建非线性MPC
nx = 12;
ny = 12;
nu = 4;
nlmpcobj = nlmpc(nx, ny, nu);
% 状态空间表达式
nlmpcobj.Model.StateFcn = "QuadrotorStateFcn";
% 雅可比矩阵 加速运算
nlmpcobj.Jacobian.StateFcn = @QuadrotorStateJacobianFcn;
% 固定随机种子
rng(0)
% 验证模型
validateFcns(nlmpcobj,rand(nx,1),rand(nu,1));
% 设置仿真参数
Ts = 0.1;
p = 18;
m = 2;
nlmpcobj.Ts = Ts;
nlmpcobj.PredictionHorizon = p;
nlmpcobj.ControlHorizon = m;
% 输入约束
nlmpcobj.MV = struct( ...
    Min={0;0;0;0}, ...
    Max={10;10;10;10}, ...
    RateMin={-2;-2;-2;-2}, ...
    RateMax={2;2;2;2} ...
    );
% 输出权重
nlmpcobj.Weights.OutputVariables = [1 1 1 1 1 1 0 0 0 0 0 0];
% 输入权重
nlmpcobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];
% 输入变化率权重
nlmpcobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];
% 设置初始状态
x = [7;-10;0;0;0;0;0;0;0;0;0;0];

% 控制目标(保持无人机悬浮的平均值)
nloptions = nlmpcmoveopt;
nloptions.MVTarget = [4.9 4.9 4.9 4.9]; 
mv = nloptions.MVTarget;
%% 开始仿真
% 仿真时间间隔
Duration = 20;
% 等待条
hbar = waitbar(0,"Simulation Progress");
% 记录上一时刻的驶入
lastMV = mv;
% 记录历史值用于绘图
xHistory = x';
uHistory = lastMV;

x_refHistory = zeros(1,12);

% 仿真循环
for k = 1:(Duration/Ts)

    % 设置参考值
    t = linspace(k*Ts, (k+p-1)*Ts,p);
    yref = QuadrotorReferenceTrajectory(t);
    x_refHistory(k,:) = QuadrotorReferenceTrajectory(k*Ts);
    
    % 计算控制量
    xk = xHistory(k,:);
    [uk,nloptions,info] = nlmpcmove(nlmpcobj,xk,lastMV,yref',[],nloptions);

    % 存储数据
    uHistory(k+1,:) = uk';
    lastMV = uk;

    % 预测下一时刻的无人机 (MVs = uk) 
    ODEFUN = @(t,xk) QuadrotorStateFcn(xk,uk);
    [TOUT,XOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)');

    % 更新无人机状态
    xHistory(k+1,:) = XOUT(end,:);

    % 更新进度条
    waitbar(k*Ts/Duration,hbar);
end

% 关闭进度条
close(hbar)

%% 绘制无人机轨迹跟踪
plotDemo_Double(x_refHistory(:,1:6), xHistory(:,1:6))

2.plotDemo_Double：绘制无人机轨迹

%% 绘制双无人机轨迹
% 机身坐标
function plotDemo_Double(X1, X2)
     figure(1);hold on; grid on; view(3);axis([-20, 20, -20, 20, -10, 20]);

    disp('开始绘制')
    iter = size(X1,1);
    for i = 1:iter
        plotQuadrotor(X1(i,:),X2(i,:),1-isequal(i, iter));
        fprintf('%d 绘制中...\n', i)
        plot3(X1(1:i,1),X1(1:i,2),X1(1:i,3),'r*');
        plot3(X2(1:i,1),X2(1:i,2),X2(1:i,3),'b*');
    end
end

function plotQuadrotor(X1,X2,fcla) % 位姿参数
    x1 = X1(1); y1 = X1(2); z1 = X1(3);
    rx1 = X1(4);ry1 = X1(5);rz1 = X1(6);
    x2 = X2(1); y2 = X2(2); z2 = X2(3);
    rx2 = X2(4);ry2 = X2(5);rz2 = X2(6); 
    % 定义无人机的参数
    body_radius = 0.8; % 机身半径
    body_height = 0.5; % 机身高度
    wing_radius = 0.4; % 旋翼半径
    wing_height = 0.25; % 旋翼高度
    body_dis = 0.4; % 旋翼与机体距离
    wing_len = body_radius + wing_radius + body_dis; % 机翼展长
    % 外观参数
    wing_color = [1 0 0];
    body_color = [1 1 0];
    
    
    %% 绘制1号无人机
    T = getMatrix(x1,y1,z1,rx1,ry1,rz1); % 获得机身的齐次交换矩阵
    T1 = T*[1 0 0 wing_len;
          0 1 0 0;
          0 0 1 0;
          0 0 0 1]; 
    T2 = T*[1 0 0 0;
          0 1 0 wing_len;
          0 0 1 0;
          0 0 0 1]; 
    T3 = T*[1 0 0 -wing_len;
          0 1 0 0;
          0 0 1 0;
          0 0 0 1]; 
    T4 = T*[1 0 0 0;
          0 1 0 -wing_len;
          0 0 1 0;
          0 0 0 1];
    
    DrawCylinder(T(1:3,4), T(1:3,3), body_radius,body_height, body_color); % 机身
    DrawCylinder(T1(1:3,4), T1(1:3,3), wing_radius,wing_height, wing_color); % 旋翼A
    DrawCylinder(T2(1:3,4), T2(1:3,3), wing_radius,wing_height, wing_color); % 旋翼B
    DrawCylinder(T3(1:3,4), T3(1:3,3), wing_radius,wing_height, wing_color); % 旋翼C
    DrawCylinder(T4(1:3,4), T4(1:3,3), wing_radius,wing_height, wing_color); % 旋翼D
    
    % 连线
    connect(T,T1); 
    connect(T,T2);
    connect(T,T3);
    connect(T,T4);


    %% 绘制2号无人机
    T = getMatrix(x2,y2,z2,rx2,ry2,rz2); % 获得机身的齐次交换矩阵
    T1 = T*[1 0 0 wing_len;
          0 1 0 0;
          0 0 1 0;
          0 0 0 1]; 
    T2 = T*[1 0 0 0;
          0 1 0 wing_len;
          0 0 1 0;
          0 0 0 1]; 
    T3 = T*[1 0 0 -wing_len;
          0 1 0 0;
          0 0 1 0;
          0 0 0 1]; 
    T4 = T*[1 0 0 0;
          0 1 0 -wing_len;
          0 0 1 0;
          0 0 0 1];
    
    DrawCylinder(T(1:3,4), T(1:3,3), body_radius,body_height, body_color); % 机身
    DrawCylinder(T1(1:3,4), T1(1:3,3), wing_radius,wing_height, wing_color); % 旋翼A
    DrawCylinder(T2(1:3,4), T2(1:3,3), wing_radius,wing_height, wing_color); % 旋翼B
    DrawCylinder(T3(1:3,4), T3(1:3,3), wing_radius,wing_height, wing_color); % 旋翼C
    DrawCylinder(T4(1:3,4), T4(1:3,3), wing_radius,wing_height, wing_color); % 旋翼D
    
    % 连线
    connect(T,T1); 
    connect(T,T2);
    connect(T,T3);
    connect(T,T4);    
%     hold off;
    pic=getframe;
    if(fcla)
        cla;
    end
end
%%
%%%%%%%%%%%%%%%
%  函数定义    %
%%%%%%%%%%%%%%%
function T = getMatrix(x,y,z,rx,ry,rz) % 获取机身的齐次变换矩阵
    T_p = [1 0 0 x;
           0 1 0 y;
           0 0 1 z;
           0 0 0 1];

    T_x = [1 0 0 0;
           0 cos(rx) -sin(rx) 0;
           0 sin(rx) cos(rx) 0;
           0 0 0 1];
    T_y = [cos(ry)  0 sin(ry) 0;
           0 1 0 0;
          -sin(ry) 0 cos(ry) 0;
           0 0 0 1];
    T_z = [cos(rz) -sin(rz) 0 0;
        sin(rz) cos(rz)  0 0;
        0 0 1 0;
        0 0 0 1];
    T = T_p*T_z*T_y*T_x;
end

%% 绘制圆柱体
function h = DrawCylinder(pos, az, radius,len, col)
    
    az0 = [0;0;1];
    ax  = cross(az0,az);
    ax_n = norm(ax);
    if ax_n < eps 
	    rot = eye(3);
    else
        ax = ax/ax_n;
        ay = cross(az,ax);
        ay = ay/norm(ay);
        rot = [ax ay az];
    end
    
    %********** make cylinder
    % col = [0 0.5 0];  % cylinder color
    
    a = 50;    % number of side faces
    theta = (0:a)/a * 2*pi;
    
    x = [radius; radius]* cos(theta);
    y = [radius; radius] * sin(theta);
    z = [len/2; -len/2] * ones(1,a+1);
    % cc = col*ones(size(x));
    
    for n=1:size(x,1)
       xyz = [x(n,:);y(n,:);z(n,:)];
       xyz2 = rot * xyz;
       x2(n,:) = xyz2(1,:);
       y2(n,:) = xyz2(2,:);
       z2(n,:) = xyz2(3,:);
    end
    
    %************* draw
    % side faces
    h = surf(x2+pos(1),y2+pos(2),z2+pos(3),'FaceColor',col,'LineStyle','none');
    
    for n=1:2
	    patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),col);
    end	
end

function connect(T1, T2)
    p1 = T1(1:3,4);
    p2 = T2(1:3,4);
    plot3([p1(1) p2(1)], [p1(2) p2(2)], [p1(3) p2(3)],'color','black','LineWidth',5);
end

3.QuadrotorReferenceTrajectory：获取参考轨迹

function xdesired = QuadrotorReferenceTrajectory(t)
% 参考轨迹
    x =6*sin(t/3);
    y = -6*sin(t/3).*cos(t/3);
    z = 6*cos(t/3);
    phi = zeros(1,length(t));
    theta = zeros(1,length(t));
    psi = zeros(1,length(t));
    xdot = zeros(1,length(t));
    ydot = zeros(1,length(t));
    zdot = zeros(1,length(t));
    phidot = zeros(1,length(t));
    thetadot = zeros(1,length(t));
    psidot = zeros(1,length(t));

    xdesired = [x;y;z;phi;theta;psi;xdot;ydot;zdot;phidot;thetadot;psidot];

end

4.QuadrotorStateFcn：状态空间方程

function out1 = QuadrotorStateFcn(in1,in2)
% 参考matlab官方代码
% 状态空间方程
    phi = in1(4,:);
    phidot = in1(10,:);
    psidot = in1(12,:);
    psi = in1(6,:);
    theta = in1(5,:);
    thetadot = in1(11,:);
    u1 = in2(1,:);
    u2 = in2(2,:);
    u3 = in2(3,:);
    u4 = in2(4,:);
    xdot = in1(7,:);
    ydot = in1(8,:);
    zdot = in1(9,:);
    t2 = cos(phi);
    t3 = cos(psi);
    t4 = cos(theta);
    t5 = sin(phi);
    t6 = sin(psi);
    t7 = sin(theta);
    t8 = phi.*2.0;
    t9 = theta.*2.0;
    t10 = thetadot.^2;
    t18 = u1+u2+u3+u4;
    t11 = t2.^2;
    t12 = t2.^3;
    t14 = t4.^2;
    t15 = t4.^3;
    t16 = sin(t8);
    t17 = sin(t9);
    t13 = t11.^2;
    t19 = 1.0./t14;
    et1 = u2.*-1.15e+2+u4.*1.15e+2-t10.*t16.*(2.53e+2./2.0)-t7.*u1.*9.2e+1+t7.*u2.*9.2e+1-t7.*u3.*9.2e+1+t7.*u4.*9.2e+1+t11.*u2.*5.5e+1-t11.*u4.*5.5e+1+phidot.*t17.*thetadot.*(2.3e+1./2.0)+psidot.*t4.*thetadot.*2.76e+2+t5.*t10.*t12.*1.21e+2+t7.*t11.*u1.*4.4e+1-t7.*t11.*u2.*4.4e+1+t7.*t11.*u3.*4.4e+1-t7.*t11.*u4.*4.4e+1-t11.*t14.*u2.*5.5e+1+t11.*t14.*u4.*5.5e+1+psidot.*t4.*t11.*thetadot.*2.53e+2-psidot.*t4.*t13.*thetadot.*1.21e+2-psidot.*t11.*t15.*thetadot.*2.53e+2+psidot.*t13.*t15.*thetadot.*1.21e+2+t2.*t5.*t10.*t14.*2.53e+2-t5.*t10.*t12.*t14.*1.21e+2+phidot.*t4.*t7.*t11.*thetadot.*2.53e+2-t2.*t4.*t5.*t7.*u1.*5.5e+1+t2.*t4.*t5.*t7.*u3.*5.5e+1;
    et2 = phidot.*psidot.*t2.*t5.*t7.*t14.*3.85e+2;
    mt1 = [xdot;ydot;zdot;phidot;thetadot;psidot;(t18.*(t5.*t6+t2.*t3.*t7))./2.0;t18.*(t3.*t5-t2.*t6.*t7).*(-1.0./2.0);(t2.*t4.*t18)./2.0-9.81e+2./1.0e+2;(t19.*(et1+et2))./5.52e+2;((t4.*u1.*6.0e+1-t4.*u3.*6.0e+1+t16.*u1.*2.2e+1-t16.*u2.*2.2e+1+t16.*u3.*2.2e+1-t16.*u4.*2.2e+1+phidot.*psidot.*t14.*1.32e+2+t7.*t10.*t11.*1.21e+2-t7.*t10.*t13.*1.21e+2+t4.*t11.*u1.*5.5e+1-t4.*t11.*u3.*5.5e+1-phidot.*psidot.*t11.*t14.*3.85e+2+t2.*t5.*t7.*u2.*5.5e+1-t2.*t5.*t7.*u4.*5.5e+1+phidot.*t2.*t4.*t5.*thetadot.*2.53e+2-psidot.*t4.*t5.*t7.*t12.*thetadot.*1.21e+2).*(-1.0./5.52e+2))./t4];
    mt2 = [(t19.*(u1.*-9.2e+1+u2.*9.2e+1-u3.*9.2e+1+u4.*9.2e+1-t7.*u2.*1.15e+2+t7.*u4.*1.15e+2+t11.*u1.*4.4e+1-t11.*u2.*4.4e+1+t11.*u3.*4.4e+1-t11.*u4.*4.4e+1+phidot.*t4.*thetadot.*2.3e+1+psidot.*t17.*thetadot.*1.38e+2+t7.*t11.*u2.*5.5e+1-t7.*t11.*u4.*5.5e+1+phidot.*t4.*t11.*thetadot.*2.53e+2-t2.*t5.*t7.*t10.*2.53e+2+t5.*t7.*t10.*t12.*1.21e+2-t2.*t4.*t5.*u1.*5.5e+1+t2.*t4.*t5.*u3.*5.5e+1+phidot.*psidot.*t2.*t5.*t14.*3.85e+2+psidot.*t4.*t7.*t11.*thetadot.*2.53e+2-psidot.*t4.*t7.*t13.*thetadot.*1.21e+2))./5.52e+2];
    out1 = [mt1;mt2];
end

5.QuadrotorStateJacobianFcn：四旋翼无人机状态空间雅可比矩阵

function [A,B] = QuadrotorStateJacobianFcn(in1,in2)
% matlab官方代码
% 四旋翼无人机雅可比矩阵
    phi = in1(4,:);
    phidot = in1(10,:);
    psidot = in1(12,:);
    psi = in1(6,:);
    theta = in1(5,:);
    thetadot = in1(11,:);
    u1 = in2(1,:);
    u2 = in2(2,:);
    u3 = in2(3,:);
    u4 = in2(4,:);
    t2 = cos(phi);
    t3 = cos(psi);
    t4 = cos(theta);
    t5 = sin(phi);
    t6 = sin(psi);
    t7 = sin(theta);
    t8 = phi.*2.0;
    t9 = theta.*2.0;
    t10 = thetadot.^2;
    t22 = u1+u2+u3+u4;
    t11 = cos(t8);
    t12 = t2.^2;
    t13 = t2.^3;
    t15 = cos(t9);
    t16 = t4.^2;
    t17 = t4.^3;
    t18 = sin(t8);
    t19 = t5.^2;
    t20 = sin(t9);
    t21 = t7.^2;
    t23 = t4.*6.0e+1;
    t24 = 1.0./t4;
    t27 = t7.*9.2e+1;
    t28 = t7.*1.15e+2;
    t33 = (t2.*t4)./2.0;
    t34 = (t3.*t5)./2.0;
    t35 = (t5.*t6)./2.0;
    t42 = t2.*t4.*t5.*5.5e+1;
    t43 = t2.*t5.*t7.*5.5e+1;
    t48 = (t2.*t3.*t7)./2.0;
    t51 = (t2.*t6.*t7)./2.0;
    t14 = t12.^2;
    t25 = 1.0./t16;
    t26 = 1.0./t17;
    t29 = -t27;
    t30 = t12.*4.4e+1;
    t31 = t12.*5.5e+1;
    t32 = t18.*2.2e+1;
    t38 = -t34;
    t44 = t7.*t12.*-4.4e+1;
    t45 = t7.*t12.*-5.5e+1;
    t53 = t7.*t42;
    t54 = t4.*t12.*thetadot.*2.53e+2;
    t55 = t4.*t12.*u3.*-5.5e+1;
    t62 = phidot.*psidot.*t12.*t16.*3.85e+2;
    t65 = t35+t48;
    t36 = -t30;
    t37 = -t31;
    t39 = t4.*t31;
    t40 = t7.*t30;
    t41 = t7.*t31;
    t52 = t16.*t31;
    t56 = t45.*u4;
    t57 = phidot.*t54;
    t58 = t7.*t10.*t14.*1.21e+2;
    t59 = t7.*t54;
    t60 = psidot.*t4.*t7.*t14.*thetadot.*1.21e+2;
    t64 = -t62;
    t66 = t38+t51;
    t46 = t39.*u1;
    t47 = t39.*u3;
    t49 = t41.*u2;
    t50 = t41.*u4;
    t61 = -t58;
    t63 = -t60;
    et1 = t10.*t11.*2.53e+2-t10.*t14.*1.21e+2+t7.*t46-t10.*t12.*t16.*2.53e+2+t10.*t14.*t16.*1.21e+2+t10.*t12.*t19.*3.63e+2+t10.*t16.*t19.*2.53e+2+t2.*t5.*u2.*1.1e+2-t2.*t5.*u4.*1.1e+2+t4.*t45.*u3-t10.*t12.*t16.*t19.*3.63e+2+t2.*t5.*t7.*u1.*8.8e+1-t2.*t5.*t7.*u2.*8.8e+1+t2.*t5.*t7.*u3.*8.8e+1-t2.*t5.*t7.*u4.*8.8e+1-t2.*t5.*t16.*u2.*1.1e+2+t2.*t5.*t16.*u4.*1.1e+2-t4.*t7.*t19.*u1.*5.5e+1+t4.*t7.*t19.*u3.*5.5e+1-phidot.*psidot.*t7.*t12.*t16.*3.85e+2+phidot.*psidot.*t7.*t16.*t19.*3.85e+2+psidot.*t2.*t4.*t5.*thetadot.*5.06e+2-psidot.*t4.*t5.*t13.*thetadot.*4.84e+2-psidot.*t2.*t5.*t17.*thetadot.*5.06e+2+psidot.*t5.*t13.*t17.*thetadot.*4.84e+2;
    et2 = phidot.*t2.*t4.*t5.*t7.*thetadot.*5.06e+2;
    et3 = t4.*u1.*9.2e+1-t4.*u2.*9.2e+1+t4.*u3.*9.2e+1-t4.*u4.*9.2e+1-phidot.*t15.*thetadot.*2.3e+1+psidot.*t7.*thetadot.*2.76e+2-t4.*t12.*u1.*4.4e+1-t4.*t12.*u3.*4.4e+1+t4.*t30.*u2+t4.*t30.*u4-phidot.*t12.*t16.*thetadot.*2.53e+2+phidot.*t12.*t21.*thetadot.*2.53e+2+psidot.*t7.*t12.*thetadot.*2.53e+2-psidot.*t7.*t14.*thetadot.*1.21e+2+t2.*t5.*t16.*u1.*5.5e+1-t4.*t7.*t12.*u2.*1.1e+2-t2.*t5.*t16.*u3.*5.5e+1+t4.*t7.*t12.*u4.*1.1e+2-t2.*t5.*t21.*u1.*5.5e+1+t2.*t5.*t21.*u3.*5.5e+1-phidot.*psidot.*t2.*t5.*t17.*3.85e+2-psidot.*t7.*t12.*t16.*thetadot.*7.59e+2+psidot.*t7.*t14.*t16.*thetadot.*3.63e+2+t2.*t4.*t5.*t7.*t10.*5.06e+2-t4.*t5.*t7.*t10.*t13.*2.42e+2;
    et4 = phidot.*psidot.*t2.*t4.*t5.*t21.*7.7e+2;
    et5 = t25.*(t4.*u2.*1.15e+2-t4.*u4.*1.15e+2+t39.*u4+t43.*u3+phidot.*t7.*thetadot.*2.3e+1-psidot.*t15.*thetadot.*2.76e+2-t4.*t12.*u2.*5.5e+1+phidot.*t7.*t12.*thetadot.*2.53e+2-psidot.*t12.*t16.*thetadot.*2.53e+2+psidot.*t14.*t16.*thetadot.*1.21e+2+psidot.*t12.*t21.*thetadot.*2.53e+2-psidot.*t14.*t21.*thetadot.*1.21e+2+t2.*t4.*t5.*t10.*2.53e+2-t4.*t5.*t10.*t13.*1.21e+2-t2.*t5.*t7.*u1.*5.5e+1+phidot.*psidot.*t2.*t4.*t5.*t7.*7.7e+2).*(-1.0./5.52e+2);
    et6 = (t7.*t26.*(t49+t56+t57+t63-u1.*9.2e+1+u2.*9.2e+1-u3.*9.2e+1+u4.*9.2e+1+psidot.*t59-t7.*u2.*1.15e+2-t12.*u2.*4.4e+1-t12.*u4.*4.4e+1+t30.*u1+t28.*u4+t30.*u3+t42.*u3+phidot.*t4.*thetadot.*2.3e+1+psidot.*t20.*thetadot.*1.38e+2-t2.*t5.*t7.*t10.*2.53e+2+t5.*t7.*t10.*t13.*1.21e+2-t2.*t4.*t5.*u1.*5.5e+1+phidot.*psidot.*t2.*t5.*t16.*3.85e+2))./2.76e+2;
    mt1 = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,(t22.*(t2.*t6-t3.*t5.*t7))./2.0,t22.*(t2.*t3+t5.*t6.*t7).*(-1.0./2.0),t4.*t5.*t22.*(-1.0./2.0),t25.*(et1+et2).*(-1.0./5.52e+2)];
    mt2 = [t24.*(t49+t56+t57+t63+t11.*u1.*4.4e+1-t11.*u2.*4.4e+1+t11.*u3.*4.4e+1-t11.*u4.*4.4e+1-t7.*t19.*u2.*5.5e+1+t7.*t19.*u4.*5.5e+1-phidot.*t4.*t19.*thetadot.*2.53e+2-t2.*t5.*t7.*t10.*2.42e+2+t5.*t7.*t10.*t13.*4.84e+2-t2.*t4.*t5.*u1.*1.1e+2+t2.*t4.*t5.*u3.*1.1e+2+phidot.*psidot.*t2.*t5.*t16.*7.7e+2+psidot.*t4.*t7.*t12.*t19.*thetadot.*3.63e+2).*(-1.0./5.52e+2)];
    mt3 = [t25.*(t46+t55+t61+t64+t7.*t10.*t12.*2.53e+2-t7.*t10.*t19.*2.53e+2+t2.*t5.*u1.*8.8e+1-t2.*t5.*u2.*8.8e+1+t2.*t5.*u3.*8.8e+1-t2.*t5.*u4.*8.8e+1-t4.*t19.*u1.*5.5e+1+t4.*t19.*u3.*5.5e+1+phidot.*psidot.*t16.*t19.*3.85e+2+t7.*t10.*t12.*t19.*3.63e+2+t2.*t5.*t7.*u2.*1.1e+2-t2.*t5.*t7.*u4.*1.1e+2+phidot.*t2.*t4.*t5.*thetadot.*5.06e+2+psidot.*t2.*t4.*t5.*t7.*thetadot.*5.06e+2-psidot.*t4.*t5.*t7.*t13.*thetadot.*4.84e+2).*(-1.0./5.52e+2),0.0,0.0,0.0,0.0,0.0,0.0,t3.*t22.*t33,t6.*t22.*t33,t2.*t7.*t22.*(-1.0./2.0)];
    mt4 = [t25.*(et3+et4).*(-1.0./5.52e+2)+(t7.*t26.*(u2.*-1.15e+2+u4.*1.15e+2+psidot.*t54-t10.*t18.*(2.53e+2./2.0)+t7.*t57-t7.*u1.*9.2e+1-t7.*u3.*9.2e+1-t12.*u4.*5.5e+1+t27.*u2+t27.*u4+t31.*u2+t40.*u1+t40.*u3+t44.*u2+t44.*u4+t52.*u4+t53.*u3+phidot.*t20.*thetadot.*(2.3e+1./2.0)+psidot.*t4.*thetadot.*2.76e+2+t5.*t10.*t13.*1.21e+2-t12.*t16.*u2.*5.5e+1-psidot.*t4.*t14.*thetadot.*1.21e+2-psidot.*t12.*t17.*thetadot.*2.53e+2+psidot.*t14.*t17.*thetadot.*1.21e+2+t2.*t5.*t10.*t16.*2.53e+2-t5.*t10.*t13.*t16.*1.21e+2-t2.*t4.*t5.*t7.*u1.*5.5e+1+phidot.*psidot.*t2.*t5.*t7.*t16.*3.85e+2))./2.76e+2];
    mt5 = [(t24.*(t7.*u1.*6.0e+1-t7.*u3.*6.0e+1+t41.*u1+t42.*u4+t45.*u3-t4.*t10.*t12.*1.21e+2+t4.*t10.*t14.*1.21e+2+phidot.*psidot.*t4.*t7.*2.64e+2-t2.*t4.*t5.*u2.*5.5e+1-phidot.*psidot.*t4.*t7.*t12.*7.7e+2+phidot.*t2.*t5.*t7.*thetadot.*2.53e+2+psidot.*t5.*t13.*t16.*thetadot.*1.21e+2-psidot.*t5.*t13.*t21.*thetadot.*1.21e+2))./5.52e+2+(t7.*t25.*(t47+t58+t62+t4.*u3.*6.0e+1-t18.*u1.*2.2e+1-t18.*u3.*2.2e+1-t23.*u1+t32.*u2+t32.*u4+t43.*u4-phidot.*psidot.*t16.*1.32e+2-t7.*t10.*t12.*1.21e+2-t4.*t12.*u1.*5.5e+1-t2.*t5.*t7.*u2.*5.5e+1-phidot.*t2.*t4.*t5.*thetadot.*2.53e+2+psidot.*t4.*t5.*t7.*t13.*thetadot.*1.21e+2))./5.52e+2,et5+et6,0.0,0.0];
    mt6 = [0.0,0.0,0.0,0.0,(t22.*(t3.*t5-t2.*t6.*t7))./2.0,(t22.*(t5.*t6+t2.*t3.*t7))./2.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,(t25.*(t59+t20.*thetadot.*(2.3e+1./2.0)+psidot.*t2.*t5.*t7.*t16.*3.85e+2))./5.52e+2,t24.*(psidot.*t16.*1.32e+2-psidot.*t12.*t16.*3.85e+2+t2.*t4.*t5.*thetadot.*2.53e+2).*(-1.0./5.52e+2),(t25.*(t54+t4.*thetadot.*2.3e+1+psidot.*t2.*t5.*t16.*3.85e+2))./5.52e+2,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0];
    mt7 = [(t25.*(phidot.*t20.*(2.3e+1./2.0)+psidot.*t4.*2.76e+2-t18.*thetadot.*2.53e+2+psidot.*t4.*t12.*2.53e+2-psidot.*t4.*t14.*1.21e+2-psidot.*t12.*t17.*2.53e+2+psidot.*t14.*t17.*1.21e+2+t5.*t13.*thetadot.*2.42e+2+phidot.*t4.*t7.*t12.*2.53e+2+t2.*t5.*t16.*thetadot.*5.06e+2-t5.*t13.*t16.*thetadot.*2.42e+2))./5.52e+2,t24.*(t7.*t12.*thetadot.*2.42e+2-t7.*t14.*thetadot.*2.42e+2+phidot.*t2.*t4.*t5.*2.53e+2-psidot.*t4.*t5.*t7.*t13.*1.21e+2).*(-1.0./5.52e+2),(t25.*(phidot.*t4.*2.3e+1+psidot.*t20.*1.38e+2+phidot.*t4.*t12.*2.53e+2+psidot.*t4.*t7.*t12.*2.53e+2-psidot.*t4.*t7.*t14.*1.21e+2-t2.*t5.*t7.*thetadot.*5.06e+2+t5.*t7.*t13.*thetadot.*2.42e+2))./5.52e+2,0.0,0.0,0.0,0.0,0.0];
    mt8 = [1.0,0.0,0.0,0.0,(t25.*(t54+t4.*thetadot.*2.76e+2-t4.*t14.*thetadot.*1.21e+2-t12.*t17.*thetadot.*2.53e+2+t14.*t17.*thetadot.*1.21e+2+phidot.*t2.*t5.*t7.*t16.*3.85e+2))./5.52e+2,(t24.*(phidot.*t16.*-1.32e+2+phidot.*t12.*t16.*3.85e+2+t4.*t5.*t7.*t13.*thetadot.*1.21e+2))./5.52e+2,(t25.*(t59+t20.*thetadot.*1.38e+2+phidot.*t2.*t5.*t16.*3.85e+2-t4.*t7.*t14.*thetadot.*1.21e+2))./5.52e+2];
    A = reshape([mt1,mt2,mt3,mt4,mt5,mt6,mt7,mt8],12,12);
    if nargout > 1
    mt9 = [0.0,0.0,0.0,0.0,0.0,0.0,t65,t66,t33,t25.*(t27+t44+t53).*(-1.0./5.52e+2),t24.*(t23+t32+t39).*(-1.0./5.52e+2),t25.*(t36+t42+9.2e+1).*(-1.0./5.52e+2),0.0,0.0,0.0,0.0,0.0,0.0,t65,t66,t33,t25.*(t29+t37+t40+t52+1.15e+2).*(-1.0./5.52e+2),(t24.*(t32-t43))./5.52e+2,t25.*(t28+t30+t45-9.2e+1).*(-1.0./5.52e+2),0.0,0.0,0.0,0.0,0.0,0.0,t65,t66,t33,(t25.*(t29+t40+t53))./5.52e+2,(t24.*(t23-t32+t39))./5.52e+2,(t25.*(t30+t42-9.2e+1))./5.52e+2,0.0,0.0,0.0,0.0,0.0,0.0,t65,t66,t33,(t25.*(t27+t37+t44+t52+1.15e+2))./5.52e+2,(t24.*(t32+t43))./5.52e+2];
    mt10 = [(t25.*(t28+t36+t45+9.2e+1))./5.52e+2];
    B = reshape([mt9,mt10],12,4);
    end
end

【效果展示】

最后仿真的结果如下图，红色为被跟踪的无人机轨迹，蓝色为被控制的无人机轨迹。