Boeing 747 Flight Controller Model

一. 介绍

引自：aircraft-flight-controller
Simulink/Matlab model of linear and nonlinear flight controller for Boeing 747.

This project was completed as part of a University course on aircraft dynamics and control. The goal was to design a flight controller (consisting of an altitude controller and heading controller) capable of the maneuvering the aircraft in a smooth and stable manner. A linearized Simulink was developed to tune the controller, and a nonlinear simulation was developed to further evaluate the performance. The full report writeup is available for further readings.

Phase1.pdf (linear)
Phase2.pdf (nonlinear)
The linearized aircraft dynamics are decomposed into longitudinal and lateral components, allowing us to independently design the heading and altitude controllers. Four inputs are used to control the aircraft (throttle, rudder, aileron, elevator). In the linearized model, the throttle/elevator combo controls the longitudinal system (i.e. altitude control) and the rudder/aileron combo controls the lateral system (i.e. heading control). In this example, the aircraft is held fixed at Mach 0.8.

Excepts from the Simulink models are shown below.

二. 线性化模型

1. Altitude Control System

2) Heading Control System

3. 非线性化模型：

三. 结论

使用线性化的运动方程设计了航向和高度控制系统，开发了Simulink模型。 非线性飞行仿真用于测试控制系统设计并观察线性和非线性动力学之间的差异。 总体而言，非线性仿真产生了与线性系统相同的动态行为，并且具有合理的结果。建立时间的差异可能归因于增加了子系统，从而产生了额外的时间延迟。

四. 代码

%% Boeing 747 Flight Controller Model

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file contains all of the Boeing 747 paramters as referenced
% from Etkin's "Dynamics of Flight: Stability and Control". The control
% model uses linearized longitudinal and lateral flight dynamics for
% tuning. In Simulink, both linear and nonlinear flight simulations were
% developed to analyze the controller's performance.

clc
close all
clear all

% Boeing 747 Parameters

W = 2.83176e+6; % weight (N)
S = 511; % wing plan area (m^2)
c_bar = 8.324; % mean chord (m)
b = 59.64; % wing span (m)
Ixx = 0.247e+8; % moments of inertia (kg m^2)
Iyy = 0.449e+8;
Izz = 0.673e+8;
Ixz = -0.212e+7;
Uo = 235.9; % steady flight velocity (m/s)
rho = 0.3045; % density (kg/m^3)
g = 9.81; % gravity (m/s^2)
m = W / g; % mass (kg)
theta_o = 0.0;
Cwo = W / (0.5 * rho * S * Uo^2); % non-dimensional term of weight \hat{mg}
CLo = Cwo * cos(theta_o); % initial C_L lift coefficients
CDo = 0.043; % initial C_D lift coefficients (given)

Czo = - CLo;
Cxo = Cwo * sin(theta_o);
CTo = CDo + Cxo;
Cmo = 0.0;

% (2.4) longitudinal terms
Cxu = -0.1080;
Cxa = 0.2193; % C_{x_\alpha}
Cxq = 0.0;
Cxdu = 0.0; % C_{x_{\dot u}}
Cxda = 0.0; % C_{x_{\dot \alpha}}
Cxdq = 0.0;
Czu = -0.1060;
Cza = -4.9200;
Czq = -5.9210;
Czdu = 0.0;
Czda = 5.8960;
Czdq = 0.0;
Cmu = 0.1043;
Cma = -1.0230;
Cmq = -23.9200;
Cmdu = 0.0;
Cmda = -6.3140;
Cmdq = 0.0;
Cxdelta_e = -3.818e-6; %C_{x_{\delta_e}}
Czdelta_e = -0.3648;
Cmdelta_e = -1.444;
Cxdelta_p = 0.3 * Cwo; % this and the following delta_p are assumptions
Czdelta_p = 0;
Cmdelta_p = 0;

% (2.5) lateral terms
Cyb = -0.8771; % C_{y_\beta}
Cyp = 0;
Cyr = 0;
Cydb = 0; % C_{y_{\dot \beta}}
Cydp = 0;
Cydr = 0;
Clb = -0.2797;
Clp = -0.3295;

