diff options
Diffstat (limited to '')
| -rw-r--r-- | midterm.m | 41 | ||||
| -rw-r--r-- | uav_ctrl_nominal.m | 37 | ||||
| -rw-r--r-- | uav_model.m | 270 | ||||
| -rw-r--r-- | uav_params.m | 81 |
4 files changed, 429 insertions, 0 deletions
diff --git a/midterm.m b/midterm.m new file mode 100644 index 0000000..8f8e5e1 --- /dev/null +++ b/midterm.m @@ -0,0 +1,41 @@ +% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich + +%% ------------------------------------------------------------------------ +% Clear environment and generate parameters + +clear; clc; close all; +params = uav_params(); + +%% ------------------------------------------------------------------------ +% Create nominal plant and controller + +% Generate nominal model +model = uav_model(params); +ctrl = uav_ctrl_nominal(params, model); + +% Model dimensions +nx = model.linear.Nx; + +% Open loop path +L = model.linear.StateSpace * ctrl.K; + +% Closed loop path under unitary negative gain +G = feedback(L, eye(nx, nx)); + +%% ------------------------------------------------------------------------ +% Simulate nominal controller + +%% ------------------------------------------------------------------------ +% Define performance goals + +%% ------------------------------------------------------------------------ +% Create uncertain model + +%% ------------------------------------------------------------------------ +% Define stability goals + +%% ------------------------------------------------------------------------ +% Perform H-infinity design + +%% ------------------------------------------------------------------------ +% Perform mu-Analysis
\ No newline at end of file diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m new file mode 100644 index 0000000..d46a1ef --- /dev/null +++ b/uav_ctrl_nominal.m @@ -0,0 +1,37 @@ +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% +% Design a nominal controller for UAV. + +function [ctrl] = uav_ctrl_nominal(params, model) + +% ------------------------------------------------------------------------ +% Design a nominal LQR controller + +% Define penalties according to following priorities + +q_pos = 10; % penalty on position +q_vel = 1; % penalty on linear velocity +q_ang = 100; % high penalty on angles +q_rate = 1000; % very high penalty on angular velocities + +r_ang = 1; % flap movement is cheap +r_thrust = 10; % thrust is more expensive on the battery + +nx = model.linear.Nx; +nu = model.linear.Nu; + +% LQR design matrices +Q = kron(diag([q_pos, q_vel, q_ang, q_rate]), eye(3)); +R = diag([r_ang, r_ang, r_ang, r_ang, r_thrust]); +N = zeros(nx, nu); % no cross terms + +% Compute controller +[K, S, poles] = lqr(model.linear.StateSpace, Q, R, N); + +% ------------------------------------------------------------------------ +% Save controller + +ctrl = struct(); +ctrl.K = K; + +end
\ No newline at end of file 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 diff --git a/uav_params.m b/uav_params.m new file mode 100644 index 0000000..fb9121f --- /dev/null +++ b/uav_params.m @@ -0,0 +1,81 @@ +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% +% Retrieve / compute parameters for UAV. + +function [params] = uav_params() + +% Unit of measurements unless specified otherwise are +% Mass in kg +% Lenghts in m +% Time in s +% Frequencies in Hz +% Angular velocities in rad / s + +params = struct(); + +% ------------------------------------------------------------------------ +% Physical constants + +params.physics = struct(... + 'Gravity', 9.81, ... + 'AirDensity', 1.204 ... % kg / m^3 +); + +% ------------------------------------------------------------------------ +% Mechanical measurements + +params.mechanical = struct(... + 'Mass', 4.0, ... + 'DuctRadius', 46e-3, ... + 'DuctHeight', 171e-3, ... + 'FlapZDistance', 98e-3, ... % flap distance along z from center of mass + 'InertiaTensor', 5e-3 * eye(3), ... % TODO, assume diagonal + 'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now +); + +% ------------------------------------------------------------------------ +% Actuator limits + +params.actuators = struct(... + 'TurbineMaxSpeed', 620.7, ... % In Hz + 'ServoAbsMaxAngleDeg', 25, ... % in Degrees + 'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm +); + +% ------------------------------------------------------------------------ +% Aerodynamics modelling + +% Compute thrust proportionality factor from actuator limits +% from thrust relation F = k * omega^2. +% FIXME: this is not ideal, need better measurements +omega_max = params.actuators.TurbineMaxSpeed; +F_max = 38.637; % in N (measured) +k_T = sqrt(F_max / omega_max); + +params.aerodynamics = struct(... + 'ThrustOmegaProp', k_T, ... + 'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random + 'DragCoefficients', [1, 1] .* 1e-3, ... % TODO + 'LiftCoefficient', 1e-3 ... % TODO +); + +% ------------------------------------------------------------------------ +% Linearization point of non-linear dynamics. + +% Compute theoretical thrust required to make the UAV hover +% from the relation mg = k * omega^2 +g = params.physics.Gravity; +m = params.mechanical.Mass; +k = params.aerodynamics.ThrustOmegaProp; + +omega_hover = sqrt(m * g / k); + +params.linearization = struct(... + 'Position', [0; 0; -3], ... % in inertial frame, z points down + 'Velocity', [0; 0; 0], ... % in inertial frame + 'Angles', [0; 0; 0], ... % in body frame + 'AngularVelocities', [0; 0; 0], ... % in body frame + 'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X +); + +end
\ No newline at end of file |