% 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 plant models: % % * A non-linear symbolic model (cannot be used directly) derived from first % principles (equations of motion). % % * A linear model obtained by linearizing the the non linear plant model at % an operating point specified in the params struct argument. And adding % models for the actuators. % % * A uncertain linear model with reference trcking 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); % ------------------------------------------------------------------------ % Model actuators % TODO: better model? w = params.actuators.ServoNominalAngularVelocity; zeta = 1; G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); w = 60; zeta = 1; G_prop = tf(w^2, [1, 2 * zeta * w, w^2]); model.actuators = struct( ... 'FlapServo', G_servo, ... 'ThrustPropeller', G_prop, ... 'StateSpace', blkdiag(eye(4) * G_servo, G_prop)); % ------------------------------------------------------------------------ % Scale inputs and outputs of linearized model S_actuators = blkdiag(... eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ... eye(1) * 1 / params.actuators.PropellerMaxAngularVelocity); S_state = blkdiag(... eye(2) * params.normalization.HPosition, ... eye(1) * params.normalization.VPosition, ... eye(2) * params.normalization.HSpeed, ... eye(1) * params.normalization.VSpeed, ... eye(2) * params.normalization.PitchRollAngle, ... eye(1) * params.normalization.YawAngle, ... eye(3) * params.normalization.AngularRate); % Scale system matrices to have inputs and outputs between zero and one B = B * inv(S_actuators); C = inv(S_state) * C; D = D * inv(S_actuators); % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; sys = pade(ss(A, B, C, D, 'OutputDelay', T), n); % Add actuators sys = sys * model.actuators.StateSpace; % Remove unnecessary states sys = minreal(sys, [], false); % slient % Number of states, inputs and outputs [nx, nu] = size(sys.B); [ny, ~] = size(sys.C); % 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 objec 'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ... ); % ------------------------------------------------------------------------ % Check properties of linearized model [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(sys); fprintf(' - Linearized system has %d states:\n', nx); fprintf(' %d stable modes, %d unstable modes.\n', nsta, nx - nsta); fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); if nctrb < 12 error('Linearized model has less than 12 controllable modes!'); end if nustab > 0 || nudetb > 0 error('Linearized model has unstabilizable or undetectable modes!'); end % ------------------------------------------------------------------------ % Compute absolute value of error caused by linearization around set point % TODO % ------------------------------------------------------------------------ % Add uncertainties using SIMULINK model % Load simulink model with uncertainties and pass in parameters h = load_system('uav_model_uncertain'); set_param('uav_model_uncertain', SimulationMode='Normal'); 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 ]; % ------------------------------------------------------------------------ % Scale inputs and outputs of uncertain model % Scaling of reference is same as position S_ref = blkdiag(... eye(2) * 1 / params.normalization.HPosition, ... eye(1) * 1 / params.normalization.VPosition); % TODO: finish S_uncert_out = blkdiag(... S_actuators, ... S_state, ... S_actuators, ... S_state(1:9, 1:9)); % 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 [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(usys); fprintf(' - Uncertain system has %d states:\n', nx); fprintf(' %d stable modes, %d unstable modes.\n', nsta, nx - nsta); fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); if nctrb < 12 error('Uncertain model has less than 12 controllable modes!'); end if nustab > 0 || nudetb > 0 error('Uncertain model has unstabilizable or undetectable modes!'); end % % Check that (A, B_u, C_y) is stabilizable and detectable A = model.uncertain.StateSpace(... model.uncertain.index.OutputUncertain, ... model.uncertain.index.InputUncertain ... ); [~, ~, ~, nustab, ~, nudetb] = pbhtest(A); if nustab > 0 || nudetb > 0 fprintf(' Uncertain system has undetectable or uncontrollable A.\n'); end B_u = model.uncertain.StateSpace(... model.uncertain.index.OutputUncertain, ... model.uncertain.index.InputNominal ... ); [~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u); if nustab > 0 || nudetb > 0 fprintf(' Uncertain system has undetectable or uncontrollable Bu.\n'); end C_y = model.uncertain.StateSpace(... model.uncertain.index.OutputNominal, ... model.uncertain.index.InputUncertain ... ); [~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y); if nustab > 0 || nudetb > 0 fprintf(' Uncertain system has undetectable or uncontrollable Cy.\n'); end % 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(ctrb(D_eu)) < length(D_eu.A) || rank(obsv(D_eu)) < length(D_eu.A) fprintf(' D_eu is not full rank!\n') end if rank(ctrb(D_yw)) < length(D_yw.A) || rank(obsv(D_yw)) < length(D_yw.A) fprintf(' D_yw is not full rank!\n') end % TODO: column rank checks end function [indices] = make_idx(start, size) indices = (start:(start + size - 1))'; end % vim: ts=2 sw=2 et: