% Compute models for Ducted Fan VTOL micro-UAV for given set of parameters. % % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich. % This work is distributed under a permissive license, see LICENSE.txt % % This function generates three models: % % * A non-linear symbolic model (cannot be used directly). % * A linear model obtained by linearizing the the non linear model at an % operating point specified in the params struct argument. % * A uncertain linear model built atop of the linear model using SIMULINK. % The uncertain model contains the performance and weighting transfer % function given in the arguments perf and params, and is stored in the % SIMULINK file uav_model_uncertain.xls. % % [MODEL] = UAV_MODEL(PARAMS, PERF, UNCERT) % % Arguments: % PARAMS Struct of design parameters and constants generated from uav_params % PERF Struct with transfer functions that describe performance % requirements (used for uncertain model). % UNCERT Struct with weighting transfer functions for the uncertainty % blocks (used for uncertain model). % % Return value: % MODEL Struct with models % % See also UAV_PARAMS function [model] = uav_model(params, perf, uncert) model = struct(); % ------------------------------------------------------------------------ % Symbolic variables % Constant scalar physical quantities and dimensions syms m g rho a b d S k_T c_d c_0 c_l J_r real; syms J_1 J_2 J_3 real; J = diag([J_1, J_2, J_3]); % Scalar position, rotation and velocities syms x y z xdot ydot zdot real; syms phi theta p q r real; psi = sym('psi', 'real'); % shadow MATLAB's psi() function % Vector position, rotation and velocities P = [x; y; z]; % position vector (inertial frame) Pdot = [xdot; ydot; zdot]; % velocity vector (intertial frame) Theta = [phi; theta; psi]; % attitude vector: [roll pitch yaw] (body frame) Omega = [p; q; r]; % angular rates (body frame) % Inputs: flap angles and ducted fan speed syms alpha_1 alpha_2 alpha_3 alpha_4 omega real; alpha = [alpha_1; alpha_2; alpha_3; alpha_4]; % Flap angles are measured relative to the body-frame z-axis and considered % positive / negative with respect to the roll / pitch axis to which they % are attached to. Reference table: % % angle attached axis lift force direction % when angle is positive % ------- ------------- ---------------------- % alpha_1 pos. x axis y direction % alpha_2 pos. y axis -x direction % alpha_3 neg. x axis y direction % alpha_4 neg. y axis -x direction % Rotation matrix to change between frames of reference: % multiplying by R moves from the inertial frame to the body frame % to go from body frame to inertial frame use R transpose (R is SO(3)) R = [ cos(theta) * cos(psi), cos(theta) * sin(psi), -sin(theta); (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)), ... (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)), ... sin(phi) * cos(theta); (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)), ... (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)), ... cos(phi) * cos(theta); ]; % Matrix to relate Euler angles to angular velocity in the body frame % i.e. dTheta/dt = U * Omega. To get the angular velocity in intertial % coordinates use (R * U). U = [ 1, sin(phi) * tan(theta), cos(phi) * tan(theta); 0, cos(phi), -sin(phi); 0, sin(phi) / cos(theta), cos(phi) / cos(theta); ]; % name of unit vectors in inertial frame uvec_i = [1; 0; 0]; uvec_j = [0; 1; 0]; uvec_k = [0; 0; 1]; % name of unit vectors in body frame uvec_x = [1; 0; 0]; uvec_y = [0; 1; 0]; uvec_z = [0; 0; 1]; % ------------------------------------------------------------------------ % Nonlinear system dynamics % Approximate air velocity field magnitude collinear to uvec_z nu = omega / pi * sqrt(k_T / (2 * a * rho)); % Aerodynamic force caused by flaps in body frame F_flap = @(alpha, uvec_n) rho * S * nu^2 / 2 * (... (c_d * alpha^2 + c_0) * uvec_z + c_l * alpha * uvec_n); F_1 = F_flap(alpha_1, uvec_y); F_2 = F_flap(alpha_2, uvec_x); F_3 = F_flap(alpha_3, uvec_y); F_4 = F_flap(alpha_4, uvec_x); % Torque caused by aerodynamics forces in body frame tau_1 = cross((d * uvec_z + a/3 * uvec_x), F_1); tau_2 = cross((d * uvec_z + a/3 * uvec_y), F_2); tau_3 = cross((d * uvec_z - a/3 * uvec_x), F_3); tau_4 = cross((d * uvec_z - a/3 * uvec_y), F_4); % Total force acting on the UAV in the body frame F = R * (m * g * uvec_k) ... % gravity - k_T * omega^2 * uvec_z ... % thrust + F_1 + F_2 + F_3 + F_4; % flaps % Total torque acting on the UAV in the body frame tau = J_r * omega * R * cross(uvec_k, Omega) + ... % gyroscopic procession tau_1 + tau_2 + tau_3 + tau_4; % flaps % State space form with state variable xi and input u % % The 12-dimensional state is given by % % - absolute position (inertial frame) in R^3 % - absolute velocity (intertial frame) in R^3 % - Euler angles (body frame) in SO(3) % - Angular rates (body frame) in R^3 % xi = [P; Pdot; Theta; Omega]; u = [alpha; omega]; % Right hand side of dynamics dxi = f(xi, u) f = [ Pdot; R' * F / m; % translational dynamics U * Omega; inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics ]; % Save function to compute the rotation matrix model.FrameRot = @(pitch, roll, yaw) ... subs(R, [phi, theta, psi], [pitch, roll, yaw]); % Save equations of non-linear model (algebraic) model.nonlinear = struct(... 'State', xi, ... 'Inputs', u, ... 'Dynamics', f ... ); % ------------------------------------------------------------------------ % Linearization at equilibrium % Equilibrium point xi_eq = [ params.linearization.Position; params.linearization.Velocity; params.linearization.Angles; params.linearization.AngularVelocities; ]; u_eq = params.linearization.Inputs; % Construct linearized state dynamics A = subs(jacobian(f, xi), [xi; u], [xi_eq; u_eq]); B = subs(jacobian(f, u), [xi; u], [xi_eq; u_eq]); % Insert values of parameters phy = struct(... 'g', params.physics.Gravity, ... 'rho', params.physics.AirDensity ... ); A = subs(A, phy); B = subs(B, phy); mech = struct(... 'm', params.mechanical.Mass, ... 'a', params.mechanical.DuctRadius, ... 'b', params.mechanical.DuctHeight, ... 'd', params.mechanical.FlapZDistance, ... 'J_1', params.mechanical.InertiaTensor(1, 1), ... 'J_2', params.mechanical.InertiaTensor(2, 2), ... 'J_3', params.mechanical.InertiaTensor(3, 3), ... 'J_r', params.mechanical.GyroscopicInertiaZ ... ); A = subs(A, mech); B = subs(B, mech); aero = struct(... 'k_T', params.aerodynamics.ThrustOmegaProp, ... 'S', params.aerodynamics.FlapArea, ... 'c_d', params.aerodynamics.DragCoefficients(1), ... 'c_0', params.aerodynamics.DragCoefficients(2), ... 'c_l', params.aerodynamics.LiftCoefficient ... ); A = subs(A, aero); B = subs(B, aero); % Evaluate constants like pi, etc and convert to double A = double(vpa(A)); B = double(vpa(B)); % The state is fully observed via hardware and refined with sensor fusion % algorithms C = eye(size(A)); D = zeros(12, 5); % Number of states, inputs and outputs [nx, nu] = size(B); [ny, ~] = size(C); % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; sys = minreal(pade(ss(A, B, C, D, 'OutputDelay', T), n), [], false); % slient % Save linearized dynamics (numerical) model.linear = struct(... 'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs 'State', xi, 'Inputs', u, ... % state and input variables 'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized 'StateSpace', sys ... % state space object ); % ------------------------------------------------------------------------ % Check properties of linearized model eigvals = eig(A); % Check system controllability / stabilizability Wc = ctrb(sys); if rank(Wc) < nx fprintf(' - Linearized system has %d uncontrollable states!\n', ... (nx - rank(Wc))); % Is the system at least stabilizable? unstabilizable = 0; for i = 1:nx if real(eigvals(i)) >= 0 % PBH test W = [(A - eigvals(i) * eye(size(A))), B]; if rank(W) < nx % fprintf(' State %d is not stabilizable\n', i); unstabilizable = unstabilizable + 1; end end end if unstabilizable > 0 fprintf(' - Linearized system has %d unstabilizable modes!\n', ... unstabilizable); else fprintf(' However, it is stabilizable.\n'); end end % Check system observability / detectability Wo = obsv(sys); if rank(Wo) < nx fprintf(' - Linearized system has %d unobservable states!\n', ... (nx - rank(Wo))); % is the system at least detectable? undetectable = 0; for i = 1:nx if real(eigvals(i)) >= 0 % PBH test W = [C; (A - eigvals(i) * eye(size(A)))]; if rank(W) < nx undetectable = undetectable + 1; end end end if undetectable > 0 fprintf(' - Linearized system has %d undetectable modes!\n', ... undetectable); else fprintf(' However, it is detectable.\n') end end % ------------------------------------------------------------------------ % Model actuators % TODO: better model, this was tuned "by looking" w = 4 * params.actuators.ServoNominalAngularVelocity; zeta = .7; G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); % figure; step(G_servo * pi / 3); grid on; G_prop = tf(1,1); model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop); % ------------------------------------------------------------------------ % Add uncertainties using SIMULINK model % Load simulink model with uncertainties and pass in parameters h = load_system('uav_model_uncertain'); hws = get_param('uav_model_uncertain', 'modelworkspace'); hws.assignin('params', params); hws.assignin('model', model); hws.assignin('perf', perf); hws.assignin('uncert', uncert); % Get uncertain model ulmod = linmod('uav_model_uncertain'); usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d); % Specify uncertainty block structure for mussv command blk_stab = [ 4, 4; % alpha uncert, full 1, 1; % omega uncert, full 12, 12; % state uncert, full ]; blk_perf = [ blk_stab; 10, 14 % always full ]; % Save uncertain model model.uncertain = struct(... 'Simulink', ulmod, ... 'BlockStructure', blk_stab, ... 'BlockStructurePerf', blk_perf, ... 'StateSpace', usys ... ); % The uncertain system is partitioned into the following matrix % % [ z ] [ A B_w B_u ] [ v ] % [ e ] = [ C_e D_ew D_eu ] [ w ] % [ y ] [ C_y D_yw D_yu ] [ u ] % % Struct below provides indices for inputs and outputs of partitioning. % Check for correctness of these values by inspecting: % % - model.uncertain.Simulink.InputName(model.uncertain.index.InputX) % - model.uncertain.Simulink.OutputName(model.uncertain.index.OutputX) % % Function make_idx(start, size) is defined below. model.uncertain.index = struct(... 'InputUncertain', make_idx( 1, 17), ... % 'v' inputs 'InputDisturbance', make_idx(18, 7), ... % 'w' inputs for noise 'InputReference', make_idx(25, 3), ... % 'w' inputs for reference 'InputExogenous', make_idx(18, 10), ... % 'w' inputs (all of them) 'InputNominal', make_idx(28, 5), ... % 'u' inputs 'OutputUncertain', make_idx( 1, 17), ... % 'z' outputs 'OutputError', make_idx(18, 14), ... % 'e' outputs 'OutputNominal', make_idx(32, 12), ... % 'y' outputs 'OutputPlots', make_idx(44, 10) ... % 'p' outputs for plots in closed loop ); idx = model.uncertain.index; % Number of inputs model.uncertain.Nv = max(size(idx.InputUncertain)); model.uncertain.Nw = max(size(idx.InputExogenous)); model.uncertain.Nu = max(size(idx.InputNominal)); model.uncertain.Nr = max(size(idx.InputReference)); % size of noise is (Nw - Nr) % Number of outputs model.uncertain.Nz = max(size(idx.OutputUncertain)); model.uncertain.Ne = max(size(idx.OutputError)); model.uncertain.Ny = max(size(idx.OutputNominal)); % ------------------------------------------------------------------------ % Check properties of uncertain model % % Check that (A, B_u, C_y) is stabilizable and detectable % A = model.uncertain.StateSpace(... % model.uncertain.index.OutputUncertain, ... % model.uncertain.index.InputUncertain ... % ); % % B_u = model.uncertain.StateSpace(... % model.uncertain.index.OutputUncertain, ... % model.uncertain.index.InputNominal ... % ); % % % TODO: do PBH test % % % Check that D_eu and D_yw are full rank % D_eu = model.uncertain.StateSpace(... % model.uncertain.index.OutputError, ... % model.uncertain.index.InputNominal ... % ); % % D_yw = model.uncertain.StateSpace(... % model.uncertain.index.OutputNominal, ... % model.uncertain.index.InputDisturbance ... % ); % if rank(D_eu) < min(size(D_eu)) % fprintf('D_eu is not full rank!\n') % end % % if rank(D_yw) < min(size(D_yw)) % fprintf('D_yw is not full rank!\n') % end end function [indices] = make_idx(start, size) indices = [start:(start + size - 1)]'; end % vim: ts=2 sw=2 et: