From 47ce5760b1c45fc5513e56dce92e29b9363bb03e Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Mon, 1 Apr 2024 14:00:16 +0200 Subject: Update a lot of small things and add SIMULINK model --- midterm.m | 68 ++++++++++++++++++++++++++++++++++++------------ uav_ctrl_nominal.m | 11 ++++---- uav_model.m | 19 +++++++------- uav_model_uncertain.slx | Bin 0 -> 41970 bytes uav_params.m | 21 ++++++++++++--- 5 files changed, 84 insertions(+), 35 deletions(-) create mode 100644 uav_model_uncertain.slx diff --git a/midterm.m b/midterm.m index 8f8e5e1..eca35bc 100644 --- a/midterm.m +++ b/midterm.m @@ -3,39 +3,75 @@ %% ------------------------------------------------------------------------ % Clear environment and generate parameters -clear; clc; close all; +clear; clc; close all; s = tf('s'); params = uav_params(); %% ------------------------------------------------------------------------ -% Create nominal plant and controller +% Create nominal plant % Generate nominal model model = uav_model(params); -ctrl = uav_ctrl_nominal(params, model); -% Model dimensions -nx = model.linear.Nx; +%% ------------------------------------------------------------------------ +% Define performance requirements -% Open loop path -L = model.linear.StateSpace * ctrl.K; +alpha_max = params.actuators.ServoAbsMaxAngleDeg * pi / 180; +W_Palpha = tf(alpha_max,1); -% Closed loop path under unitary negative gain -G = feedback(L, eye(nx, nx)); +W_Pomega = tf(1,1); +W_Pxy = tf(1,1); +W_Pz = tf(1,1); -%% ------------------------------------------------------------------------ -% Simulate nominal controller +figure(1); hold on; +bodemag(W_Palpha); +bodemag(W_Pomega); +bodemag(W_Pxy); +bodemag(W_Pz); -%% ------------------------------------------------------------------------ -% Define performance goals +grid on; +legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ... + '$W_{P,xy}$', '$W_{P,z}$', ... + 'interpreter', 'latex', 'fontSize', 8); +title('Performance requirements'); + + +W_PP = blkdiag(W_Pxy * eye(2), W_Pz); + +perf = struct(... + 'FlapAngle', W_Palpha * eye(4), ... + 'Thrust', W_Pomega, ... + 'PositionAccuracy', W_PP); %% ------------------------------------------------------------------------ -% Create uncertain model +% Define stability requirements + +% Mechanically, flaps are constrained to a max of 20~25 degrees, +% we assume a precision of +% also, flaps tend to get less precise at higher frequencies +alpha_max = 2 * pi * 20; +W_malpha = alpha_max / (s + 2) * eye(4); + +W_momega = tf(1,1); +W_mTheta = tf(1,1) * eye(3); +W_mOmega = tf(1,1) * eye(3); + +uncert = struct(... + 'FlapAngle', W_malpha, ... + 'Thrust', W_momega, ... + 'EulerAnglesApprox', W_mTheta, ... + 'AngularRateApprox', W_mOmega); %% ------------------------------------------------------------------------ -% Define stability goals +% Create uncertain system + +% Load simulink model with uncertainties +usys = linmod('uav_model_uncertain'); +G = ss(usys.a, usys.b, usys.c, usys.d); %% ------------------------------------------------------------------------ % Perform H-infinity design %% ------------------------------------------------------------------------ -% Perform mu-Analysis \ No newline at end of file +% Perform mu-Analysis & DK iteration + +% vim: ts=2 sw=2 et: diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m index d46a1ef..1030233 100644 --- a/uav_ctrl_nominal.m +++ b/uav_ctrl_nominal.m @@ -9,13 +9,13 @@ function [ctrl] = uav_ctrl_nominal(params, model) % Define penalties according to following priorities -q_pos = 10; % penalty on position -q_vel = 1; % penalty on linear velocity +q_pos = 1; % penalty on position +q_vel = 10; % penalty on linear velocity q_ang = 100; % high penalty on angles -q_rate = 1000; % very high penalty on angular velocities +q_rate = 100; % very high penalty on angular velocities r_ang = 1; % flap movement is cheap -r_thrust = 10; % thrust is more expensive on the battery +r_thrust = 100; % thrust is more expensive on the battery nx = model.linear.Nx; nu = model.linear.Nu; @@ -34,4 +34,5 @@ N = zeros(nx, nu); % no cross terms ctrl = struct(); ctrl.K = K; -end \ No newline at end of file +end +% vim: ts=2 sw=2 et: diff --git a/uav_model.m b/uav_model.m index 2ae4d96..5650946 100644 --- a/uav_model.m +++ b/uav_model.m @@ -1,5 +1,5 @@ % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich -% +% % Compute model for UAV for given set of parameters. function [model] = uav_model(params) @@ -95,7 +95,7 @@ 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 + + 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 @@ -117,7 +117,7 @@ u = [alpha; omega]; f = [ Pdot; R' * F / m; % translational dynamics - U * Omega; + U * Omega; inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics ]; @@ -184,10 +184,8 @@ D = zeros(12, 5); % Create state space object sys = ss(A, B, C, D); -% ------------------------------------------------------------------------ -% Measurement model - -% TODO: add sensor fusion delay +% T = params.actuators.MeasurementDelay; +% sys = ss(A, B, C, D, 'OutputDelay', T); % ------------------------------------------------------------------------ % Check properties of linearized model @@ -198,7 +196,7 @@ 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 @@ -222,10 +220,10 @@ end % Check system observability / detectability Wo = obsv(sys); if rank(Wo) < nx - fprintf('Linearized system has %d unobservable state!\n', ... + fprintf('Linearized system has %d unobservable states!\n', ... (nx - rank(Wo))); % is the system at least detectable? - undetectable = 0 + undetectable = 0; for i = 1:nx if real(eigvals(i)) >= 0 % PBH test @@ -268,3 +266,4 @@ model.linear = struct(... ); end +% vim: ts=2 sw=2 et: diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx new file mode 100644 index 0000000..1ba0805 Binary files /dev/null and b/uav_model_uncertain.slx differ diff --git a/uav_params.m b/uav_params.m index dd0c101..3ceda9e 100644 --- a/uav_params.m +++ b/uav_params.m @@ -29,16 +29,21 @@ params.mechanical = struct(... '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 + 'InertiaTensor', 5e-3 * eye(3), ... % TODO true values, assume diagonal 'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now ); % ------------------------------------------------------------------------ % Actuator limits +% Cheap servos usually give a "speed" in seconds / 60 degrees without load +servo_speed = 0.19; % seconds per 60 deg +servo_angular_velocity = (pi / 3) / servo_speed; + params.actuators = struct(... 'TurbineMaxSpeed', 620.7, ... % In Hz 'ServoAbsMaxAngleDeg', 25, ... % in Degrees + 'ServoNominalAngularVelocity', servo_angular_velocity, ... 'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm ); @@ -53,9 +58,9 @@ F_max = 38.637; % in N (measured) k_T = sqrt(F_max / omega_max); params.aerodynamics = struct(... - 'ThrustOmegaProp', k_T, ... + 'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N) 'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random - 'DragCoefficients', [1, 1] .* 1e-3, ... % TODO + 'DragCoefficients', [1, 4] .* 1e-3, ... % TODO 'LiftCoefficient', 1e-3 ... % TODO ); @@ -78,4 +83,12 @@ params.linearization = struct(... 'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X ); -end \ No newline at end of file +% ------------------------------------------------------------------------ +% Performance requirements + +params.performance = struct(... + 'MaxHorizontalSpeed', 1, ... +); + +end +% vim: ts=2 sw=2 et: -- cgit v1.2.1