diff --git a/figs/nass_tomography_hac_iff_m1.pdf b/figs/nass_tomography_hac_iff_m1.pdf new file mode 100644 index 0000000..e1e7336 Binary files /dev/null and b/figs/nass_tomography_hac_iff_m1.pdf differ diff --git a/figs/nass_tomography_hac_iff_m1.png b/figs/nass_tomography_hac_iff_m1.png new file mode 100644 index 0000000..3723add Binary files /dev/null and b/figs/nass_tomography_hac_iff_m1.png differ diff --git a/figs/nass_tomography_hac_iff_m25.pdf b/figs/nass_tomography_hac_iff_m25.pdf new file mode 100644 index 0000000..0ea6d74 Binary files /dev/null and b/figs/nass_tomography_hac_iff_m25.pdf differ diff --git a/figs/nass_tomography_hac_iff_m25.png b/figs/nass_tomography_hac_iff_m25.png new file mode 100644 index 0000000..5dfef20 Binary files /dev/null and b/figs/nass_tomography_hac_iff_m25.png differ diff --git a/figs/nass_tomography_hac_iff_m50.pdf b/figs/nass_tomography_hac_iff_m50.pdf new file mode 100644 index 0000000..0b3c20d Binary files /dev/null and b/figs/nass_tomography_hac_iff_m50.pdf differ diff --git a/figs/nass_tomography_hac_iff_m50.png b/figs/nass_tomography_hac_iff_m50.png new file mode 100644 index 0000000..c1892ce Binary files /dev/null and b/figs/nass_tomography_hac_iff_m50.png differ diff --git a/matlab/mat/ustation_disturbance_psd.mat b/matlab/mat/ustation_disturbance_psd.mat new file mode 100644 index 0000000..35aa61c Binary files /dev/null and b/matlab/mat/ustation_disturbance_psd.mat differ diff --git a/matlab/nass_model.slx b/matlab/nass_model.slx index 149fbe6..27a30cc 100644 Binary files a/matlab/nass_model.slx and b/matlab/nass_model.slx differ diff --git a/matlab/src/initializeSimplifiedNanoHexapod.m b/matlab/src/initializeSimplifiedNanoHexapod.m index 5687f47..80d6d29 100644 --- a/matlab/src/initializeSimplifiedNanoHexapod.m +++ b/matlab/src/initializeSimplifiedNanoHexapod.m @@ -15,7 +15,7 @@ function [nano_hexapod] = initializeSimplifiedNanoHexapod(args) %% Actuators args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof' args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6 - args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e4 + args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e4 args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605 args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302 args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50 diff --git a/simscape-nass.org b/simscape-nass.org index 87e8ee6..f1a4be4 100644 --- a/simscape-nass.org +++ b/simscape-nass.org @@ -434,6 +434,7 @@ Explain how to compute the errors in the frame of the struts (rotating): - Robustness to payload mass - Root Locus - Damping optimization +- *Parallel stiffness?* Explain which samples are tested: - 1kg, 25kg, 50kg @@ -475,7 +476,7 @@ Explain which samples are tested: - [ ] Added parallel stiffness #+begin_src matlab -%% Identify the plant dynamics using the Simscape model +%% Identify the IFF plant dynamics using the Simscape model % Initialize each Simscape model elements initializeGround(); @@ -485,8 +486,9 @@ initializeRy(); initializeRz(); initializeMicroHexapod(); initializeSimplifiedNanoHexapod(); -initializeSample('type', 'cylindrical'); +initializeSample('type', 'cylindrical', 'm', 1); +% Initial Simscape Configuration initializeSimscapeConfiguration('gravity', false); initializeDisturbances('enable', false); initializeLoggingConfiguration('log', 'none'); @@ -494,27 +496,386 @@ 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 [V] -% io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vs'); io_i = io_i + 1; % Force Sensors voltages [V] -% io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors [m] +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] +#+end_src -% % With no payload -% Gm = exp(-1e-4*s)*linearize(mdl, io); -% Gm.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -% Gm.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ... -% 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; +#+begin_src matlab +%% Identify for multi payload masses (no rotation) +initializeReferences(); % No Spindle Rotation +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'}; + +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'}; + +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'}; +#+end_src + +#+begin_src matlab +%% Effect of Rotation +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 1); % 360 deg/s +initializeSample('type', 'cylindrical', 'm', 1); +G_iff_m1_Rz = linearize(mdl, io, 0.1); +G_iff_m1_Rz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_iff_m1_Rz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; +#+end_src + +Coupling + +#+begin_src matlab :exports none :results none +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_m1(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_iff_m1(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_iff_m1(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', 'northwest', '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,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +Effect of rotation + +#+begin_src matlab :exports none :results none +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_Rz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_iff_Rz(1,1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$f_{ni}/f_i$') +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(G_iff_Rz(i,i), freqs, 'Hz'))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(G_iff_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', 'northwest', '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_Rz(i,i), freqs, 'Hz'))), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +Effect of payload mass + +#+begin_src matlab :exports none :results none +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', 'northwest', '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([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); #+end_src ** Controller Design +Low pass filter needs to be added (because now: DC gain) + +\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} +\end{equation} + +#+begin_src matlab +%% IFF Controller Design +% Second order high pass filter +wz = 2*pi*3; +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'}; +#+end_src + +#+begin_src matlab :exports none :tangle no +% The designed IFF controller is saved +save('./matlab/mat/nass_K_iff.mat', 'Kiff'); +#+end_src + +#+begin_src matlab :eval no +% The designed IFF controller is saved +save('./mat/nass_K_iff.mat', 'Kiff'); +#+end_src + +Loop Gain: +#+begin_src matlab :exports none :results none +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', '$f_{ni}/f_i$ - 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', '$f_{ni}/f_i$ - 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', '$f_{ni}/f_i$ - 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', 'northwest', '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([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +Root Locus => Stability + +#+begin_src matlab :exports none :results none +%% Root Locus plot of the Decentralized IFF Control +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}$'); + +hold off; +axis equal; +xlim([-900, 50]); ylim([-50, 900]); +xticks([-900:100:0]); +yticks([0:100:900]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Root Locus plot of the Decentralized IFF Control +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}$'); + +hold off; +axis equal; +xlim([-900, 50]); ylim([-50, 900]); +xticks([-900:100:0]); +yticks([0:100:900]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Root Locus plot of the Decentralized IFF Control +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}$'); + +hold off; +axis equal; +xlim([-900, 50]); ylim([-50, 900]); +xticks([-900:100:0]); +yticks([0:100:900]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlabel('Real part'); ylabel('Imaginary part'); +#+end_src + - Use Integral controller (with parallel stiffness) - Show Root Locus (show that without parallel stiffness => unstable?) - Choose optimal gain. Here in MIMO, cannot have optimal damping for all modes. (there is a paper that tries to optimize that) -- Show robustness to change of payload (loci?) / Change of rotating velocity ? +- [ ] Show robustness to change of payload (loci?) / Change of rotating velocity ? - Reference to paper showing stability in MIMO for decentralized IFF + ** Sensitivity to disturbances Disturbances: @@ -576,11 +937,291 @@ From control kinematics: - [ ] Show effect of payload mass - [ ] Compare with undamped plants +#+begin_src matlab +%% 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] +#+end_src + +#+begin_src matlab +%% 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'}; +#+end_src + +#+begin_src matlab +%% 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'}; +#+end_src + +Effect of rotation: + +#+begin_src matlab +bodeFig({G_m1(1,1), G_m1_Rz(1,1)}, struct('phase', true)); +#+end_src + +Effect of IFF: + +#+begin_src matlab +%% 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 + +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'}; + +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'}; + +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'}; +#+end_src + +#+begin_src matlab +%% Check stability +isstable(G_hac_m1) && isstable(G_hac_m25) && isstable(G_hac_m50) +#+end_src + +#+begin_src matlab +bodeFig({G_m1(1,1), G_hac_m1(1,1)}, struct('phase', true)); +#+end_src + +Effect of payload mass + +#+begin_src matlab +bodeFig({G_hac_m1(1,1), G_hac_m25(1,1), G_hac_m50(1,1)}, freqs, struct('phase', true)); +#+end_src + +Advantage of using IFF: +#+begin_src matlab :exports none :results none +%% 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; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +% xlim([1, 5e2]); +#+end_src + ** Controller design - [ ] Show design HAC with formulas and parameters - [ ] Show robustness with Loci for all masses +\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\pi5\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi30\,\text{rad/s} \right) +\end{equation} + +#+begin_src matlab +%% HAC Design +% Wanted crossover +wc = 2*pi*10; % [rad/s] + +% Integrator +H_int = wc/s; + +% Lead to increase phase margin +a = 4; % 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/100); + +% Gain to have unitary crossover at 5Hz +H_gain = 1./abs(evalfr(G_hac_m25(1,1), 1j*wc)); + +% Decentralized HAC +Khac = -H_gain * ... % Gain + H_int * ... % Integrator + H_lead * ... % Lead + H_lpf * ... % Low Pass filter + eye(6); % 6x6 Diagonal +#+end_src + +#+begin_src matlab :exports none :tangle no +% The designed HAC controller is saved +save('./matlab/mat/nass_K_hac.mat', 'Khac'); +#+end_src + +#+begin_src matlab :eval no +% The designed HAC controller is saved +save('./mat/nass_K_hac.mat', 'Khac'); +#+end_src + +"Decentralized" Loop Gain: +#+begin_src matlab :exports none :results none +%% 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; +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Khac(i,i)*G_hac_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-2, 1e2]); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); + plot(freqs, 180/pi*angle(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); + plot(freqs, 180/pi*angle(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), freqs, '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([1, 5e2]); +#+end_src + +Characteristic Loci for three masses: +#+begin_src matlab :exports none +%% Plot of the eigenvalues of L in the complex plane +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; +for i = 1: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'); +end +for i = 1:6 + 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'); +end +for i = 1:6 + 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]); +#+end_src + ** Sensitivity to disturbances - Compute transfer functions from spindle vertical error to sample vertical error with HAC-IFF @@ -595,6 +1236,213 @@ From control kinematics: - Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]]) - Validation of concept +#+begin_src matlab +% 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 +initializeSimplifiedNanoHexapod('type', 'none'); +initializeController('type', 'open-loop'); +sim(mdl); +exp_tomo_ol_m1 = simout; +#+end_src + +#+begin_src matlab +% Closed-Loop Simulation with NASS +initializeSimplifiedNanoHexapod(); +initializeController('type', 'hac-iff'); +load('nass_K_iff.mat', 'Kiff'); +load('nass_K_hac.mat', 'Khac'); +sim(mdl); +exp_tomo_cl_m1 = simout; +#+end_src + +#+begin_src matlab :exports none :results none +%% Simulation of tomography experiment - no payload, 30rpm - 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([-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; +#+end_src + +#+begin_src matlab :exports none :results none +%% Simulation of tomography experiment - no payload, 30rpm - YZ errors +figure; +tiledlayout(1, 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') +theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad] +plot(0.1*cos(theta), 0.05*sin(theta), 'k--', 'DisplayName', 'Beam size') +hold off; +xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]'); +axis equal +xlim([-1.1, 1.1]); ylim([-0.3, 0.3]); +xticks([-1:0.1:1]); +yticks([-0.3:0.1:0.3]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +#+end_src + +#+begin_src matlab +% Closed-Loop Simulation with NASS - Other masses +initializeSample('type', 'cylindrical', 'm', 25); +sim(mdl); +exp_tomo_cl_m25 = simout; +initializeSample('type', 'cylindrical', 'm', 50); +sim(mdl); +exp_tomo_cl_m50 = simout; +#+end_src + +#+begin_src matlab :exports none :results none +%% Simulation of tomography experiment - no payload, 30rpm - YZ errors +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(1e6*exp_tomo_cl_m50.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m50.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 50$ kg') +plot(1e6*exp_tomo_cl_m25.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m25.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 25$ kg') +plot(1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e6*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(0.1*cos(theta), 0.05*sin(theta), 'k--', 'DisplayName', 'Beam size') +hold off; +xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]'); +axis equal +xlim([-1.1, 1.1]); ylim([-0.3, 0.3]); +xticks([-1:0.1:1]); +yticks([-0.3:0.1:0.3]); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +#+end_src + +#+begin_src matlab :exports none :results none +%% 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]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/nass_tomography_hac_iff_m1.pdf', 'width', 'third', 'height', 'normal'); +#+end_src + +#+begin_src matlab :exports none :results none +%% 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:100:200]); yticks([-100:50:100]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/nass_tomography_hac_iff_m25.pdf', 'width', 'third', 'height', 'normal'); +#+end_src + +#+begin_src matlab :exports none :results none +%% 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]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/nass_tomography_hac_iff_m50.pdf', 'width', 'third', 'height', 'normal'); +#+end_src + +#+name: fig:nass_tomography_hac_iff +#+caption: Simulation of tomography experiments +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m1} $m = 1\,kg$} +#+attr_latex: :options {0.33\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/nass_tomography_hac_iff_m1.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m25} $m = 25\,kg$} +#+attr_latex: :options {0.33\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/nass_tomography_hac_iff_m25.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m50} $m = 50\,kg$} +#+attr_latex: :options {0.33\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/nass_tomography_hac_iff_m50.png]] +#+end_subfigure +#+end_figure + * Conclusion <> @@ -602,6 +1450,50 @@ From control kinematics: * Bibliography :ignore: #+latex: \printbibliography[heading=bibintoc,title={Bibliography}] +* Helping Functions :noexport: +** Initialize Path +#+NAME: m-init-path +#+BEGIN_SRC matlab +addpath('./matlab/'); % Path for scripts + +%% Path for functions, data and scripts +addpath('./matlab/mat/'); % Path for Computed FRF +addpath('./matlab/src/'); % Path for functions +addpath('./matlab/STEPS/'); % Path for STEPS +addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files + +%% Data directory +data_dir = './matlab/mat/' +#+END_SRC + +#+NAME: m-init-path-tangle +#+BEGIN_SRC matlab +%% 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/'; +#+END_SRC + +** Initialize Simscape Model +#+NAME: m-init-simscape +#+begin_src matlab +% Simulink Model name +mdl = 'nass_model'; +#+end_src + +** Initialize other elements +#+NAME: m-init-other +#+BEGIN_SRC matlab +%% Colors for the figures +colors = colororder; + +%% Frequency Vector [Hz] +freqs = logspace(0, 3, 1000); +#+END_SRC * Matlab Functions :noexport: ** =initializeSimscapeConfiguration=: Simscape Configuration :PROPERTIES: @@ -1935,7 +2827,7 @@ function [nano_hexapod] = initializeSimplifiedNanoHexapod(args) %% Actuators args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof' args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6 - args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e4 + args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e4 args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605 args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302 args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50 @@ -3245,44 +4137,3 @@ if isfield(stewart.kinematics, 'C') end #+end_src -* Helping Functions :noexport: -** Initialize Path -#+NAME: m-init-path -#+BEGIN_SRC matlab -addpath('./matlab/'); % Path for scripts - -%% Path for functions, data and scripts -addpath('./matlab/mat/'); % Path for Computed FRF -addpath('./matlab/src/'); % Path for functions -addpath('./matlab/STEPS/'); % Path for STEPS -addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files - -%% Data directory -data_dir = './matlab/mat/' -#+END_SRC - -#+NAME: m-init-path-tangle -#+BEGIN_SRC matlab -%% 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/'; -#+END_SRC - -** Initialize Simscape Model -#+NAME: m-init-simscape -#+begin_src matlab -% Simulink Model name -mdl = 'nass_model'; -#+end_src - -** Initialize other elements -#+NAME: m-init-other -#+BEGIN_SRC matlab -%% Colors for the figures -colors = colororder; -#+END_SRC diff --git a/simscape-nass.pdf b/simscape-nass.pdf index 7e76748..8552d80 100644 Binary files a/simscape-nass.pdf and b/simscape-nass.pdf differ diff --git a/simscape-nass.tex b/simscape-nass.tex index fda9e8f..661b780 100644 --- a/simscape-nass.tex +++ b/simscape-nass.tex @@ -1,4 +1,4 @@ -% Created 2025-02-12 Wed 15:35 +% Created 2025-02-12 Wed 17:40 % Intended LaTeX compiler: pdflatex \documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt} @@ -97,13 +97,12 @@ Explain how to compute the errors in the frame of the struts (rotating): \item Say that there are many control strategies. It will be the topic of chapter 2.3. Here, we start with something simple: control in the frame of the struts -\item[{$\square$}] block diagram of the complete control architecture \end{itemize} \begin{figure}[htbp] \centering \includegraphics[scale=1,width=\linewidth]{figs/nass_control_architecture.png} -\caption{\label{fig:nass_control_architecture}Figure caption} +\caption{\label{fig:nass_control_architecture}The physical systems are shown in blue, the control kinematics in red, the decentralized Integral Force Feedback in yellow and the centralized High Authority Controller in green.} \end{figure} \chapter{Decentralized Active Damping} @@ -113,6 +112,7 @@ Here, we start with something simple: control in the frame of the struts \item Robustness to payload mass \item Root Locus \item Damping optimization +\item \textbf{Parallel stiffness?} \end{itemize} Explain which samples are tested: @@ -132,17 +132,37 @@ Explain which samples are tested: \item[{$\square$}] Added parallel stiffness \end{itemize} +Coupling + +Effect of rotation + +Effect of payload mass + \section{Controller Design} +Low pass filter needs to be added (because now: DC gain) + +\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} +\end{equation} + +Loop Gain: +Root Locus => Stability + \begin{itemize} \item Use Integral controller (with parallel stiffness) \item Show Root Locus (show that without parallel stiffness => unstable?) \item Choose optimal gain. Here in MIMO, cannot have optimal damping for all modes. (there is a paper that tries to optimize that) -\item Show robustness to change of payload (loci?) / Change of rotating velocity ? +\item[{$\square$}] Show robustness to change of payload (loci?) / Change of rotating velocity ? \item Reference to paper showing stability in MIMO for decentralized IFF \end{itemize} + \section{Sensitivity to disturbances} Disturbances: @@ -181,6 +201,13 @@ From control kinematics: \item[{$\square$}] Compare with undamped plants \end{itemize} +Effect of rotation: + +Effect of IFF: + +Effect of payload mass + +Advantage of using IFF: \section{Controller design} \begin{itemize} @@ -188,6 +215,12 @@ From control kinematics: \item[{$\square$}] Show robustness with Loci for all masses \end{itemize} +\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\pi5\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi30\,\text{rad/s} \right) +\end{equation} + +``Decentralized'' Loop Gain: +Characteristic Loci for three masses: \section{Sensitivity to disturbances} \begin{itemize} @@ -206,6 +239,28 @@ Compare without the NASS, and with just IFF \item Validation of concept \end{itemize} +\begin{figure}[htbp] +\begin{subfigure}{0.33\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/nass_tomography_hac_iff_m1.png} +\end{center} +\subcaption{\label{fig:nass_tomography_hac_iff_m1} $m = 1\,kg$} +\end{subfigure} +\begin{subfigure}{0.33\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/nass_tomography_hac_iff_m25.png} +\end{center} +\subcaption{\label{fig:nass_tomography_hac_iff_m25} $m = 25\,kg$} +\end{subfigure} +\begin{subfigure}{0.33\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/nass_tomography_hac_iff_m50.png} +\end{center} +\subcaption{\label{fig:nass_tomography_hac_iff_m50} $m = 50\,kg$} +\end{subfigure} +\caption{\label{fig:nass_tomography_hac_iff}Simulation of tomography experiments} +\end{figure} + \chapter{Conclusion} \label{sec:nass_conclusion}