From c644ae7f8a58321f2fb09f5aec275b63b90ba53c Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Mon, 17 Feb 2025 23:04:39 +0100 Subject: [PATCH] Tangle matlab files --- matlab/nass_1_kinematics.m | 23 + matlab/nass_2_active_damping.m | 515 +++++++++++++++ matlab/nass_3_hac.m | 1105 ++++++++++++++++++++++++++++++++ simscape-nass.org | 25 +- 4 files changed, 1661 insertions(+), 7 deletions(-) create mode 100644 matlab/nass_1_kinematics.m create mode 100644 matlab/nass_2_active_damping.m create mode 100644 matlab/nass_3_hac.m diff --git a/matlab/nass_1_kinematics.m b/matlab/nass_1_kinematics.m new file mode 100644 index 0000000..b1b5de1 --- /dev/null +++ b/matlab/nass_1_kinematics.m @@ -0,0 +1,23 @@ +%% 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 +addpath('./src/'); % Path for functions +addpath('./STEPS/'); % Path for STEPS +addpath('./subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './mat/'; + +% Simulink Model name +mdl = 'nass_model'; + +%% Colors for the figures +colors = colororder; + +%% Frequency Vector [Hz] +freqs = logspace(0, 3, 1000); diff --git a/matlab/nass_2_active_damping.m b/matlab/nass_2_active_damping.m new file mode 100644 index 0000000..0c7312f --- /dev/null +++ b/matlab/nass_2_active_damping.m @@ -0,0 +1,515 @@ +%% 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 +addpath('./src/'); % Path for functions +addpath('./STEPS/'); % Path for STEPS +addpath('./subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './mat/'; + +% Simulink Model name +mdl = 'nass_model'; + +%% Colors for the figures +colors = colororder; + +%% Frequency Vector [Hz] +freqs = logspace(0, 3, 1000); + +% IFF Plant +% <> + +% Transfer functions from actuator forces $f_i$ to force sensor measurements $f_{mi}$ are computed using the multi-body model. +% Figure ref:fig:nass_iff_plant_effect_kp examines how parallel stiffness affects the plant dynamics, with identification performed at maximum spindle velocity $\Omega_z = 360\,\text{deg/s}$ and with a payload mass of 25 kg. + +% Without parallel stiffness (Figure ref:fig:nass_iff_plant_no_kp), the dynamics exhibits non-minimum phase zeros at low frequency, confirming predictions from the three-degree-of-freedom rotating model. +% Adding parallel stiffness (Figure ref:fig:nass_iff_plant_kp) transforms these into minimum phase complex conjugate zeros, enabling unconditionally stable decentralized IFF implementation. + +% Though both cases show significant coupling around resonances, stability is guaranteed by the collocated arrangement of actuators and sensors [[cite:&preumont08_trans_zeros_struc_contr_with]]. + + +%% Identify the IFF plant dynamics using the Simscape model + +% Initialize each Simscape model elements +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeSimplifiedNanoHexapod(); + +% Initial Simscape Configuration +initializeSimscapeConfiguration('gravity', false); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); +initializeReferences(); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N] + +%% Identify for multi payload masses (no rotation) +initializeReferences(); % No Spindle Rotation +% 1kg Sample +initializeSample('type', 'cylindrical', 'm', 1); +G_iff_m1 = linearize(mdl, io); +G_iff_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff_m1.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +% 25kg Sample +initializeSample('type', 'cylindrical', 'm', 25); +G_iff_m25 = linearize(mdl, io); +G_iff_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff_m25.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +% 50kg Sample +initializeSample('type', 'cylindrical', 'm', 50); +G_iff_m50 = linearize(mdl, io); +G_iff_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff_m50.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Effect of Rotation +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 1); % 360 deg/s +initializeSample('type', 'cylindrical', 'm', 25); +G_iff_m25_Rz = linearize(mdl, io, 0.1); +G_iff_m25_Rz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff_m25_Rz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Effect of Rotation - No added parallel stiffness +initializeSimplifiedNanoHexapod('actuator_kp', 0); +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 1); % 360 deg/s +initializeSample('type', 'cylindrical', 'm', 25); +G_iff_m25_Rz_no_kp = linearize(mdl, io, 0.1); +G_iff_m25_Rz_no_kp.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff_m25_Rz_no_kp.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% IFF Plant - Without parallel stiffness +f = logspace(-1,3,1000); +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,1), f, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$') +for i = 2:6 + plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e1]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz')))), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-20, 200]); +yticks([0:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([f(1), f(end)]); + +%% IFF Plant - With added parallel stiffness +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), f, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $k_p = 50N/mm$') +for i = 2:6 + plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$f_{ni}/f_j$ - $k_p = 50N/mm$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e1]); +leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-20, 200]); +yticks([0:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([f(1), f(end)]); + + + +% #+name: fig:nass_iff_plant_effect_kp +% #+caption: Effect of stiffness parallel to the force sensor on the IFF plant with $\Omega_z = 360\,\text{deg/s}$ and payload mass of 25kg. The dynamics without parallel stiffness has non-minimum phase zeros at low frequency (\subref{fig:nass_iff_plant_no_kp}). The added parallel stiffness transforms the non-minimum phase zeros to complex conjugate zeros (\subref{fig:nass_iff_plant_kp}) +% #+attr_latex: :options [htbp] +% #+begin_figure +% #+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_no_kp}without parallel stiffness} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :width 0.95\linewidth +% [[file:figs/nass_iff_plant_no_kp.png]] +% #+end_subfigure +% #+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_kp}with parallel stiffness} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :width 0.95\linewidth +% [[file:figs/nass_iff_plant_kp.png]] +% #+end_subfigure +% #+end_figure + +% The effect of rotation, shown in Figure ref:fig:nass_iff_plant_effect_rotation, is negligible as the actuator stiffness ($k_a = 1\,N/\mu m$) is large compared to the negative stiffness induced by gyroscopic effects (estimated from the 3DoF rotating model). + +% Figure ref:fig:nass_iff_plant_effect_payload illustrate the effect of payload mass on the plant dynamics. +% While the poles and zeros are shifting with payload mass, the alternating pattern of poles and zeros is maintained, ensuring that the phase remains bounded between 0 and 180 degrees, and thus good robustness properties. + + +%% Effect of spindle's rotation on the IFF Plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$ deg/s') +plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +% plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... +% 'DisplayName', '$f_{ni}/f_j$') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:)); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-20, 200]); +yticks([0:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Effect of the payload's mass on the IFF Plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$f_{ni}/f_i$ - 1kg') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$f_{ni}/f_i$ - 25kg') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$f_{ni}/f_i$ - 50kg') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-20, 200]); +yticks([0:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Controller Design +% <> + +% Previous analysis using the 3DoF rotating model showed that decentralized Integral Force Feedback (IFF) with pure integrators is unstable due to gyroscopic effects caused by spindle rotation. +% This finding is also confirmed with the multi-body model of the NASS: the system is unstable when using pure integrators and without parallel stiffness. + +% This instability can be mitigated by introducing sufficient stiffness in parallel with the force sensors. +% However, as illustrated in Figure ref:fig:nass_iff_plant_kp, adding parallel stiffness increases the low frequency gain. +% If using pure integrators, this would results in high loop gain at low frequencies, adversely affecting the damped plant dynamics, which is undesirable. +% To resolve this issue, a second-order high-pass filter is introduced to limit the low frequency gain, as shown in Equation eqref:eq:nass_kiff. + +% \begin{equation}\label{eq:nass_kiff} +% \bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix} +% K_{\text{IFF}}(s) & & 0 \\ +% & \ddots & \\ +% 0 & & K_{\text{IFF}}(s) +% \end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s} \cdot \frac{\frac{s^2}{\omega_z^2}}{\frac{s^2}{\omega_z^2} + 2 \xi_z \frac{s}{\omega_z} + 1} +% \end{equation} + +% The cut-off frequency of the second-order high-pass filter is tuned to be below the frequency of the complex conjugate zero for the highest mass, which is at $5\,\text{Hz}$. +% The overall gain is then increased to have large loop gain around resonances to be damped, as illustrated in Figure ref:fig:nass_iff_loop_gain. + + +%% Verify that parallel stiffness permits to have a stable plant +Kiff_pure_int = -200/s*eye(6); +isstable(feedback(G_iff_m25_Rz, Kiff_pure_int, 1)) +isstable(feedback(G_iff_m25_Rz_no_kp, Kiff_pure_int, 1)) + +%% IFF Controller Design +% Second order high pass filter +wz = 2*pi*2; +xiz = 0.7; +Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1); + +Kiff = -200 * ... % Gain + 1/(0.01*2*pi + s) * ... % LPF: provides integral action + Ghpf * ... % 2nd order HPF (limit low frequency gain) + eye(6); % Diagonal 6x6 controller (i.e. decentralized) + +Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'}; +Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; + +% The designed IFF controller is saved +save('./mat/nass_K_iff.mat', 'Kiff'); + +%% Loop gain for the decentralized IFF +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '1kg') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '25kg') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '50kg') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-110, 200]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:nass_iff_loop_gain +% #+caption: Loop gain for the decentralized IFF: $K_{\text{IFF}}(s) \cdot \frac{f_{mi}}{f_i}(s)$ +% #+RESULTS: +% [[file:figs/nass_iff_loop_gain.png]] + +% To verify stability, root loci for the three payload configurations are computed and shown in Figure ref:fig:nass_iff_root_locus. +% The results demonstrate that the closed-loop poles remain within the left-half plane, indicating the robust stability properties of the applied decentralized IFF. + + +%% Root Locus for the Decentralized IFF controller - 1kg Payload +figure; +gains = logspace(-2, 1, 200); + +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); +nexttile(); +hold on; + +plot(real(pole(G_iff_m1)), imag(pole(G_iff_m1)), 'x', 'color', colors(1,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(G_iff_m1)), imag(tzero(G_iff_m1)), 'o', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(G_iff_m1, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(G_iff_m1, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'kx', ... + 'DisplayName', '$g_{opt}$'); + +xline(0); +yline(0); +hold off; +axis equal; +xlim([-900, 100]); ylim([-100, 900]); +xticks([-900:100:0]); +yticks([0:100:900]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); + +%% Root Locus for the Decentralized IFF controller - 25kg Payload +gains = logspace(-2, 1, 200); + +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); +nexttile(); +hold on; + +plot(real(pole(G_iff_m25)), imag(pole(G_iff_m25)), 'x', 'color', colors(2,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(G_iff_m25)), imag(tzero(G_iff_m25)), 'o', 'color', colors(2,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(G_iff_m25, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(G_iff_m25, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'kx', ... + 'DisplayName', '$g_{opt}$'); + +xline(0); +yline(0); +hold off; +axis equal; +xlim([-900, 100]); ylim([-100, 900]); +xticks([-900:100:0]); +yticks([0:100:900]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); + +%% Root Locus for the Decentralized IFF controller - 50kg Payload +gains = logspace(-2, 1, 200); + +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); +nexttile(); +hold on; + +plot(real(pole(G_iff_m50)), imag(pole(G_iff_m50)), 'x', 'color', colors(3,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(G_iff_m50)), imag(tzero(G_iff_m50)), 'o', 'color', colors(3,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(G_iff_m50, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(G_iff_m50, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'kx', ... + 'DisplayName', '$g_{opt}$'); + +xline(0); +yline(0); +hold off; +axis equal; +xlim([-900, 100]); ylim([-100, 900]); +xticks([-900:100:0]); +yticks([0:100:900]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); diff --git a/matlab/nass_3_hac.m b/matlab/nass_3_hac.m new file mode 100644 index 0000000..3b7cb1e --- /dev/null +++ b/matlab/nass_3_hac.m @@ -0,0 +1,1105 @@ +%% 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 +addpath('./src/'); % Path for functions +addpath('./STEPS/'); % Path for STEPS +addpath('./subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './mat/'; + +% Simulink Model name +mdl = 'nass_model'; + +%% Colors for the figures +colors = colororder; + +%% Frequency Vector [Hz] +freqs = logspace(0, 3, 1000); + +% HAC Plant + + +%% Identify the IFF plant dynamics using the Simscape model + +% Initialize each Simscape model elements +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeSimplifiedNanoHexapod(); +initializeSample('type', 'cylindrical', 'm', 1); + +% Initial Simscape Configuration +initializeSimscapeConfiguration('gravity', false); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); +initializeReferences(); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m] + +%% Identify HAC Plant without using IFF +initializeSample('type', 'cylindrical', 'm', 1); +G_m1 = linearize(mdl, io); +G_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +initializeSample('type', 'cylindrical', 'm', 25); +G_m25 = linearize(mdl, io); +G_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +initializeSample('type', 'cylindrical', 'm', 50); +G_m50 = linearize(mdl, io); +G_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +%% Effect of Rotation +initializeSample('type', 'cylindrical', 'm', 1); +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 1); % 360 deg/s + +G_m1_Rz = linearize(mdl, io, 0.1); +G_m1_Rz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m1_Rz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + + + +% - Effect of rotation: ref:fig:nass_undamped_plant_effect_Wz +% Add some coupling at low frequency, but still small at the considered velocity. +% This is thanks to the relatively stiff nano-hexapod (CF rotating model) +% - Effect of payload mass: +% Decrease resonance frequencies +% Increase coupling: ref:fig:nass_undamped_plant_effect_mass +% => control challenge for high payload masses +% - Other effects such as: Ry tilt angle, Rz spindle position, micro-hexapod position are found to have negligible effect on the plant dynamics. +% This is thanks to the fact the the plant dynamics is well decoupled from the micro-station dynamics. + + +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_m1(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 0$') +plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 360$ deg/s') +plot(freqs, abs(squeeze(freqresp(G_m1(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$') +plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_m1(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-11, 2e-5]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:)); + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz')))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-200, 20]); +yticks([-180:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 1 kg') +plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 25 kg') +plot(freqs, abs(squeeze(freqresp(G_m50(1,1), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 50 kg') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m50(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', colors(3,:), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-11, 2e-5]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:)); + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:)); + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz')))), 'color', colors(3,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-200, 20]); +yticks([-180:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:nass_undamped_plant_effect +% #+caption: Effect of the Spindle's rotational velocity on the positioning plant (\subref{fig:nass_undamped_plant_effect_Wz}) and effect of the payload's mass on the positioning plant (\subref{fig:nass_undamped_plant_effect_mass}) +% #+attr_latex: :options [htbp] +% #+begin_figure +% #+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_Wz}Effect of rotational velocity $\Omega_z$} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :width 0.95\linewidth +% [[file:figs/nass_undamped_plant_effect_Wz.png]] +% #+end_subfigure +% #+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_mass}Effect of payload's mass} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :width 0.95\linewidth +% [[file:figs/nass_undamped_plant_effect_mass.png]] +% #+end_subfigure +% #+end_figure + + +% - Effect of IFF on the plant ref:fig:nass_comp_undamped_damped_plant_m1 +% Modes are well damped +% Small coupling increase at low frequency +% - Benefits of using IFF ref:fig:nass_hac_plants +% with added damping, the set of plants to be controlled (with payloads from 1kg to 50kg) is more easily controlled. +% Between 10 and 50Hz, the plant dynamics does not vary a lot with the frequency, whereas without active damping, it would be impossible to design a robust controller with bandwidth above 10Hz that is robust to the change of payload + + +%% Identify HAC Plant without using IFF +initializeReferences(); % No Spindle Rotation +initializeController('type', 'iff'); % Implemented IFF controller +load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller + +% 1kg payload +initializeSample('type', 'cylindrical', 'm', 1); +G_hac_m1 = linearize(mdl, io); +G_hac_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_hac_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +% 25kg payload +initializeSample('type', 'cylindrical', 'm', 25); +G_hac_m25 = linearize(mdl, io); +G_hac_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_hac_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +% 50kg payload +initializeSample('type', 'cylindrical', 'm', 50); +G_hac_m50 = linearize(mdl, io); +G_hac_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_hac_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +% Check stability +if not(isstable(G_hac_m1) && isstable(G_hac_m25) && isstable(G_hac_m50)) + warning('One of HAC plant is not stable') +end + +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, OL') +plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, with IFF') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-10, 2e-5]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:)); + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz')))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-200, 20]); +yticks([-180:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Comparison of all the undamped FRF and all the damped FRF +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped - $\epsilon\mathcal{L}_i/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'DisplayName', 'Damped - $\epsilon\mathcal{L}_i/f_i^\prime$'); +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); +end +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_hac_m50(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +% ylim([1e-8, 1e-4]); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m50(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +ylim([-200, 20]); +yticks([-180:45:180]); + +linkaxes([ax1,ax2],'x'); +% xlim([1, 5e2]); + +% Effect of micro-station compliance + +% Micro-Station complex dynamics has almost no effect on the plant dynamics (Figure ref:fig:nass_effect_ustation_compliance): +% - adds some alternating poles and zeros above 100Hz, which should not be an issue for control + + +%% Identify plant with "rigid" micro-station +initializeGround('type', 'rigid'); +initializeGranite('type', 'rigid'); +initializeTy('type', 'rigid'); +initializeRy('type', 'rigid'); +initializeRz('type', 'rigid'); +initializeMicroHexapod('type', 'rigid'); +initializeSimplifiedNanoHexapod(); +initializeSample('type', 'cylindrical', 'm', 25); + +initializeReferences(); +initializeController('type', 'open-loop'); % Implemented IFF controller +load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m] + +G_m25_rigid = linearize(mdl, io); +G_m25_rigid.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m25_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +%% Effect of the micro-station limited compliance on the plant dynamics +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_m25_rigid( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, OL') +plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, with IFF') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_m25_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_m25_rigid( i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-10, 2e-5]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:)); + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-200, 20]); +yticks([-180:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Higher or lower nano-hexapod stiffness? + +% *Goal*: confirm the analysis with simpler models (uniaxial and 3DoF) that a nano-hexapod stiffness of $\approx 1\,N/\mu m$ should give better performances than a very stiff or very soft nano-hexapod. + +% - *Stiff nano-hexapod*: +% uniaxial model: high nano-hexapod stiffness induce coupling between the nano-hexapod and the micro-station dynamics. +% considering the complex dynamics of the micro-station as shown by the modal analysis, that would result in a complex system to control +% To show that, a nano-hexapod with actuator stiffness equal to 100N/um is initialized, payload of 25kg. +% The dynamics from $\bm{f}$ to $\bm{\epsilon}_{\mathcal{L}}$ is identified and compared to the case where the micro-station is infinitely rigid (figure ref:fig:nass_stiff_nano_hexapod_coupling_ustation): +% - Coupling induced by the micro-station: much more complex and difficult to model / predict +% - Similar to what was predicted using the uniaxial model +% - *Soft nano-hexapod*: +% Nano-hexapod with stiffness of 0.01N/um is initialized, payload of 25kg. +% Dynamics is identified with no spindle rotation, and with spindle rotation of 36deg/s and 360deg/s (Figure ref:fig:nass_soft_nano_hexapod_effect_Wz) +% - Rotation as huge effect on the dynamics: unstable for high rotational velocities, added coupling due to gyroscopic effects, and change of resonance frequencies as a function of the rotational velocity +% - Simple 3DoF rotating model is helpful to understand the complex effect of the rotation => similar conclusion +% - Say that controlling the frame of the struts is not adapted with a soft nano-hexapod, but we should rather control in the frame matching the center of mass of the payload, but we would still obtain large coupling and change of dynamics due to gyroscopic effects. + + +%% Identify Dynamics with a Stiff nano-hexapod (100N/um) + +% Initialize each Simscape model elements +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e3); +initializeSample('type', 'cylindrical', 'm', 25); + +% Initial Simscape Configuration +initializeSimscapeConfiguration('gravity', false); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); +initializeReferences(); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m] + +% Identify Plant +G_m25_pz = linearize(mdl, io); +G_m25_pz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +%% Compare with Nano-Hexapod alone (rigid micro-station) +initializeGround('type', 'rigid'); +initializeGranite('type', 'rigid'); +initializeTy('type', 'rigid'); +initializeRy('type', 'rigid'); +initializeRz('type', 'rigid'); +initializeMicroHexapod('type', 'rigid'); + +% Identify Plant +G_m25_pz_rigid = linearize(mdl, io); +G_m25_pz_rigid.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +%% Stiff nano-hexapod - Coupling with the micro-station +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid') +plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station') +plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - Rigid') +plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - $\mu$-station') +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-12, 3e-7]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:)); + plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-200, 20]); +yticks([-180:45:180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Identify Dynamics with a Soft nano-hexapod (0.01N/um) +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeSimplifiedNanoHexapod('actuator_k', 1e4, 'actuator_kp', 0, 'actuator_c', 1); + +% Initialize each Simscape model elements +initializeSample('type', 'cylindrical', 'm', 25); % 25kg payload +initializeController('type', 'open-loop'); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N] +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m] + +% Identify the dynamics without rotation +initializeReferences(); +G_m1_vc = linearize(mdl, io); +G_m1_vc.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m1_vc.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +% Identify the dynamics with 36 deg/s rotation +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 10); % 36 deg/s + +G_m1_vc_Rz_slow = linearize(mdl, io, 0.1); +G_m1_vc_Rz_slow.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m1_vc_Rz_slow.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +% Identify the dynamics with 360 deg/s rotation +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 1); % 360 deg/s + +G_m1_vc_Rz_fast = linearize(mdl, io, 0.1); +G_m1_vc_Rz_fast.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_m1_vc_Rz_fast.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'}; + +%% Soft Nano-Hexapod - effect of rotational velocity on the dynamics +f = logspace(-1,2,200); +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(squeeze(freqresp(G_m1_vc(1,1), f, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$') +plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(1,1), f, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 36$ deg/s') +plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(1,1), f, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s') +for i = 1:5 + for j = i+1:6 + plot(f, abs(squeeze(freqresp(G_m1_vc(i,j), f, 'Hz'))), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,j), f, 'Hz'))), 'color', [colors(3,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(f, abs(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +for i = 2:6 + plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end +for i = 2:6 + plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:), ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-2]); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:)); + plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:)); + plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180:90:180]); + +linkaxes([ax1,ax2],'x'); +xlim([f(1), f(end)]); + +% Controller design + +% In this section, a high authority controller is design such that: +% - it is robust to the change of payload mass (i.e. is should be stable for all the damped plants of Figure ref:fig:nass_hac_plants) +% - it has reasonably high bandwidth to give good performances (here 10Hz) + +% eqref:eq:nass_robust_hac + +% \begin{equation}\label{eq:nass_robust_hac} +% K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}, \quad \left( \omega_c = 2\pi10\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi80\,\text{rad/s} \right) +% \end{equation} + + +%% HAC Design +% Wanted crossover +wc = 2*pi*10; % [rad/s] + +% Integrator +H_int = wc/s; + +% Lead to increase phase margin +a = 2; % Amount of phase lead / width of the phase lead / high frequency gain +H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); + +% Low Pass filter to increase robustness +H_lpf = 1/(1 + s/2/pi/80); + +% Gain to have unitary crossover at wc +H_gain = 1./abs(evalfr(G_hac_m50(1,1), 1j*wc)); + +% Decentralized HAC +Khac = -H_gain * ... % Gain + H_int * ... % Integrator + H_lead * ... % Low Pass filter + H_lpf * ... % Low Pass filter + eye(6); % 6x6 Diagonal + +% The designed HAC controller is saved +save('./mat/nass_K_hac.mat', 'Khac'); + + + +% - "Decentralized" Loop Gain: +% Bandwidth around 10Hz +% - Characteristic Loci: +% Stable for all payloads with acceptable stability margins + + +%% "Diagonal" loop gain for the High Authority Controller +f = logspace(-1, 2, 1000); +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), ... + 'color', [colors(1,:), 0.5], 'DisplayName', '1kg'); +plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), ... + 'color', [colors(2,:), 0.5], 'DisplayName', '25kg'); +plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), ... + 'color', [colors(3,:), 0.5], 'DisplayName', '50kg'); +for i = 2:6 + plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5]); + plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5]); + plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([0.1, 100]); + +%% Characteristic Loci for the High Authority Controller +Ldet_m1 = zeros(6, length(freqs)); +Lmimo_m1 = squeeze(freqresp(-G_hac_m1*Khac, freqs, 'Hz')); +for i_f = 2:length(freqs) + Ldet_m1(:, i_f) = eig(squeeze(Lmimo_m1(:,:,i_f))); +end +Ldet_m25 = zeros(6, length(freqs)); +Lmimo_m25 = squeeze(freqresp(-G_hac_m25*Khac, freqs, 'Hz')); +for i_f = 2:length(freqs) + Ldet_m25(:, i_f) = eig(squeeze(Lmimo_m25(:,:,i_f))); +end +Ldet_m50 = zeros(6, length(freqs)); +Lmimo_m50 = squeeze(freqresp(-G_hac_m50*Khac, freqs, 'Hz')); +for i_f = 2:length(freqs) + Ldet_m50(:, i_f) = eig(squeeze(Lmimo_m50(:,:,i_f))); +end + +figure; +hold on; +plot(real(squeeze(Ldet_m1(1,:))), imag(squeeze(Ldet_m1(1,:))), ... + '.', 'color', colors(1, :), ... + 'DisplayName', '1kg'); +plot(real(squeeze(Ldet_m1(1,:))),-imag(squeeze(Ldet_m1(1,:))), ... + '.', 'color', colors(1, :), ... + 'HandleVisibility', 'off'); +plot(real(squeeze(Ldet_m25(1,:))), imag(squeeze(Ldet_m25(1,:))), ... + '.', 'color', colors(2, :), ... + 'DisplayName', '25kg'); +plot(real(squeeze(Ldet_m25(1,:))),-imag(squeeze(Ldet_m25(1,:))), ... + '.', 'color', colors(2, :), ... + 'HandleVisibility', 'off'); +plot(real(squeeze(Ldet_m50(1,:))), imag(squeeze(Ldet_m50(1,:))), ... + '.', 'color', colors(3, :), ... + 'DisplayName', '50kg'); +plot(real(squeeze(Ldet_m50(1,:))),-imag(squeeze(Ldet_m50(1,:))), ... + '.', 'color', colors(3, :), ... + 'HandleVisibility', 'off'); +for i = 2:6 + plot(real(squeeze(Ldet_m1(i,:))), imag(squeeze(Ldet_m1(i,:))), ... + '.', 'color', colors(1, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet_m1(i,:))), -imag(squeeze(Ldet_m1(i,:))), ... + '.', 'color', colors(1, :), ... + 'HandleVisibility', 'off'); + + plot(real(squeeze(Ldet_m25(i,:))), imag(squeeze(Ldet_m25(i,:))), ... + '.', 'color', colors(2, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet_m25(i,:))), -imag(squeeze(Ldet_m25(i,:))), ... + '.', 'color', colors(2, :), ... + 'HandleVisibility', 'off'); + + plot(real(squeeze(Ldet_m50(i,:))), imag(squeeze(Ldet_m50(i,:))), ... + '.', 'color', colors(3, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet_m50(i,:))), -imag(squeeze(Ldet_m50(i,:))), ... + '.', 'color', colors(3, :), ... + 'HandleVisibility', 'off'); +end +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real Part'); ylabel('Imaginary Part'); +axis square +xlim([-1.8, 0.2]); ylim([-1, 1]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +% TODO Sensitivity to disturbances :noexport: + +% - Compute transfer functions from spindle vertical error to sample vertical error with HAC-IFF +% Compare without the NASS, and with just IFF +% - Same for horizontal + + +% Initialize each Simscape model elements +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeSimplifiedNanoHexapod(); +initializeSample('type', 'cylindrical', 'm', 1); + +% Initial Simscape Configuration +initializeSimscapeConfiguration('gravity', false); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); +initializeReferences(); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_y'); io_i = io_i + 1; % Spindle Lateral Vibration [N] +io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Spindle Vertical Vibration [N] +io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fdy_z'); io_i = io_i + 1; % Vertical Ground Motion [m] +io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwy'); io_i = io_i + 1; % Vertical Ground Motion [m] +io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Vertical Ground Motion [m] +io(io_i) = linio([mdl, '/NASS'], 2, 'output', [], 'y'); io_i = io_i + 1; % Lateral Displacement [m] +io(io_i) = linio([mdl, '/NASS'], 2, 'output', [], 'z'); io_i = io_i + 1; % Vertical Displacement [m] + +Gd_ol = linearize(mdl, io); +Gd_ol.InputName = {'Frz_y', 'Frz_z', 'Fdy_z', 'Dwy', 'Dwz'}; +Gd_ol.OutputName = {'Dy', 'Dz'}; + +initializeController('type', 'iff'); % Implemented IFF controller +load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller + +Gd_iff = linearize(mdl, io); +Gd_iff.InputName = {'Frz_y', 'Frz_z', 'Fdy_z', 'Dwy', 'Dwz'}; +Gd_iff.OutputName = {'Dy', 'Dz'}; + +initializeController('type', 'hac-iff'); % Implemented IFF controller +load('nass_K_hac.mat', 'Khac'); % Load designed HAC controller + +Gd_hac_iff = linearize(mdl, io); +Gd_hac_iff.InputName = {'Frz_y', 'Frz_z', 'Fdy_z', 'Dwy', 'Dwz'}; +Gd_hac_iff.OutputName = {'Dy', 'Dz'}; + +dist = load('ustation_disturbance_psd.mat'); + + + +% Spindle, lateral: + +figure; +hold on; +plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dy', 'Frz_y'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_iff('Dy', 'Frz_y'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dy', 'Frz_y'), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $D_z/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]'); +xticks([1e0, 1e1, 1e2]); +xlim([1, 500]); + + + +% Spindle, vertical: + +freqs = logspace(-1,3,1000); +figure; +hold on; +plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dz', 'Frz_z'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_iff('Dz', 'Frz_z'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dz', 'Frz_z'), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $D_z/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]'); + + + +% Ground motion, vertical: + +freqs = logspace(-1,3,1000); +figure; +hold on; +plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dz', 'Dwz'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_iff('Dz', 'Dwz'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dz', 'Dwz'), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $D_z/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]'); +xticks([1e0, 1e1, 1e2]); +% xlim([1, 500]); + + + +% Ground motion, lateral: + +figure; +hold on; +plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dy', 'Dwy'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_iff('Dy', 'Dwy'), freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dy', 'Dwy'), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $D_y/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]'); +xticks([1e0, 1e1, 1e2]); +xlim([1, 500]); + + + +% Noise Budget: + +figure; +hold on; +plot(dist.gm_dist.f, sqrt(flip(-cumtrapz(flip(dist.gm_dist.f), flip(dist.gm_dist.pxx_y.*abs(squeeze(freqresp(Gd_ol( 'Dy', 'Dwy'), dist.gm_dist.f, 'Hz'))).^2))))); +plot(dist.gm_dist.f, sqrt(flip(-cumtrapz(flip(dist.gm_dist.f), flip(dist.gm_dist.pxx_y.*abs(squeeze(freqresp(Gd_iff( 'Dy', 'Dwy'), dist.gm_dist.f, 'Hz'))).^2))))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('ASD [m/sqrt(Hz)]'); xlabel('Frequency [Hz]'); +xticks([1e0, 1e1, 1e2]); +xlim([1, 500]); + +% Tomography experiment + +% - Validation of concept with tomography scans at the highest rotational velocity of $\Omega_z = 360\,\text{deg/s}$ +% - Compare obtained results with the smallest beam size that is expected with future beamline upgrade: 200nm (horizontal size) x 100nm (vertical size) +% - Take into account the two main sources of disturbances: ground motion, spindle vibrations +% Other noise sources are not taken into account here as they will be optimized latter (detail design phase): measurement noise, electrical noise for DAC and voltage amplifiers, ... + +% The open-loop errors and the closed-loop errors for the tomography scan with the light sample $1\,kg$ are shown in Figure ref:fig:nass_tomo_1kg_60rpm. + + +% Sample is not centered with the rotation axis +% This is done by offsetfing the micro-hexapod by 0.9um +P_micro_hexapod = [0.9e-6; 0; 0]; % [m] + +open(mdl); +set_param(mdl, 'StopTime', '2'); + +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod('AP', P_micro_hexapod); +initializeSample('type', 'cylindrical', 'm', 1); + +initializeSimscapeConfiguration('gravity', false); +initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3); + +initializeDisturbances(... + 'Dw_x', true, ... % Ground Motion - X direction + 'Dw_y', true, ... % Ground Motion - Y direction + 'Dw_z', true, ... % Ground Motion - Z direction + 'Fdy_x', false, ... % Translation Stage - X direction + 'Fdy_z', false, ... % Translation Stage - Z direction + 'Frz_x', true, ... % Spindle - X direction + 'Frz_y', true, ... % Spindle - Y direction + 'Frz_z', true); % Spindle - Z direction + +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 1, ... + 'Dh_pos', [P_micro_hexapod; 0; 0; 0]); + +% Open-Loop Simulation without Nano-Hexapod - 1kg payload +initializeSimplifiedNanoHexapod('type', 'none'); +initializeController('type', 'open-loop'); +sim(mdl); +exp_tomo_ol_m1 = simout; + +% Closed-Loop Simulation with NASS +initializeSimplifiedNanoHexapod(); +initializeController('type', 'hac-iff'); +load('nass_K_iff.mat', 'Kiff'); +load('nass_K_hac.mat', 'Khac'); + +% 1kg payload +initializeSample('type', 'cylindrical', 'm', 1); +sim(mdl); +exp_tomo_cl_m1 = simout; + +% 25kg payload +initializeSample('type', 'cylindrical', 'm', 25); +sim(mdl); +exp_tomo_cl_m25 = simout; + +% 50kg payload +initializeSample('type', 'cylindrical', 'm', 50); +sim(mdl); +exp_tomo_cl_m50 = simout; + +% Slower tomography for high payload mass +% initializeReferences(... +% 'Rz_type', 'rotating', ... +% 'Rz_period', 10, ... % 36deg/s +% 'Dh_pos', [P_micro_hexapod; 0; 0; 0]); +% initializeSample('type', 'cylindrical', 'm', 50); +% set_param(mdl, 'StopTime', '5'); +% sim(mdl); +% exp_tomo_cl_m50_slow = simout; + +%% Simulation of tomography experiment - 1kg payload - 360deg/s - XY errors +figure; +hold on; +plot(1e6*exp_tomo_ol_m1.y.x.Data, 1e6*exp_tomo_ol_m1.y.y.Data, 'DisplayName', 'OL') +plot(1e6*exp_tomo_cl_m1.y.x.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL') +hold off; +xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]'); +axis equal +xlim([-2, 2]); ylim([-2, 2]); +xticks([-2:1:2]); +yticks([-2:1:2]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +%% Simulation of tomography experiment - no payload, 30rpm - YZ errors +figure; +tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(1e6*exp_tomo_ol_m1.y.y.Data, 1e6*exp_tomo_ol_m1.y.z.Data, 'DisplayName', 'OL') +plot(1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL') +hold off; +xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]'); +axis equal +xlim([-2, 2]); ylim([-0.4, 0.4]); +xticks([-2:1:2]); +yticks([-2:0.2:2]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile(); +hold on; +plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL') +theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad] +plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size') +hold off; +xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]'); +axis equal +xlim([-500, 500]); ylim([-100, 100]); +xticks([-500:100:500]); +yticks([-100:50:100]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; + + + +% #+name: fig:nass_tomo_1kg_60rpm +% #+caption: Position error of the sample in the XY (\subref{fig:nass_tomo_1kg_60rpm_xy}) and YZ (\subref{fig:nass_tomo_1kg_60rpm_yz}) planes during a simulation of a tomography experiment at $360\,\text{deg/s}$. 1kg payload is placed on top of the nano-hexapod. +% #+attr_latex: :options [htbp] +% #+begin_figure +% #+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_xy}XY plane} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :scale 0.9 +% [[file:figs/nass_tomo_1kg_60rpm_xy.png]] +% #+end_subfigure +% #+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_yz}YZ plane} +% #+attr_latex: :options {0.48\textwidth} +% #+begin_subfigure +% #+attr_latex: :scale 0.9 +% [[file:figs/nass_tomo_1kg_60rpm_yz.png]] +% #+end_subfigure +% #+end_figure + +% - Effect of payload mass (Figure ref:fig:nass_tomography_hac_iff): +% Worse performance for high masses, as expected from the control analysis, but still acceptable considering that the rotational velocity of 360deg/s is only used for light payloads. + + +%% Simulation of tomography experiment - no payload, 30rpm - YZ errors +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 1$ kg') +theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad] +plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size') +hold off; +xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]'); +axis equal +xlim([-200, 200]); ylim([-100, 100]); +xticks([-200:50:200]); yticks([-100:50:100]); + +%% Simulation of tomography experiment - no payload, 30rpm - YZ errors +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(1e9*exp_tomo_cl_m25.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m25.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 25$ kg') +theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad] +plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size') +hold off; +xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]'); +axis equal +xlim([-200, 200]); ylim([-100, 100]); +xticks([-200:50:200]); yticks([-100:50:100]); + +%% Simulation of tomography experiment - no payload, 30rpm - YZ errors +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(1e9*exp_tomo_cl_m50.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m50.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 50$ kg') +theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad] +plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size') +hold off; +xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]'); +axis equal +xlim([-200, 200]); ylim([-100, 100]); +xticks([-200:50:200]); yticks([-100:50:100]); diff --git a/simscape-nass.org b/simscape-nass.org index ec43852..efd9456 100644 --- a/simscape-nass.org +++ b/simscape-nass.org @@ -248,19 +248,30 @@ One big advantage of doing the control in the cartesian plane, is that we don't Maybe this should be done *here*. Here it can be reminded when doing the control in the cartesian frame. -** TODO [#B] Determine which .mat files are used and which are not +** DONE [#B] Determine which .mat files are used and which are not +CLOSED: [2025-02-17 Mon 23:04] + +#+begin_src matlab :eval no :tangle no +load("nass_model_conf_log.mat"); +load("nass_model_conf_simscape.mat"); +dist = load("nass_model_disturbances.mat"); +load("nass_model_references.mat"); +load("nass_model_controller.mat"); +load("nass_model_stages.mat"); +J_L_to_X = inv(nano_hexapod.geometry.J); +#+end_src - [ ] matlab/mat/conf_log.mat - [ ] matlab/mat/conf_simscape.mat - [ ] matlab/mat/conf_simulink.mat - [ ] matlab/mat/nano_hexapod.mat - [ ] matlab/mat/nass_disturbances.mat -- [ ] matlab/mat/nass_model_conf_log.mat -- [ ] matlab/mat/nass_model_conf_simscape.mat -- [ ] matlab/mat/nass_model_controller.mat -- [ ] matlab/mat/nass_model_disturbances.mat -- [ ] matlab/mat/nass_model_references.mat -- [ ] matlab/mat/nass_model_stages.mat +- [X] matlab/mat/nass_model_conf_log.mat +- [X] matlab/mat/nass_model_conf_simscape.mat +- [X] matlab/mat/nass_model_controller.mat +- [X] matlab/mat/nass_model_disturbances.mat +- [X] matlab/mat/nass_model_references.mat +- [X] matlab/mat/nass_model_stages.mat - [ ] matlab/mat/nass_references.mat - [ ] matlab/mat/nass_stages.mat