584 lines
26 KiB
Matlab
584 lines
26 KiB
Matlab
% Matlab Init :noexport:ignore:
|
|
|
|
%% test_id31_4_hac.m
|
|
|
|
%% 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_id31';
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(log10(1), log10(2e3), 1000);
|
|
|
|
%% Sampling Time
|
|
Ts = 1e-4;
|
|
|
|
%% Specifications for Experiments
|
|
specs_dz_peak = 50; % [nm]
|
|
specs_dy_peak = 100; % [nm]
|
|
specs_ry_peak = 0.85; % [urad]
|
|
specs_dz_rms = 15; % [nm RMS]
|
|
specs_dy_rms = 30; % [nm RMS]
|
|
specs_ry_rms = 0.25; % [urad RMS]
|
|
|
|
% Damped Plant
|
|
% <<ssec:test_id31_iff_hac_plant>>
|
|
|
|
% To verify whether the multi-body model accurately represents the measured damped dynamics, both the direct terms and coupling terms corresponding to the first actuator are compared in Figure ref:fig:test_id31_comp_simscape_hac.
|
|
% Considering the complexity of the system's dynamics, the model can be considered to represent the system's dynamics with good accuracy, and can therefore be used to tune the feedback controller and evaluate its performance.
|
|
|
|
|
|
% Load the estimated damped plant from the multi-body model
|
|
load('test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
|
|
% Load the measured damped plants
|
|
load('test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');
|
|
% Load the undamped plant for comparison
|
|
load('test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', 'f');
|
|
|
|
figure;
|
|
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)));
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))));
|
|
text(12, 3e-5, '$\epsilon_{\mathcal{L}1}/u_1^\prime$', 'Horiz','left', 'Vert','top')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
|
|
yticks([1e-7, 1e-6, 1e-5]);
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_hac_m0_Wz0(:, 2, 1)));
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL2', 'u1'), freqs, 'Hz'))));
|
|
text(12, 3e-5, '$\epsilon_{\mathcal{L}2}/u_1^\prime$', 'Horiz','left', 'Vert','top')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
|
|
ax3 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_hac_m0_Wz0(:, 3, 1)))
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL3', 'u1'), freqs, 'Hz'))))
|
|
text(12, 3e-5, '$\epsilon_{\mathcal{L}3}/u_1^\prime$', 'Horiz','left', 'Vert','top')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
|
|
ax4 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_hac_m0_Wz0(:, 4, 1)));
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL4', 'u1'), freqs, 'Hz'))));
|
|
text(12, 3e-5, '$\epsilon_{\mathcal{L}4}/u_1^\prime$', 'Horiz','left', 'Vert','top')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
xticks([10, 20, 50, 100, 200])
|
|
yticks([1e-7, 1e-6, 1e-5]);
|
|
|
|
ax5 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_hac_m0_Wz0(:, 5, 1)));
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL5', 'u1'), freqs, 'Hz'))));
|
|
text(12, 3e-5, '$\epsilon_{\mathcal{L}5}/u_1^\prime$', 'Horiz','left', 'Vert','top')
|
|
hold off;
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([10, 20, 50, 100, 200])
|
|
|
|
ax6 = nexttile();
|
|
hold on;
|
|
plot(f, abs(G_hac_m0_Wz0(:, 6, 1)), ...
|
|
'DisplayName', 'Measurements');
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL6', 'u1'), freqs, 'Hz'))), ...
|
|
'DisplayName', 'Model (2-DoF APA)');
|
|
text(12, 3e-5, '$\epsilon_{\mathcal{L}6}/u_1^\prime$', 'Horiz','left', 'Vert','top')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
xticks([10, 20, 50, 100, 200])
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
|
|
xlim([10, 5e2]); ylim([1e-7, 4e-5]);
|
|
|
|
|
|
|
|
% #+name: fig:test_id31_comp_simscape_hac
|
|
% #+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal ($u_1^\prime$) of the damped plant to the estimated errors ($\epsilon_{\mathcal{L}_i}$) in the frame of the six struts by the external metrology
|
|
% #+RESULTS:
|
|
% [[file:figs/test_id31_comp_simscape_hac.png]]
|
|
|
|
% The challenge here is to tune a high authority controller such that it is robust to the change in dynamics due to different payloads being used.
|
|
% Without using the HAC-LAC strategy, it would be necessary to design a controller that provides good performance for all undamped dynamics (blue curves in Figure ref:fig:test_id31_comp_all_undamped_damped_plants), which is a very complex control problem.
|
|
% With the HAC-LAC strategy, the designed controller must be robust to all the damped dynamics (red curves in Figure ref:fig:test_id31_comp_all_undamped_damped_plants), which is easier from a control perspective.
|
|
% This is one of the key benefits of using the HAC-LAC strategy.
|
|
|
|
|
|
%% 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(f, abs(G_int_m0_Wz0(:,1,1)), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped - $\epsilon\mathcal{L}_i/u_i$');
|
|
plot(f, abs(G_hac_m0_Wz0(:,1,1)), 'color', [colors(2,:), 0.5], 'DisplayName', 'damped - $\epsilon\mathcal{L}_i/u_i^\prime$');
|
|
for i = 1:6
|
|
plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
end
|
|
for i = 1:6
|
|
plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
ylim([2e-7, 4e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*unwrapphase(angle(-G_int_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
|
|
plot(f, 180/pi*unwrapphase(angle(-G_int_m1_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
|
|
plot(f, 180/pi*unwrapphase(angle(-G_int_m2_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
|
|
plot(f, 180/pi*unwrapphase(angle(-G_int_m3_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
|
|
plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
|
|
plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
|
|
plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(2,:), 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([-270, 20])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 5e2]);
|
|
|
|
% Interaction Analysis
|
|
% <<sec:test_id31_hac_interaction_analysis>>
|
|
|
|
% The control strategy here is to apply a diagonal control in the frame of the struts; thus, it is important to determine the frequency at which the multivariable effects become significant, as this represents a critical limitation of the control approach.
|
|
% To conduct this interaction analysis, the acrfull:rga $\bm{\Lambda_G}$ is first computed using eqref:eq:test_id31_rga for the plant dynamics identified with the multiple payload masses.
|
|
|
|
% \begin{equation}\label{eq:test_id31_rga}
|
|
% \bm{\Lambda_G}(\omega) = \bm{G}(j\omega) \star \left(\bm{G}(j\omega)^{-1}\right)^{T}, \quad (\star \text{ means element wise multiplication})
|
|
% \end{equation}
|
|
|
|
% Then, acrshort:rga numbers are computed using eqref:eq:test_id31_rga_number and are use as a metric for interaction [[cite:&skogestad07_multiv_feedb_contr chapt. 3.4]].
|
|
|
|
% \begin{equation}\label{eq:test_id31_rga_number}
|
|
% \text{RGA number}(\omega) = \|\bm{\Lambda_G}(\omega) - \bm{I}\|_{\text{sum}}
|
|
% \end{equation}
|
|
|
|
% The obtained acrshort:rga numbers are compared in Figure ref:fig:test_id31_hac_rga_number.
|
|
% The results indicate that higher payload masses increase the coupling when implementing control in the strut reference frame (i.e., decentralized approach).
|
|
% This indicates that achieving high bandwidth feedback control is increasingly challenging as the payload mass increases.
|
|
% This behavior can be attributed to the fundamental approach of implementing control in the frame of the struts.
|
|
% Above the suspension modes of the nano-hexapod, the motion induced by the piezoelectric actuators is no longer dictated by kinematics but rather by the inertia of the different parts.
|
|
% This design choice, while beneficial for system simplicity, introduces inherent limitations in the system's ability to handle larger masses at high frequency.
|
|
|
|
|
|
%% Interaction Analysis - RGA Number
|
|
rga_m0 = zeros(1,size(G_hac_m0_Wz0,1));
|
|
for i = 1:length(rga_m0)
|
|
rga_m0(i) = sum(sum(abs(inv(squeeze(G_hac_m0_Wz0(i,:,:)).').*squeeze(G_hac_m0_Wz0(i,:,:)) - eye(6))));
|
|
end
|
|
|
|
rga_m1 = zeros(1,size(G_hac_m1_Wz0,1));
|
|
for i = 1:length(rga_m1)
|
|
rga_m1(i) = sum(sum(abs(inv(squeeze(G_hac_m1_Wz0(i,:,:)).').*squeeze(G_hac_m1_Wz0(i,:,:)) - eye(6))));
|
|
end
|
|
|
|
rga_m2 = zeros(1,size(G_hac_m2_Wz0,1));
|
|
for i = 1:length(rga_m2)
|
|
rga_m2(i) = sum(sum(abs(inv(squeeze(G_hac_m2_Wz0(i,:,:)).').*squeeze(G_hac_m2_Wz0(i,:,:)) - eye(6))));
|
|
end
|
|
|
|
rga_m3 = zeros(1,size(G_hac_m3_Wz0,1));
|
|
for i = 1:length(rga_m3)
|
|
rga_m3(i) = sum(sum(abs(inv(squeeze(G_hac_m3_Wz0(i,:,:)).').*squeeze(G_hac_m3_Wz0(i,:,:)) - eye(6))));
|
|
end
|
|
|
|
%% RGA-number for the damped plants - Comparison of all the payload conditions
|
|
figure;
|
|
hold on;
|
|
plot(f, rga_m0, 'DisplayName', '$m = 0$ kg')
|
|
plot(f, rga_m1, 'DisplayName', '$m = 13$ kg')
|
|
plot(f, rga_m2, 'DisplayName', '$m = 26$ kg')
|
|
plot(f, rga_m3, 'DisplayName', '$m = 39$ kg')
|
|
xlabel('Frequency [Hz]'); ylabel('RGA number');
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlim([1, 1e2]); ylim([0, 10]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
% Robust Controller Design
|
|
% <<ssec:test_id31_iff_hac_controller>>
|
|
|
|
% A diagonal controller was designed to be robust against changes in payload mass, which means that every damped plant shown in Figure ref:fig:test_id31_comp_all_undamped_damped_plants must be considered during the controller design.
|
|
% For this controller design, a crossover frequency of $5\,\text{Hz}$ was chosen to limit the multivariable effects, as explain in Section ref:sec:test_id31_hac_interaction_analysis.
|
|
% One integrator is added to increase the low-frequency gain, a lead is added around $5\,\text{Hz}$ to increase the stability margins and a first-order low-pass filter with a cut-off frequency of $30\,\text{Hz}$ is added to improve the robustness to dynamical uncertainty at high frequency.
|
|
% The controller transfer function is shown in eqref:eq:test_id31_robust_hac.
|
|
|
|
% \begin{equation}\label{eq:test_id31_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\pi5\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi30\,\text{rad/s} \right)
|
|
% \end{equation}
|
|
|
|
% The obtained "decentralized" loop-gains (i.e. the diagonal element of the controller times the diagonal terms of the plant) are shown in Figure ref:fig:test_id31_hac_loop_gain.
|
|
% The closed-loop stability was verified by computing the characteristic Loci (Figure ref:fig:test_id31_hac_characteristic_loci).
|
|
% However, small stability margins were observed for the highest mass, indicating that some multivariable effects are in play.
|
|
|
|
|
|
%% HAC Design
|
|
% Wanted crossover
|
|
wc = 2*pi*5; % [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/30);
|
|
|
|
% Gain to have unitary crossover at 5Hz
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
|
H_gain = 1./abs(G_hac_m0_Wz0(i_f, 1, 1));
|
|
|
|
% Decentralized HAC
|
|
Khac = H_gain * ... % Gain
|
|
H_int * ... % Integrator
|
|
H_lpf * ... % Low Pass filter
|
|
eye(6); % 6x6 Diagonal
|
|
|
|
% The designed HAC controller is saved
|
|
save('./mat/test_id31_K_hac_robust.mat', 'Khac');
|
|
|
|
%% Decentralized Loop gain for the High Authority Controller
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f(2:end), abs(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:), 'DisplayName', '$0$ kg');
|
|
plot(f(2:end), abs(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:), 'DisplayName', '$13$ kg');
|
|
plot(f(2:end), abs(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:), 'DisplayName', '$26$ kg');
|
|
plot(f(2:end), abs(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:), 'DisplayName', '$39$ kg');
|
|
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-5, 1e2]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f(2:end), 180/pi*angle(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(1,:));
|
|
plot(f(2:end), 180/pi*angle(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(2,:));
|
|
plot(f(2:end), 180/pi*angle(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(3,:));
|
|
plot(f(2:end), 180/pi*angle(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f(2:end), 'Hz'))), 'color', colors(4,:));
|
|
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
|
|
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([1, 1e3]);
|
|
|
|
%% Compute the Eigenvalues of the loop gain
|
|
Ldet = zeros(4, 6, length(f));
|
|
|
|
% Loop gain
|
|
Lmimo = pagemtimes(permute(G_hac_m0_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
|
for i_f = 2:length(f)
|
|
Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
|
|
Lmimo = pagemtimes(permute(G_hac_m1_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
|
for i_f = 2:length(f)
|
|
Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
|
|
Lmimo = pagemtimes(permute(G_hac_m2_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
|
for i_f = 2:length(f)
|
|
Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
|
|
Lmimo = pagemtimes(permute(G_hac_m3_Wz0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
|
for i_f = 2:length(f)
|
|
Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
|
|
%% Plot of the eigenvalues of L in the complex plane
|
|
figure;
|
|
hold on;
|
|
plot(real(squeeze(Ldet(1, 1,:))), imag(squeeze(Ldet(1, 1,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'DisplayName', '$m = 0$ kg');
|
|
plot(real(squeeze(Ldet(2, 1,:))), imag(squeeze(Ldet(2, 1,:))), ...
|
|
'.', 'color', colors(2, :), ...
|
|
'DisplayName', '$m = 13$ kg');
|
|
plot(real(squeeze(Ldet(3, 1,:))), imag(squeeze(Ldet(3, 1,:))), ...
|
|
'.', 'color', colors(3, :), ...
|
|
'DisplayName', '$m = 26$ kg');
|
|
plot(real(squeeze(Ldet(4, 1,:))), imag(squeeze(Ldet(4, 1,:))), ...
|
|
'.', 'color', colors(4, :), ...
|
|
'DisplayName', '$m = 39$ kg');
|
|
for i_mass = 1:4
|
|
plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
|
|
'.', 'color', colors(i_mass, :), ...
|
|
'HandleVisibility', 'off');
|
|
for i = 1:6
|
|
plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
|
|
'.', 'color', colors(i_mass, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
|
|
'.', 'color', colors(i_mass, :), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
axis square
|
|
xlim([-1.5, 0.5]); ylim([-1, 1]);
|
|
|
|
% Performance estimation with simulation of Tomography scans
|
|
% <<ssec:test_id31_iff_hac_perf>>
|
|
|
|
% To estimate the performances that can be expected with this HAC-LAC architecture and the designed controller, simulations of tomography experiments were performed[fn:test_id31_4].
|
|
% The rotational velocity was set to $180\,\text{deg/s}$, and no payload was added on top of the nano-hexapod.
|
|
% An open-loop simulation and a closed-loop simulation were performed and compared in Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim.
|
|
% The obtained closed-loop positioning accuracy was found to comply with the requirements as it succeeded to keep the point of interest on the beam (Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim_yz).
|
|
|
|
|
|
%% Tomography experiment
|
|
% Sample is not centered with the rotation axis
|
|
% This is done by offsetfing the micro-hexapod by 0.9um
|
|
P_micro_hexapod = [2.5e-6; 0; -0.3e-6]; % [m]
|
|
|
|
open(mdl);
|
|
set_param(mdl, 'StopTime', '3'); % 6 turns at 180deg/s (30rpm)
|
|
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod('AP', P_micro_hexapod);
|
|
initializeNanoHexapod('flex_bot_type', '2dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof');
|
|
initializeSample('type', '0');
|
|
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-4);
|
|
initializeController('type', 'open-loop');
|
|
|
|
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', 360/180, ... % 180deg/s, 30rpm
|
|
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
|
|
|
|
% Open-Loop Simulation
|
|
sim(mdl);
|
|
exp_tomo_ol_m0_Wz180 = simout;
|
|
|
|
% Closed-Loop Simulation
|
|
load('test_id31_K_iff.mat', 'Kiff');
|
|
load('test_id31_K_hac_robust.mat', 'Khac');
|
|
initializeController('type', 'hac-iff');
|
|
initializeSample('type', '0');
|
|
sim(mdl);
|
|
exp_tomo_cl_m0_Wz180 = simout;
|
|
|
|
% Save the simulation results
|
|
save('./mat/test_id31_exp_tomo_ol_cl_30rpm_sim.mat', 'exp_tomo_ol_m0_Wz180', 'exp_tomo_cl_m0_Wz180');
|
|
|
|
%% Simulation of tomography experiment - no payload, 30rpm - XY errors
|
|
figure;
|
|
hold on;
|
|
plot(1e6*exp_tomo_ol_m0_Wz180.y.x.Data, 1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 'DisplayName', 'OL')
|
|
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
|
|
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
|
|
hold off;
|
|
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-3, 3]); ylim([-3, 3]);
|
|
xticks([-3:1:3]);
|
|
yticks([-3:1:3]);
|
|
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_m0_Wz180.y.y.Data, 1e6*exp_tomo_ol_m0_Wz180.y.z.Data, 'DisplayName', 'OL')
|
|
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
|
|
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
|
|
hold off;
|
|
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-3, 3]); ylim([-0.6, 0.6]);
|
|
xticks([-3:1:3]);
|
|
yticks([-3:0.3:3]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e9*exp_tomo_cl_m0_Wz180.y.z.Data(2e3: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([-300, 300]); ylim([-100, 100]);
|
|
% xticks([-3:1:3]);
|
|
% yticks([-3:1:3]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
% Robustness estimation with simulation of Tomography scans
|
|
% <<ssec:test_id31_iff_hac_robustness>>
|
|
|
|
% To verify the robustness against payload mass variations, four simulations of tomography experiments were performed with payloads as shown Figure ref:fig:test_id31_picture_masses (i.e. $0\,kg$, $13\,kg$, $26\,kg$ and $39\,kg$).
|
|
% The rotational velocity was set at $6\,\text{deg/s}$, which is the typical rotational velocity for heavy samples.
|
|
|
|
% The closed-loop systems were stable under all payload conditions, indicating good control robustness.
|
|
% However, the positioning errors worsen as the payload mass increases, especially in the lateral $D_y$ direction, as shown in Figure ref:fig:test_id31_hac_tomography_Wz36_simulation.
|
|
% However, it was decided that this controller should be tested experimentally and improved only if necessary.
|
|
|
|
|
|
%% Simulation of tomography experiments at 1RPM with all payloads
|
|
% Configuration
|
|
open(mdl);
|
|
set_param(mdl, 'StopTime', '2'); % 30 degrees at 1rpm
|
|
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);
|
|
initializeController('type', 'hac-iff');
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 360/6, ... % 6deg/s, 1 rpm
|
|
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
|
|
|
|
% Perform the simulations
|
|
initializeSample('type', '0');
|
|
sim(mdl);
|
|
exp_tomo_cl_m0_1rpm = simout;
|
|
initializeSample('type', '1');
|
|
sim(mdl);
|
|
exp_tomo_cl_m1_1rpm = simout;
|
|
initializeSample('type', '2');
|
|
sim(mdl);
|
|
exp_tomo_cl_m2_1rpm = simout;
|
|
initializeSample('type', '3');
|
|
sim(mdl);
|
|
exp_tomo_cl_m3_1rpm = simout;
|
|
|
|
% Save the simulation results
|
|
save('./mat/test_id31_exp_tomo_cl_1rpm_sim.mat', 'exp_tomo_cl_m0_1rpm', 'exp_tomo_cl_m1_1rpm', 'exp_tomo_cl_m2_1rpm', 'exp_tomo_cl_m3_1rpm');
|
|
|
|
%% Positioning errors in the Y-Z plane during tomography experiments simulated using the multi-body model
|
|
figure;
|
|
tiledlayout(2, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile;
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m0_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m0_1rpm.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 0$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
|
|
axis equal
|
|
xticks([-400:100:400]); yticks([-100:100:100]);
|
|
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m1_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1_1rpm.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 13$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
|
|
axis equal
|
|
xticks([-400:100:400]); yticks([-100:100:100]);
|
|
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax3 = nexttile;
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m2_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m2_1rpm.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 26$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
|
|
axis equal
|
|
xticks([-400:100:400]); yticks([-100:100:100]);
|
|
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax4 = nexttile;
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m3_1rpm.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m3_1rpm.y.z.Data(1e3:end), 'color', colors(4,:), 'DisplayName', '$m = 39$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'HandleVisibility', 'off')
|
|
axis equal
|
|
xticks([-400:100:400]); yticks([-100:100:100]);
|
|
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
linkaxes([ax1,ax2,ax3, ax4],'xy');
|
|
xlim([-450, 450]); ylim([-100, 100]);
|