340 lines
10 KiB
Matlab
340 lines
10 KiB
Matlab
% Matlab Init :noexport:ignore:
|
|
|
|
%% dcm_hac_iff.m
|
|
% Development of the HAC-IFF control strategy
|
|
|
|
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for data
|
|
|
|
%% Simscape Model - Nano Hexapod
|
|
addpath('./STEPS/')
|
|
|
|
%% Initialize Parameters for Simscape model
|
|
controller.type = 0; % Open Loop Control
|
|
|
|
%% Options for Linearization
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Open Simulink Model
|
|
mdl = 'simscape_dcm';
|
|
|
|
open(mdl)
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(1, 3, 1000);
|
|
|
|
% System Identification
|
|
% Let's identify the damped plant.
|
|
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Load DCM Kinematics and IFF controller
|
|
load('dcm_kinematics.mat');
|
|
load('Kiff.mat');
|
|
|
|
%% Identification of the damped plant with IFF
|
|
controller.type = 1; % IFF
|
|
G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
|
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
|
|
%% Comparison of the damped and undamped plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'd - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'uh - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'ur - IFF');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-12, 1e-8]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
% High Authority Controller
|
|
% Let's design a controller with a bandwidth of 100Hz.
|
|
% As the plant is well decoupled and well approximated by a constant at low frequency, the high authority controller can easily be designed with SISO loop shaping.
|
|
|
|
|
|
%% Controller design
|
|
wc = 2*pi*100; % Wanted crossover frequency [rad/s]
|
|
a = 2; % Lead parameter
|
|
|
|
Khac = diag(1./diag(abs(evalfr(G_dp, 1j*wc)))) * ... % Diagonal controller
|
|
wc/s * ... % Integrator
|
|
1/(sqrt(a))*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))) * ... % Lead
|
|
1/(s^2/(4*wc)^2 + 2*s/(4*wc) + 1); % Low pass filter
|
|
|
|
%% Save the HAC controller
|
|
save('mat/Khac_iff.mat', 'Khac');
|
|
|
|
%% Loop Gain
|
|
L_hac_lac = G_dp * Khac;
|
|
|
|
%% Bode Plot of the Loop Gain
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-2, 1e1]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
|
|
|
|
% #+name: fig:hac_iff_loop_gain_bode_plot
|
|
% #+caption: Bode Plot of the Loop gain for the High Authority Controller
|
|
% #+RESULTS:
|
|
% [[file:figs/hac_iff_loop_gain_bode_plot.png]]
|
|
|
|
|
|
%% Compute the Eigenvalues of the loop gain
|
|
Ldet = zeros(3, length(freqs));
|
|
|
|
Lmimo = squeeze(freqresp(L_hac_lac, freqs, 'Hz'));
|
|
for i_f = 1:length(freqs)
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
|
|
|
|
|
|
% As shown in the Root Locus plot in Figure [[fig:loci_hac_iff_fast_jack]], the closed loop system should be stable.
|
|
|
|
|
|
%% Plot of the eigenvalues of L in the complex plane
|
|
figure;
|
|
hold on;
|
|
% Angle used to draw the circles
|
|
theta = linspace(0, 2*pi, 100);
|
|
% Unit circle
|
|
plot(cos(theta), sin(theta), '--');
|
|
% Circle for module margin
|
|
plot(-1 + min(min(abs(Ldet + 1)))*cos(theta), min(min(abs(Ldet + 1)))*sin(theta), '--');
|
|
|
|
for i = 1:3
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
|
|
end
|
|
% Unstable Point
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
axis square;
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
|
|
|
% Performances
|
|
% In order to estimate the performances of the HAC-IFF control strategy, the transfer function from motion errors of the stepper motors to the motion error of the crystal is identified both in open loop and with the HAC-IFF strategy.
|
|
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Jack Motion Erros {3x1} [m]
|
|
io(io_i) = linio([mdl, '/stepper_errors'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Interferometer Output {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'output'); io_i = io_i + 1;
|
|
|
|
%% Identification of the transmissibility of errors in open-loop
|
|
controller.type = 0; % Open Loop
|
|
T_ol = inv(J_s_111)*linearize(mdl, io)*J_a_111;
|
|
T_ol.InputName = {'e_dz', 'e_ry', 'e_rx'};
|
|
T_ol.OutputName = {'dx', 'ry', 'rx'};
|
|
|
|
%% Load DCM Kinematics and IFF controller
|
|
load('dcm_kinematics.mat');
|
|
load('Kiff.mat');
|
|
|
|
%% Identification of the transmissibility of errors with HAC-IFF
|
|
controller.type = 3; % IFF
|
|
T_hl = inv(J_s_111)*linearize(mdl, io)*J_a_111;
|
|
T_hl.InputName = {'e_dz', 'e_ry', 'e_rx'};
|
|
T_hl.OutputName = {'dx', 'ry', 'rx'};
|
|
|
|
|
|
|
|
% #+RESULTS:
|
|
% : 1
|
|
|
|
% And both transmissibilities are compared in Figure [[fig:stepper_transmissibility_comp_ol_hac_iff]].
|
|
|
|
|
|
%% Transmissibility of stepper errors
|
|
f = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(freqresp(T_ol(1,1), f, 'Hz'))), '-', ...
|
|
'DisplayName', '$d_z$ - OL');
|
|
plot(f, abs(squeeze(freqresp(T_ol(2,2), f, 'Hz'))), '-', ...
|
|
'DisplayName', '$r_y$ - OL');
|
|
plot(f, abs(squeeze(freqresp(T_ol(3,3), f, 'Hz'))), '-', ...
|
|
'DisplayName', '$r_x$ - OL');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f, abs(squeeze(freqresp(T_hl(1,1), f, 'Hz'))), '--', ...
|
|
'DisplayName', '$d_z$ - HAC-IFF');
|
|
plot(f, abs(squeeze(freqresp(T_hl(2,2), f, 'Hz'))), '--', ...
|
|
'DisplayName', '$r_y$ - HAC-IFF');
|
|
plot(f, abs(squeeze(freqresp(T_hl(3,3), f, 'Hz'))), '--', ...
|
|
'DisplayName', '$r_x$ - HAC-IFF');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Stepper transmissibility');
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-2, 1e2]);
|
|
xlim([f(1), f(end)]);
|
|
|
|
% Close Loop noise budget
|
|
|
|
%% Load disturbances
|
|
load('asd_noises_disturbances.mat');
|
|
|
|
|
|
|
|
% Let's compute the amplitude spectral density of the jack motion errors due to the sensor noise, the actuator noise and disturbances.
|
|
|
|
|
|
%% Computation of ASD of contribution of inputs to the closed-loop motion
|
|
% Error due to disturbances
|
|
asd_d = abs(squeeze(freqresp(Wd*(1/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
|
|
% Error due to actuator noise
|
|
asd_u = abs(squeeze(freqresp(Wu*(G_dp(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
|
|
% Error due to sensor noise
|
|
asd_n = abs(squeeze(freqresp(Wn*(G_dp(1,1)*Khac(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
|
|
|
|
|
|
|
|
% The closed-loop ASD is then:
|
|
|
|
%% ASD of the closed-loop motion
|
|
asd_cl = sqrt(asd_d.^2 + asd_u.^2 + asd_n.^2);
|
|
|
|
|
|
|
|
% The obtained ASD are shown in Figure [[fig:close_loop_asd_noise_budget_hac_iff]].
|
|
|
|
|
|
%% Noise Budget (ASD)
|
|
f = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, asd_n, 'DisplayName', '$n$');
|
|
plot(f, asd_u, 'DisplayName', '$d_u$');
|
|
plot(f, asd_d, 'DisplayName', '$d$');
|
|
plot(f, asd_cl, 'k--', 'DisplayName', '$y$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
xlim([f(1), f(end)]);
|
|
ylim([1e-16, 1e-8]);
|
|
|
|
|
|
|
|
% #+name: fig:close_loop_asd_noise_budget_hac_iff
|
|
% #+caption: Closed Loop noise budget
|
|
% #+RESULTS:
|
|
% [[file:figs/close_loop_asd_noise_budget_hac_iff.png]]
|
|
|
|
% Let's compare the open-loop and close-loop cases (Figure [[fig:cps_comp_ol_cl_hac_iff]]).
|
|
|
|
% Amplitude spectral density of the open loop motion errors [m/sqrt(Hz)]
|
|
asd_ol = abs(squeeze(freqresp(Wd, f, 'Hz')));
|
|
|
|
% CPS of open-loop motion [m^2]
|
|
cps_ol = flip(-cumtrapz(flip(f), flip(asd_ol.^2)));
|
|
% CPS of closed-loop motion [m^2]
|
|
cps_cl = flip(-cumtrapz(flip(f), flip(asd_cl.^2)));
|
|
|
|
%% Cumulative Power Spectrum - Motion error of fast jack
|
|
figure;
|
|
hold on;
|
|
plot(f, cps_ol, 'DisplayName', sprintf('OL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_ol(1))));
|
|
plot(f, cps_cl, 'DisplayName', sprintf('CL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_cl(1))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('CPS [$m^2$]');
|
|
legend('location', 'southwest', 'FontSize', 8);
|
|
xlim([f(1), f(end)]);
|
|
% ylim([1e-16, 1e-8]);
|