Clr = 0.3040;
Cldb = 0; % C_{l_{\dot \beta}}
Cldp = 0;
Cldr = 0;
Cnb = 0.1946;
Cnp = -0.04073;
Cnr = -0.2737;
Cndb = 0; % C_{n_{\dot \beta}}
Cndp = 0;
Cndr = 0;
Cydelta_a = 0; %C_{y_{\delta_a}}
Cydelta_r = 0.1146;
Cldelta_a = -1.368e-2;
Cldelta_r = 6.976e-3;
Cndelta_a = -1.973e-4;
Cndelta_r = -0.1257;

%% Dimensional Terms

% Longitudinal
% Forward force
Xu = rho*Uo*S*Cwo*sin(theta_o) + 1/2*rho*Uo*S*Cxu;
Xw = 1/2*rho*Uo*S*Cxa;

% Vertical force
Zu = -rho*Uo*S*Cwo*cos(theta_o) + 1/2*rho*Uo*S*Czu;
Zw = 1/2*rho*Uo*S*Cza;
Zq = 1/4*rho*Uo*c_bar*S*Czq;

% Pitch Moment
Mu = 1/2*rho*Uo*c_bar*S*Cmu;
Mw = 1/2*rho*Uo*c_bar*S*Cma;
Mq = 1/4*rho*Uo*c_bar^2*S*Cmq;
Zw_dot = 1/4*rho*c_bar*S*Czda;
Mw_dot = 1/4*rho*c_bar^2*S*Cmda;

% Control Derivatives
Xdelta_e = Cxdelta_e*1/2*rho*Uo^2*S;
Xdelta_p = Cxdelta_p*1/2*rho*Uo^2*S;
Zdelta_e = Czdelta_e*1/2*rho*Uo^2*S;
Zdelta_p = Czdelta_p*1/2*rho*Uo^2*S;
Mdelta_e = Cmdelta_e*1/2*rho*Uo^2*S*c_bar;
Mdelta_p = Cmdelta_p*1/2*rho*Uo^2*S*c_bar;

% Lateral
% Side force
Yv = Cyb*1/2*rho*Uo*S;
Yp = Cyp*1/4*rho*Uo*S*b;
Yr = Cyr*1/4*rho*Uo*S*b;

% Roll Moment
Lv = Clb*1/2*rho*Uo*S*b;
Lp = Clp*1/4*rho*Uo*S*b^2;
Lr = Clr*1/4*rho*Uo*S*b^2;

% Yaw Moment
Nv = Cnb*1/2*rho*Uo*S*b;
Np = Cnp*1/4*rho*Uo*S*b^2;
Nr = Cnr*1/4*rho*Uo*S*b^2;

% Control Derviatives
Ydelta_r = Cydelta_r*1/2*rho*Uo^2*S;
Ldelta_a = Cldelta_a*1/2*rho*Uo^2*S*b;
Ndelta_a = Cndelta_a*1/2*rho*Uo^2*S*b;
Ldelta_r = Cldelta_r*1/2*rho*Uo^2*S*b;
Ndelta_r = Cndelta_r*1/2*rho*Uo^2*S*b;

% Simplified Products of Inertia
Ix2 = (Ixx*Izz-Ixz^2)/Izz;
Iz2 = (Ixx*Izz-Ixz^2)/Ixx;
Ixz2 = Ixz/(Ixx*Izz-Ixz^2);


%% State Space Representation

% Add rho to account for change in density


% Longitudinal
% System Matrix
a11_long = Xu/m;
a12_long = Xw/m;
a13_long = 0;
a14_long = -g*cos(theta_o);
a15_long = 0;

a21_long = Zu/(m-Zw_dot);
a22_long = Zw/(m-Zw_dot);
a23_long = (Zq + m*Uo)/(m - Zw_dot);
a24_long = -m*g*sin(theta_o)/(m - Zw_dot);
a25_long = 0;

a31_long = 1/Iyy*(Mu + Mw_dot*Zu/(m - Zw_dot));
a32_long = 1/Iyy*(Mw + Mw_dot*Zw/(m - Zw_dot));
a33_long = 1/Iyy*(Mq + Mw_dot*(Zq + m*Uo)/(m - Zw_dot));
a34_long = -Mw_dot*m*g*sin(theta_o)/(Iyy*(m - Zw_dot));
a35_long = 0;

