diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 177 |
1 files changed, 95 insertions, 82 deletions
@@ -1,75 +1,24 @@ -% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich -% % Controller design for a ducted fan VTOL micro-UAV. +% +% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich +% This work is distributed under a permissive license, see LICENSE.txt % ------------------------------------------------------------------------ % Clear environment and generate parameters clear; clc; close all; s = tf('s'); + +fprintf('Generating system parameters...\n') params = uav_params(); +ctrl = struct(); -do_plots = false; +do_plots = true; % ------------------------------------------------------------------------ %% Define performance requirements -% Mechanically, flaps are constrained to a max of 20~25 degrees, -% and they have a maximal angular speed -alpha_max = params.actuators.ServoAbsMaxAngle; -alpha_dot_max = params.actuators.ServoNominalAngularVelocity; - -W_Palpha = (s + 100 * alpha_dot_max) / (s + alpha_dot_max); -W_Palpha = alpha_max * W_Palpha / dcgain(W_Palpha); % adjust gain - -% Mechanically we have a maximal angular velocity for the propeller in the -% thruster, also there are a lot of unmodelled dynamics in the thruster -omega_max = params.actuators.TurbineMaxSpeed; -W_Pomega = (s + 50 * omega_max) / (s + omega_max); - -% We want a nice and smooth movements -v_xy_max = params.performance.MaxHorizontalSpeed; -v_z_max = params.performance.MaxVerticalSpeed; - -W_Pxy = 1 / (s + 1 / v_xy_max); -W_Pz = 1 / (s + 1 / v_z_max); - -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); - - 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'); -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, ... - 'Position', W_PP, ... - 'Velocity', W_PPdot, ... - 'Angle', W_PTheta, ... - 'AngularVelocity', W_POmega); +fprintf('Generating performance requirements...\n') +perf = uav_requirements(params, do_plots); % ------------------------------------------------------------------------ %% Define stability requirements @@ -78,48 +27,112 @@ W_malpha = tf(1,1); W_momega = tf(1,1); W_mState = tf(1,1); -if do_plots - figure; hold on; - - 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') -end +% if do_plots +% figure; hold on; +% +% 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') +% end uncert = struct(... 'FlapAngle', W_malpha * eye(4), ... 'Thrust', W_momega, ... 'StateLinApprox', W_mState * eye(12)); - + % ------------------------------------------------------------------------ -% Create UAV model +%% Create UAV model +fprintf('Generating system model...\n'); model = uav_model(params, perf, uncert); % ------------------------------------------------------------------------ +%% Perform LQR design + +% fprintf('Performing LQR controller design...\n') +% ctrl.lqr = uav_ctrl_lqr(params, model); + +% ------------------------------------------------------------------------ %% Perform H-infinity design +fprintf('Performing H-infinty controller design...\n') + idx = model.uncertain.index; -idx_ey = [idx.OutputError; idx.OutputNominal]; -idx_wu = [idx.InputDisturbance; idx.InputReference; idx.InputNominal]; +P = model.uncertain.StateSpace; + +% Get nominal system without uncertainty (for lower LFT) +P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ... + [idx.InputExogenous; 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)); +hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + 'AutoScale', 'off', 'RelTol', 1e-3); +[K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); +ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf); + +if gamma >= 1 + fprintf('Failed to syntesize controller (closed loop is unstable).\n') +end + +% ------------------------------------------------------------------------ +%% Measure Performance + +fprintf('Simulating closed loop...\n'); -hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', 'RelTol', 0.01); -[K_inf, N_inf, gamma, info] = hinfsyn(G, nmeas, nctrl, hinfopt); +nsamples = 500; +do_noise = true; +% uav_sim_step(params, model, ctrl.lqr, nsamples, do_plots); + +simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise); + +fprintf('Writing simulation results...\n'); +cols = [ + simout.StepX(:, simout.index.Position), ... + simout.StepX(:, simout.index.Velocity), ... + simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ... + simout.StepX(:, simout.index.Angles) * 180 / pi]; + +writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') % ------------------------------------------------------------------------ -% Verify performance satisfaction +%% Verify performance satisfaction via mu-analysis + +% omega = logspace(-3, 3, 250); +% +% N_inf = lft(P, K_inf); +% N_inf_frd = frd(N_inf, omega); +% +% % robust stability +% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ... +% model.uncertain.BlockStructure); +% +% % robust performance +% blk_perf = [model.uncertain.BlockStructure; +% model.uncertain.BlockStructurePerf]; +% +% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf); +% +% % nominal performance +% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous)); +% +% if do_plots +% figure; hold on; +% bodemag(mu_inf_rs(1)); +% bodemag(mu_inf_np(1)); +% bodemag(mu_inf_rs(2)); +% bodemag(mu_inf_np(2)); +% +% grid on; +% title('$\mu_\Delta(N)$', 'Interpreter', 'latex'); +% end % ------------------------------------------------------------------------ % Perform mu-Analysis & DK iteration |