diff options
Diffstat (limited to 'uav_model.m')
| -rw-r--r-- | uav_model.m | 270 |
1 files changed, 270 insertions, 0 deletions
diff --git a/uav_model.m b/uav_model.m new file mode 100644 index 0000000..7b2bf9d --- /dev/null +++ b/uav_model.m @@ -0,0 +1,270 @@ +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% +% Compute model for UAV for given set of parameters. + +function [model] = uav_model(params) + +% ------------------------------------------------------------------------ +% 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 +]; + +% ------------------------------------------------------------------------ +% 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 +sys = ss(A, B, C, D); + +% ------------------------------------------------------------------------ +% Measurement model + +% TODO: add sensor fusion delay + +% ------------------------------------------------------------------------ +% 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 state!\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.') + end +end + +% ------------------------------------------------------------------------ +% Save model + +model = struct(); + +% Function to compute the rotation matrix +model.FrameRot = @(pitch, roll, yaw) ... + subs(R, [phi, theta, psi], [pitch, roll, yaw]); + +% Equations of non-linear model (algebraic) +model.nonlinear = struct(... + 'State', xi, ... + 'Inputs', u, ... + 'Dynamics', f ... +); + +% 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 +); + +end |