a41_long = 0;
a42_long = 0;
a43_long = 1;
a44_long = 0;
a45_long = 0;

a51_long = -sin(theta_o);
a52_long = cos(theta_o);
a53_long = 0;
a54_long = -Uo*cos(theta_o);
a55_long = 0;

A_long = [a11_long, a12_long, a13_long, a14_long, a15_long; 
    a21_long, a22_long, a23_long, a24_long, a25_long;
    a31_long, a32_long, a33_long, a34_long, a35_long; 
    a41_long, a42_long, a43_long, a44_long, a45_long; 
    a51_long, a52_long, a53_long, a54_long, a55_long];


% Control Matrix
b11_long = Xdelta_e/m;
b12_long = Xdelta_p/m;
b21_long = Zdelta_e/m;
b22_long = Zdelta_p/m;
b31_long = Mdelta_e/Iyy;
b32_long = Mdelta_p/Iyy;
b41_long = 0;
b42_long = 0;
b51_long = 0;
b52_long = 0;

B_long = [b11_long, b12_long;
    b21_long, b22_long;
    b31_long, b32_long;
    b41_long, b42_long;
    b51_long, b52_long];

% C and D matrix from ch. 6 notes
C_long = eye(5);
D_long = [0, 0;
    0, 0;
    0, 0;
    0, 0;
    0, 0];

% Lateral
% System Matrix
a11_lat = Yv/m;
a12_lat = Yp/m;
a13_lat = Yr/m - Uo;
a14_lat = g*cos(theta_o);
a15_lat = 0;
a21_lat = Lv/Ix2 + Nv*Ixz2;
a22_lat = Lp/Ix2 + Np*Ixz2;
a23_lat = Lr/Ix2 + Nr*Ixz2;
a24_lat = 0;
a25_lat = 0;

a31_lat = Nv/Iz2 + Lv*Ixz2;
a32_lat = Np/Iz2 + Lp*Ixz2;
a33_lat = Nr/Iz2 + Lr*Ixz2;
a34_lat = 0;
a35_lat = 0;

a41_lat = 0;
a42_lat = 1;
a43_lat = tan(theta_o);
a44_lat = 0;
a45_lat = 0;

a51_lat = 0;
a52_lat = 0;
a53_lat = sec(theta_o);
a54_lat = 0;
a55_lat = 0;

A_lat = [a11_lat, a12_lat, a13_lat, a14_lat, a15_lat;
        a21_lat, a22_lat, a23_lat, a24_lat, a25_lat; 
        a31_lat, a32_lat, a33_lat, a34_lat, a35_lat;
        a41_lat, a42_lat, a43_lat, a44_lat, a45_lat; 
        a51_lat, a52_lat, a53_lat, a54_lat, a55_lat];

% Control Matrix
b11_lat = 0;
b12_lat = Ydelta_r/m;
b21_lat = Ldelta_a/Ix2+Ndelta_a*Ixz2;
b22_lat = Ldelta_r/Ix2+Ndelta_r*Ixz2;
b31_lat = Ndelta_a/Iz2+Ldelta_a*Ixz2;
b32_lat = Ndelta_r/Iz2+Ldelta_r*Ixz2;
b41_lat = 0;
b42_lat = 0;
b51_lat = 0;
b52_lat = 0;

B_lat = [b11_lat, b12_lat;
    b21_lat, b22_lat;
    b31_lat, b32_lat;
    b41_lat, b42_lat;
    b51_lat, b52_lat];

% C and D matrix from ch. 6 notes
C_lat = eye(5);
D_lat = zeros(5,2);


% Constructing State Space Representations

sys_long = ss(A_long, B_long, C_long, D_long,'OutputName',{'u','w','q','\theta','Ze'},...
'InputName',{'\delta_e','\delta_p'});

G_long = tf(sys_long(4,1)); % Extracting transfer function

sys_lat = ss(A_lat, B_lat, C_lat, D_lat,'OutputName',{'v','p','r','\phi','\psi'},...
'InputName',{'\delta_a','\delta_r'});

G_lat = tf(sys_long(3,1)); % Extracting transfer function

Gup = tf(sys_long(1,2));


% Visualizations of Longitudinal Results (Leave Commented)

