diff options
| -rw-r--r-- | uav.m | 94 | ||||
| -rw-r--r-- | uav_ctrl_nominal.m | 38 | ||||
| -rw-r--r-- | uav_model.m | 95 | ||||
| -rw-r--r-- | uav_model_uncertain.slx | bin | 41970 -> 54074 bytes | |||
| -rw-r--r-- | uav_params.m | 7 |
5 files changed, 153 insertions, 81 deletions
@@ -8,8 +8,10 @@ clear; clc; close all; s = tf('s'); params = uav_params(); +do_plots = false; + % ------------------------------------------------------------------------ -% Define performance requirements +%% Define performance requirements % Mechanically, flaps are constrained to a max of 20~25 degrees, % and they have a maximal angular speed @@ -31,61 +33,69 @@ v_z_max = params.performance.MaxVerticalSpeed; W_Pxy = 1 / (s + 1 / v_xy_max); W_Pz = 1 / (s + 1 / v_z_max); -% Bode plots of performance requirements -figure; hold on; +if do_plots + % Bode plots of performance requirements + figure; hold on; + + bodemag(1/W_Palpha); + bodemag(1/W_Pomega); + bodemag(1/W_Pxy); + bodemag(1/W_Pz); -bodemag(1/W_Palpha); -bodemag(1/W_Pomega); -bodemag(1/W_Pxy); -bodemag(1/W_Pz); + grid on; + legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ... + '$W_{P,xy}$', '$W_{P,z}$', ... + 'interpreter', 'latex', 'fontSize', 8); + title('Performance requirements'); -grid on; -legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ... - '$W_{P,xy}$', '$W_{P,z}$', ... - 'interpreter', 'latex', 'fontSize', 8); -title('Performance requirements'); -% Step response of position requirements -figure; hold on; -step(W_Pxy); step(W_Pz); -grid on; -legend('$W_{P,xy}$', '$W_{P,z}$', 'interpreter', 'latex', 'fontSize', 8); -title('Step responses of position performance requirements'); + % Step response of position requirements + figure; hold on; + step(W_Pxy); step(W_Pz); + grid on; + legend('$W_{P,xy}$', '$W_{P,z}$', 'interpreter', 'latex', 'fontSize', 8); + title('Step responses of position performance requirements'); +end % Construct performance for position vector by combining xy and z W_PP = blkdiag(W_Pxy * eye(2), W_Pz); +W_PPdot = tf(1,1) * eye(3); +W_PTheta = tf(1,1) * eye(3); +W_POmega = tf(1,1) * eye(3); perf = struct(... 'FlapAngle', W_Palpha * eye(4), ... 'Thrust', W_Pomega, ... - 'PositionAccuracy', W_PP); + 'Position', W_PP, ... + 'Velocity', W_PPdot, ... + 'Angle', W_PTheta, ... + 'AngularVelocity', W_POmega); % ------------------------------------------------------------------------ -% Define stability requirements +%% Define stability requirements W_malpha = tf(1,1); W_momega = tf(1,1); -W_mTheta = tf(1,1); -W_mOmega = tf(1,1); +W_mState = tf(1,1); -figure; hold on; +if do_plots + figure; hold on; -bodemag(W_malpha); -bodemag(W_momega); -bodemag(W_mTheta); -bodemag(W_mOmega); + bodemag(W_malpha); + bodemag(W_momega); + bodemag(W_mState); -grid on; -legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ... - '$W_{m,\Theta}$', '$W_{m,\Omega}$', ... - 'interpreter', 'latex', 'fontSize', 8); -title('Uncertainties') + grid on; + legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ... + '$W_{m,\Theta}$', '$W_{m,\Omega}$', ... + 'interpreter', 'latex', 'fontSize', 8); + title('Uncertainties') +end uncert = struct(... 'FlapAngle', W_malpha * eye(4), ... 'Thrust', W_momega, ... - 'EulerAnglesApprox', W_mTheta * eye(3), ... - 'AngularRateApprox', W_mOmega * eye(3)); + 'StateLinApprox', W_mState * eye(12)); % ------------------------------------------------------------------------ % Create UAV model @@ -93,10 +103,20 @@ uncert = struct(... model = uav_model(params, perf, uncert); % ------------------------------------------------------------------------ -% Perform H-infinity design +%% Perform H-infinity design + +idx = model.uncertain.index; +idx_ey = [idx.OutputError; idx.OutputNominal]; +idx_wu = [idx.InputDisturbance; idx.InputReference; idx.InputNominal]; + +nmeas = max(size(idx.OutputNominal)); % size of y +nctrl = max(size(idx.InputNominal)); % size of u + +% Get nominal system without uncertainty (lower LFT) +G = minreal(model.uncertain.StateSpace(idx_ey, idx_wu)); -Gnom = model.linear.StateSpace; -G = model.uncertain.StateSpace; +hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', 'RelTol', 0.01); +[K_inf, N_inf, gamma, info] = hinfsyn(G, nmeas, nctrl, hinfopt); % ------------------------------------------------------------------------ % Verify performance satisfaction diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m deleted file mode 100644 index 1030233..0000000 --- a/uav_ctrl_nominal.m +++ /dev/null @@ -1,38 +0,0 @@ -% 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 = 1; % penalty on position -q_vel = 10; % penalty on linear velocity -q_ang = 100; % high penalty on angles -q_rate = 100; % very high penalty on angular velocities - -r_ang = 1; % flap movement is cheap -r_thrust = 100; % 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 -% vim: ts=2 sw=2 et: diff --git a/uav_model.m b/uav_model.m index ce7cddb..747f1b7 100644 --- a/uav_model.m +++ b/uav_model.m @@ -1,6 +1,30 @@ -% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% Compute models for Ducted Fan VTOL micro-UAV for given set of parameters. % -% Compute model for UAV for given set of parameters. +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich. +% 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 fil euav_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) @@ -210,6 +234,7 @@ model.linear = struct(... % ------------------------------------------------------------------------ % Check properties of linearized model + eigvals = eig(A); % Check system controllability / stabilizability @@ -269,6 +294,7 @@ end 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); @@ -279,9 +305,72 @@ usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d); % Save uncertain model model.uncertain = struct(... - 'Simulink', ulmod, ... + 'Simulink', ulmod, ... '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, 12), ... % 'w' inputs for noise + 'InputReference', make_idx(30, 12), ... % 'w' inputs for reference + 'InputNominal', make_idx(42, 5), ... % 'u' inputs + 'OutputUncertain', make_idx( 1, 17), ... % 'z' outputs + 'OutputError', make_idx(18, 17), ... % 'e' outputs + 'OutputNominal', make_idx(35, 12) ... % 'y' outputs +); + +% ------------------------------------------------------------------------ +% 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: diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx Binary files differindex 1ba0805..b958b25 100644 --- a/uav_model_uncertain.slx +++ b/uav_model_uncertain.slx diff --git a/uav_params.m b/uav_params.m index 0e82148..7bed89c 100644 --- a/uav_params.m +++ b/uav_params.m @@ -1,6 +1,6 @@ -% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% Retrieve or compute parameters for ducted-fan VTOL micro-UAV. % -% Retrieve / compute parameters for UAV. +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich function [params] = uav_params() @@ -48,7 +48,7 @@ params.actuators = struct(... params.measurements = struct(... 'SensorFusionBandwidth', 1e3, ... - 'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm + 'SensorFusionDelay', 8e-3 ... ); % ------------------------------------------------------------------------ @@ -73,6 +73,7 @@ params.aerodynamics = struct(... % Compute theoretical thrust required to make the UAV hover % from the relation mg = k * omega^2 +% FIXME: This value should probably be replaced with a measurement g = params.physics.Gravity; m = params.mechanical.Mass; k = params.aerodynamics.ThrustOmegaProp; |