% % Closed Loop
% cl = feedback(sys_lat(4,1), -1);
% 
% % Open Loop vs Closed Loop Response 
% figure(1)
% step(sys_lat(4,1),'-r',cl,'--k')
% hold off
%  
% figure(2)
% impulse(sys_lat(4,1),'-r',cl,'--k')
% hold off
%  
% figure(3)
% rlocus(-sys_lat(4,1))
% sgrid
% hold off
% 
% % sim('Linear_Model',200)
% % [l1,l2] = size(Z);
% % sim('Linear_Model',500)
% % comet3(Uo*cos(psi)*200, Uo*sin(psi)*200, Z)
% % comet3(x,y,z)

4.Simulink仿真

运行模型

5. matlab建模

非线化模型

% This function simulates the dynamics of the quadrotor. The inputs are the
% Euler angles and motor forces. The output of the function is the updated
% quadrotor linear accelerations in the Global Frame and the rotational
% accelerations in the Body Frame. See www.wilselby.com for derivations of
% these equations

function quad_dynamics_nonlinear
global Quad;

%% Update Accelerations

Quad.X_ddot = (-[cos(Quad.phi)*sin(Quad.theta)*cos(Quad.psi)+sin(Quad.phi)*sin(Quad.psi)]*Quad.U1-Quad.Kdx*Quad.X_dot)/Quad.m;
Quad.Y_ddot = (-[cos(Quad.phi)*sin(Quad.psi)*sin(Quad.theta)-cos(Quad.psi)*sin(Quad.phi)]*Quad.U1-Quad.Kdy*Quad.Y_dot)/Quad.m;
Quad.Z_ddot = (-[cos(Quad.phi)*cos(Quad.theta)]*Quad.U1-Quad.Kdz*Quad.Z_dot)/Quad.m+Quad.g;

Quad.p_dot = (Quad.q*Quad.r*(Quad.Jy - Quad.Jz) - Quad.Jp*Quad.p*Quad.Obar + Quad.l*Quad.U2)/Quad.Jx;
Quad.q_dot = (Quad.p*Quad.r*(Quad.Jz - Quad.Jx) + Quad.Jp*Quad.q*Quad.Obar + Quad.l*Quad.U3)/Quad.Jy;
Quad.r_dot = (Quad.p*Quad.q*(Quad.Jx - Quad.Jy) + Quad.U4)/Quad.Jz;

Quad.phi_dot   = Quad.p + sin(Quad.phi)*tan(Quad.theta)*Quad.q + cos(Quad.phi)*tan(Quad.theta)*Quad.r;
Quad.theta_dot = cos(Quad.phi)*Quad.q - sin(Quad.phi)*Quad.r;
Quad.psi_dot   = sin(Quad.phi)/cos(Quad.theta)*Quad.q + cos(Quad.phi)/cos(Quad.theta)*Quad.r;

%% Disturbance model

Quad.X_ddot = Quad.X_ddot + Quad.X_dis/Quad.m; 
Quad.Y_ddot = Quad.Y_ddot + Quad.Y_dis/Quad.m; 
Quad.Z_ddot = Quad.Z_ddot + Quad.Z_dis/Quad.m; 
Quad.phi_dot = Quad.phi_dot + Quad.phi_dis/Quad.Jx*Quad.Ts; 
Quad.theta_dot = Quad.theta_dot + Quad.theta_dis/Quad.Jy*Quad.Ts; 
Quad.psi_dot = Quad.psi_dot + Quad.psi_dis/Quad.Jz*Quad.Ts;

%% Update Velocities and Positions

% Calculating the Z velocity & position
Quad.Z_dot = Quad.Z_ddot*Quad.Ts + Quad.Z_dot;
Quad.Z = Quad.Z_dot*Quad.Ts + Quad.Z;

% Calculating the X velocity & position
Quad.X_dot = Quad.X_ddot*Quad.Ts + Quad.X_dot;
Quad.X = Quad.X_dot*Quad.Ts + Quad.X;

% Calculating the Y velocity & position
Quad.Y_dot = Quad.Y_ddot*Quad.Ts + Quad.Y_dot;
Quad.Y = Quad.Y_dot*Quad.Ts + Quad.Y;

% Calculating p,q,r
Quad.p = Quad.p_dot*Quad.Ts+Quad.p;
Quad.q = Quad.q_dot*Quad.Ts+Quad.q;
Quad.r = Quad.r_dot*Quad.Ts+Quad.r;

% Calculating angular velocity and position
Quad.phi = Quad.phi_dot*Quad.Ts + Quad.phi;
Quad.theta = Quad.theta_dot*Quad.Ts+Quad.theta;
Quad.psi = Quad.psi_dot*Quad.Ts+Quad.psi;

%% Update Plotting Variables

% Flip positive Z axis up for intuitive plotting
Quad.Z_plot(Quad.counter) = -Quad.Z;
Quad.Z_ref_plot(Quad.counter) = -Quad.Z_des;

Quad.X_plot(Quad.counter) = Quad.X;
Quad.X_ref_plot(Quad.counter) = Quad.X_des;

Quad.Y_plot(Quad.counter) = Quad.Y;
Quad.Y_ref_plot(Quad.counter) = Quad.Y_des;

Quad.phi_plot(Quad.counter) = Quad.phi;
Quad.phi_ref_plot(Quad.counter) = Quad.phi_des;

Quad.theta_plot(Quad.counter) = Quad.theta;
Quad.theta_ref_plot(Quad.counter) = Quad.theta_des;

Quad.psi_plot(Quad.counter) = Quad.psi;
Quad.psi_ref_plot(Quad.counter) = Quad.psi_des;

Quad.counter = Quad.counter + 1;

end

动力学

% This function simulates the dynamics of the quadrotor. The inputs are the
% Euler angles and motor forces. The output of the function is the updated
% quadrotor linear accelerations in the Global Frame and the rotational
% accelerations in the Body Frame. See www.wilselby.com for derivations of
% these equations

function quad_dynamics
global Quad;

%% Update Accelerations

Quad.X_ddot = ([cos(Quad.phi)*sin(Quad.theta)*cos(Quad.psi)+sin(Quad.phi)*sin(Quad.psi)]*Quad.U1-Quad.Kdx*Quad.X_dot)/Quad.m;
Quad.Y_ddot = ([cos(Quad.phi)*sin(Quad.psi)*sin(Quad.theta)-cos(Quad.psi)*sin(Quad.phi)]*Quad.U1-Quad.Kdy*Quad.Y_dot)/Quad.m;
Quad.Z_ddot = ([cos(Quad.phi)*cos(Quad.theta)]*Quad.U1-Quad.Kdz*Quad.Z_dot)/Quad.m-Quad.g;

Quad.p_dot = (Quad.q*Quad.r*(Quad.Jy - Quad.Jz) - Quad.Jp*Quad.p*Quad.Obar + Quad.l*Quad.U2)/Quad.Jx;
Quad.q_dot = (Quad.p*Quad.r*(Quad.Jz - Quad.Jx) + Quad.Jp*Quad.q*Quad.Obar + Quad.l*Quad.U3)/Quad.Jy;
Quad.r_dot = (Quad.p*Quad.q*(Quad.Jx - Quad.Jy) + Quad.U4)/Quad.Jz;

%% Update Velocities and Positions
% TODO - update math for this transformation
%Quad.phi_dot   = Quad.p + sin(Quad.phi)*tan(Quad.theta)*Quad.q + cos(Quad.phi)*tan(Quad.theta)*Quad.r;
%Quad.theta_dot = cos(Quad.phi)*Quad.q - sin(Quad.phi)*Quad.r;
%Quad.psi_dot   = sin(Quad.phi)/cos(Quad.theta)*Quad.q + cos(Quad.phi)/cos(Quad.theta)*Quad.r;
Quad.phi_ddot = Quad.p_dot;
Quad.theta_ddot = Quad.q_dot;
Quad.psi_ddot = Quad.r_dot;

% Calculating the Z velocity & position
Quad.Z_dot = Quad.Z_ddot*Quad.Ts + Quad.Z_dot;
Quad.Z = Quad.Z_dot*Quad.Ts + Quad.Z;

% Calculating the X velocity & position
Quad.X_dot = Quad.X_ddot*Quad.Ts + Quad.X_dot;
Quad.X = Quad.X_dot*Quad.Ts + Quad.X;

% Calculating the Y velocity & position
Quad.Y_dot = Quad.Y_ddot*Quad.Ts + Quad.Y_dot;
Quad.Y = Quad.Y_dot*Quad.Ts + Quad.Y;

% Calculating angular velocity and position
Quad.phi_dot = Quad.phi_ddot*Quad.Ts + Quad.phi_dot;
Quad.phi = Quad.phi_dot*Quad.Ts + Quad.phi;

Quad.theta_dot = Quad.theta_ddot*Quad.Ts + Quad.theta_dot;
Quad.theta = Quad.theta_dot*Quad.Ts+Quad.theta;

Quad.psi_dot = Quad.psi_ddot*Quad.Ts + Quad.psi_dot;
Quad.psi = Quad.psi_dot*Quad.Ts+Quad.psi;

% TODO - Reimplement once math is updated Calculating p,q,r
%Quad.p = Quad.p_dot*Quad.Ts+Quad.p;
%Quad.q = Quad.q_dot*Quad.Ts+Quad.q;
%Quad.r = Quad.r_dot*Quad.Ts+Quad.r;
Quad.p = Quad.phi_dot;
Quad.q = Quad.theta_dot;
Quad.r = Quad.psi_dot;

%% Update Plotting Variables

% Plotting variables
Quad.Z_plot(Quad.counter) = Quad.Z;
Quad.Z_ref_plot(Quad.counter) = Quad.Z_des;

Quad.X_plot(Quad.counter) = Quad.X;
Quad.X_ref_plot(Quad.counter) = Quad.X_des;

Quad.Y_plot(Quad.counter) = Quad.Y;
Quad.Y_ref_plot(Quad.counter) = Quad.Y_des;

Quad.phi_plot(Quad.counter) = Quad.phi;
Quad.phi_ref_plot(Quad.counter) = Quad.phi_des;

Quad.theta_plot(Quad.counter) = Quad.theta;
Quad.theta_ref_plot(Quad.counter) = Quad.theta_des;

Quad.psi_plot(Quad.counter) = Quad.psi;
Quad.psi_ref_plot(Quad.counter) = Quad.psi_des;

Quad.counter = Quad.counter + 1;

end

电机

% This function maps the control inputs to desired motor speeds. These will
% be the actual commands sent  to the Electronic Speed Controllers from the
% Autopilot. The ESCs will then command each motor to rotate at the desired
% speed.

global Quad

% Calculate square of motor speeds based on desired control inputs
m1 = Quad.U1/(4*Quad.KT) - Quad.U3/(2*Quad.KT*Quad.l) - Quad.U4/(4*Quad.Kdx);
m2 = Quad.U1/(4*Quad.KT) - Quad.U2/(2*Quad.KT*Quad.l) + Quad.U4/(4*Quad.Kdx);
m3 = Quad.U1/(4*Quad.KT) + Quad.U3/(2*Quad.KT*Quad.l) - Quad.U4/(4*Quad.Kdx);
m4 = Quad.U1/(4*Quad.KT) + Quad.U2/(2*Quad.KT*Quad.l) + Quad.U4/(4*Quad.Kdx);

% Limit the motor speeds
if(abs(m1)>Quad.motor_max
    m1 = sign(m1)*Quad.motor_max;
end

if(abs(m2)>Quad.motor_max
    m2 = sign(m2)*Quad.motor_max;
end

if(abs(m3)>Quad.motor_max
    m3 = sign(m3)*Quad.motor_max;
end

if(abs(m4)>Quad.motor_max
    m4 = sign(m4)*Quad.motor_max;
end

% Calculate actual desired motoer speeds (radian/s)
Quad.O1 = sign(m1)*sqrt(abs(m1));
Quad.O2 = sign(m2)*sqrt(abs(m2));
Quad.O3 = sign(m3)*sqrt(abs(m3));
Quad.O4 = sign(m4)*sqrt(abs(m4));

Quad.Obar = Quad.O1 - Quad.O2 + Quad.O3 - Quad.O4;

Quad.O1_plot(Quad.counter) = Quad.O1;
Quad.O2_plot(Quad.counter) = Quad.O2;
Quad.O3_plot(Quad.counter) = Quad.O3;
Quad.O4_plot(Quad.counter) = Quad.O4;


% TODO convert back to U1 and plot see Forces.m