2020-05-20 15:49:43 +02:00
#+TITLE : Amplified Piezoelectric Stack Actuator
#+SETUPFILE : ./setup/org-setup-file.org
* Introduction :ignore:
The presented model is based on cite:souleille18_concep_activ_mount_space_applic.
The model represents the amplified piezo APA100M from Cedrat-Technologies (Figure [[fig:souleille18_model_piezo ]]).
The parameters are shown in the table below.
#+name : fig:souleille18_model_piezo
#+caption : Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator
[[file:./figs/souleille18_model_piezo.png ]]
#+caption : Parameters used for the model of the APA 100M
| | Value | Meaning |
|-------+-------------------+----------------------------------------------------------------|
| $m$ | $1\,[kg]$ | Payload mass |
| $k_e$ | $4.8\,[N/\mu m]$ | Stiffness used to adjust the pole of the isolator |
| $k_1$ | $0.96\,[N/\mu m]$ | Stiffness of the metallic suspension when the stack is removed |
| $k_a$ | $65\,[N/\mu m]$ | Stiffness of the actuator |
| $c_1$ | $10\,[N/(m/s)]$ | Added viscous damping |
* Simplified Model
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+BEGIN_SRC matlab
simulinkproject('../');
#+END_SRC
#+begin_src matlab
open 'amplified_piezo_model.slx'
#+end_src
** Parameters
#+begin_src matlab
m = 1; % [kg]
ke = 4.8e6; % [N/m]
ce = 5; % [N/(m/s)]
me = 0.001; % [kg]
k1 = 0.96e6; % [N/m]
c1 = 10; % [N/(m/s)]
ka = 65e6; % [N/m]
ca = 5; % [N/(m/s)]
ma = 0.001; % [kg]
h = 0.2; % [m]
#+end_src
IFF Controller:
#+begin_src matlab
Kiff = -8000/s;
#+end_src
** Identification
Identification in open-loop.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'amplified_piezo_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion
io(io_i) = linio([mdl, '/f'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % External Force
io(io_i) = linio([mdl, '/Fs'], 3, 'openoutput'); io_i = io_i + 1; % Force Sensors
io(io_i) = linio([mdl, '/x1'], 1, 'openoutput'); io_i = io_i + 1; % Mass displacement
G = linearize(mdl, io, 0);
G.InputName = {'w', 'f', 'F'};
G.OutputName = {'Fs', 'x1'};
#+end_src
Identification in closed-loop.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'amplified_piezo_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/w'], 1, 'input'); io_i = io_i + 1; % Base Motion
io(io_i) = linio([mdl, '/f'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/F'], 1, 'input'); io_i = io_i + 1; % External Force
io(io_i) = linio([mdl, '/Fs'], 3, 'output'); io_i = io_i + 1; % Force Sensors
io(io_i) = linio([mdl, '/x1'], 1, 'output'); io_i = io_i + 1; % Mass displacement
Giff = linearize(mdl, io, 0);
Giff.InputName = {'w', 'f', 'F'};
Giff.OutputName = {'Fs', 'x1'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 3, 1);
title('$\displaystyle \frac{x_1}{w}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('x1', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Giff('x1', 'w'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');
ax2 = subplot(2, 3, 2);
title('$\displaystyle \frac{x_1}{f}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('x1', 'f'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Giff('x1', 'f'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax3 = subplot(2, 3, 3);
title('$\displaystyle \frac{x_1}{F}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('x1', 'F'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Giff('x1', 'F'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax4 = subplot(2, 3, 4);
title('$\displaystyle \frac{F_s}{w}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('Fs', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'w'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');
ax5 = subplot(2, 3, 5);
title('$\displaystyle \frac{F_s}{f}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('Fs', 'f'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'f'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax6 = subplot(2, 3, 6);
title('$\displaystyle \frac{F_s}{F}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('Fs', 'F'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'F'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_tf_ol_and_cl.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_tf_ol_and_cl
#+caption : Matrix of transfer functions from input to output in open loop (blue) and closed loop (red)
#+RESULTS :
[[file:figs/amplified_piezo_tf_ol_and_cl.png ]]
** Root Locus
#+begin_src matlab :exports none :post
figure;
gains = logspace(1, 6, 500);
hold on;
plot(real(pole(G('Fs', 'f'))), imag(pole(G('Fs', 'f'))), 'kx');
plot(real(tzero(G('Fs', 'f'))), imag(tzero(G('Fs', 'f'))), 'ko');
for k = 1:length(gains)
cl_poles = pole(feedback(G('Fs', 'f'), -gains(k)/s));
plot(real(cl_poles), imag(cl_poles), 'k.');
end
hold off;
axis square;
xlim([-2500, 100]); ylim([0, 2600]);
xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name : fig:amplified_piezo_root_locus
#+caption : Root Locus
#+RESULTS :
[[file:figs/amplified_piezo_root_locus.png ]]
2020-05-25 10:45:43 +02:00
** Analytical Model
2020-05-25 10:25:13 +02:00
If we apply the Newton's second law of motion on the top mass, we obtain:
\[ ms^2 x_1 = F + k_1 (w - x_1) + k_e (x_e - x_1) \]
Then, we can write that the measured force $F_s$ is equal to:
\[ F_s = k_a(w - x_e) + f = -k_e (x_1 - x_e) \]
which gives:
\[ x_e = \frac{k_a}{k_e + k_a} w + \frac{1}{k_e + k_a} f + \frac{k_e}{k_e + k_a} x_1 \]
Re-injecting that into the previous equations gives:
\[ x_1 = F \frac{1}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} + w \frac{k_1 + \frac{k_e k_a}{k_e + k_a}}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} + f \frac{\frac{k_e}{k_e + k_a}}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \]
\[ F_s = - F \frac{\frac{k_e k_a}{k_e + k_a}}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} + w \frac{k_e k_a}{k_e + k_a} \Big( \frac{ms^2}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \Big) - f \frac{k_e}{k_e + k_a} \Big( \frac{ms^2 + k_1}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \Big) \]
#+begin_src matlab
Ga = 1/(m*s^2 + k1 + ke*ka/ (ke + ka)) * ...
[ 1 , k1 + ke*ka/(ke + ka) , ke/(ke + ka) ;
-ke*ka/(ke + ka), ke*ka/ (ke + ka)*m*s^2 , -ke/(ke+ka)* (m*s^2 + k1)];
Ga.InputName = {'F', 'w', 'f'};
Ga.OutputName = {'x1', 'Fs'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
ax1 = subplot(2, 3, 1);
title('$\displaystyle \frac{x_1}{w}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('x1', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('x1', 'w'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');
ax2 = subplot(2, 3, 2);
title('$\displaystyle \frac{x_1}{f}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('x1', 'f'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('x1', 'f'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax3 = subplot(2, 3, 3);
title('$\displaystyle \frac{x_1}{F}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('x1', 'F'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('x1', 'F'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax4 = subplot(2, 3, 4);
title('$\displaystyle \frac{F_s}{w}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('Fs', 'w'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'w'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');
ax5 = subplot(2, 3, 5);
title('$\displaystyle \frac{F_s}{f}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('Fs', 'f'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'f'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax6 = subplot(2, 3, 6);
title('$\displaystyle \frac{F_s}{F}$')
hold on;
plot(freqs, abs(squeeze(freqresp(G('Fs', 'F'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'F'), freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/comp_simscape_analytical.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:comp_simscape_analytical
#+caption : Comparison of the Identified Simscape Dynamics (solid) and the Analytical Model (dashed)
#+RESULTS :
[[file:figs/comp_simscape_analytical.png ]]
2020-05-25 11:05:52 +02:00
** Analytical Analysis
For Integral Force Feedback Control, the plant is:
\[ \frac{F_s}{f} = \frac{k_e}{k_e + k_a} \Big( \frac{ms^2 + k_1}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \Big) \]
As high frequency, this converge to:
\[ \frac{F_s}{f} \underset{\omega\to\infty}{\longrightarrow} \frac{k_e}{k_e + k_a} \]
And at low frequency:
\[ \frac{F_s}{f} \underset{\omega\to 0}{\longrightarrow} \frac{k_e}{k_e + k_a} \frac{k_1}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]
It has two complex conjugate zeros at:
\[ z = \pm j \sqrt{\frac{k_1}{m}} \]
And two complex conjugate poles at:
\[ p = \pm j \sqrt{\frac{k_1 + \frac{k_e k_a}{k_e + k_a}}{m}} \]
If maximal damping is to be attained with IFF, the distance between the zero and the pole is to be maximized.
Thus, we wish to maximize $p/z$, which is equivalent as to minimize $k_1$ and have $k_e \approx k_a$ (supposing $k_e + k_a \approx \text{cst}$).
2020-09-01 13:51:12 +02:00
#+begin_src matlab
m = 1;
k1 = 1e6;
ka = 1e6;
ke = 1e6;
Giff.InputName = {'f'};
Giff.OutputName = {'Fs'};
#+end_src
#+begin_src matlab :exports none
m = 1;
ka = 1e6;
ke = 1e6;
freqs = logspace(-1, 3, 1000);
figure;
hold on;
for k1 = [1e5, 1e6, 1e7, 1e8]
Giff = 1/(m*s^2 + k1 + ke*ka/ (ke + ka)) * (-ke/(ke+ka))*(m*s^2 + k1);
plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), ...
'DisplayName', sprintf('$k_1 = %.1e$', k1));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+begin_src matlab :exports none
m = 1;
k1 = 1e6;
ke = 1e6;
freqs = logspace(-1, 3, 1000);
figure;
hold on;
for ka = [1e5, 1e6, 1e7, 1e8]
Giff = 1/(m*s^2 + k1 + ke*ka/ (ke + ka)) * (-ke/(ke+ka))*(m*s^2 + k1);
plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), ...
'DisplayName', sprintf('$k_a = %.1e$', ka));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+begin_src matlab :exports none
m = 1;
k1 = 1e6;
ka = 1e6;
freqs = logspace(-1, 3, 1000);
figure;
hold on;
for ke = [1e5, 1e6, 1e7, 1e8]
Giff = 1/(m*s^2 + k1 + ke*ka/ (ke + ka)) * (-ke/(ke+ka))*(m*s^2 + k1);
plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), ...
'DisplayName', sprintf('$k_e = %.1e$', ke));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
2020-05-20 15:49:43 +02:00
* Rotating X-Y platform
2020-09-01 13:51:12 +02:00
** Introduction :ignore:
This analysis gave rise to a paper cite:dehaeze20_activ_dampin_rotat_platf_integ_force_feedb.
2020-05-20 15:49:43 +02:00
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+BEGIN_SRC matlab
simulinkproject('../');
#+END_SRC
#+begin_src matlab
open 'amplified_piezo_xy_rotating_stage.slx'
#+end_src
** Parameters
#+begin_src matlab
m = 1; % [kg]
ke = 4.8e6; % [N/m]
ce = 5; % [N/(m/s)]
me = 0.001; % [kg]
k1 = 0.96e6; % [N/m]
c1 = 10; % [N/(m/s)]
ka = 65e6; % [N/m]
ca = 5; % [N/(m/s)]
ma = 0.001; % [kg]
h = 0.2; % [m]
#+end_src
#+begin_src matlab
Kiff = tf(0);
#+end_src
** Identification
Rotating speed in rad/s:
#+begin_src matlab
Ws = 2*pi*[0, 1, 10, 100];
#+end_src
#+begin_src matlab
Gs = {zeros(length(Ws), 1)};
#+end_src
Identification in open-loop.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'amplified_piezo_xy_rotating_stage';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/fx'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/fy'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fs'], 2, 'openoutput'); io_i = io_i + 1;
for i = 1:length(Ws)
ws = Ws(i);
G = linearize(mdl, io, 0);
G.InputName = {'fx', 'fy'};
G.OutputName = {'Fsx', 'Fsy'};
Gs(i) = {G};
end
#+end_src
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
title('$\displaystyle \frac{F_{s,x}}{f_x}$')
hold on;
for i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsx', 'fx'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');
ax2 = subplot(2, 2, 2);
title('$\displaystyle \frac{F_{s,y}}{f_x}$')
hold on;
for i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsy', 'fx'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax3 = subplot(2, 2, 3);
title('$\displaystyle \frac{F_{s,x}}{f_y}$')
hold on;
for i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsx', 'fy'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');
ax4 = subplot(2, 2, 4);
title('$\displaystyle \frac{F_{s,y}}{f_y}$')
hold on;
for i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsy', 'fy'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplitifed_piezo_xy_rotation_plant_iff.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplitifed_piezo_xy_rotation_plant_iff
#+caption : Transfer function matrix from forces to force sensors for multiple rotation speed
#+RESULTS :
[[file:figs/amplitifed_piezo_xy_rotation_plant_iff.png ]]
** Root Locus
#+begin_src matlab :exports none :post
figure;
gains = logspace(1, 6, 500);
hold on;
for i = 1:length(Ws)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gs{i})), imag(pole(Gs{i})), 'x');
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gs{i})), imag(tzero(Gs{i})), 'o');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gs{i}, -gains(k)/s*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.');
end
end
hold off;
axis square;
xlim([-2900, 100]); ylim([0, 3000]);
xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_xy_rotation_root_locus.pdf', 'width', 'tall', 'height', 'wide');
#+end_src
#+name : fig:amplified_piezo_xy_rotation_root_locus
#+caption : Root locus for 3 rotating speed
#+RESULTS :
[[file:figs/amplified_piezo_xy_rotation_root_locus.png ]]
** Analysis
The negative stiffness induced by the rotation is equal to $m \omega_0^2$.
Thus, the maximum rotation speed where IFF can be applied is:
\[ \omega_\text{max} = \sqrt{\frac{k_1}{m}} \approx 156\,[Hz] \]
Let's verify that.
#+begin_src matlab
Ws = 2*pi*[140, 160];
#+end_src
#+begin_src matlab :exports none
Gs = {zeros(length(Ws), 1)};
#+end_src
Identification
#+begin_src matlab
%% Name of the Simulink File
mdl = 'amplified_piezo_xy_rotating_stage';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/fx'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/fy'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fs'], 2, 'openoutput'); io_i = io_i + 1;
for i = 1:length(Ws)
ws = Ws(i);
G = linearize(mdl, io, 0);
G.InputName = {'fx', 'fy'};
G.OutputName = {'Fsx', 'Fsy'};
Gs(i) = {G};
end
#+end_src
#+begin_src matlab :exports none
figure;
gains = logspace(1, 6, 500);
hold on;
for i = 1:length(Ws)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gs{i})), imag(pole(Gs{i})), 'x');
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gs{i})), imag(tzero(Gs{i})), 'o');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gs{i}, -gains(k)/s*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.');
end
end
hold off;
axis square;
xlim([-100, 50]); ylim([0, 150]);
xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_xy_rotating_unstable_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name : fig:amplified_piezo_xy_rotating_unstable_root_locus
#+caption : Root Locus for the two considered rotation speed. For the red curve, the system is unstable.
#+RESULTS :
[[file:figs/amplified_piezo_xy_rotating_unstable_root_locus.png ]]
2020-05-20 16:04:05 +02:00
* Stewart Platform with Amplified Actuators
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
#+begin_src matlab
open('nass_model.slx')
#+end_src
2020-05-20 16:41:34 +02:00
** Initialization
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'hac-iff');
#+end_src
We set the stiffness of the payload fixation:
#+begin_src matlab
Kp = 1e8; % [N/m]
#+end_src
2020-09-01 13:51:12 +02:00
** APA-100 Amplified Actuator
*** Identification
2020-05-20 16:41:34 +02:00
#+begin_src matlab
K = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
#+end_src
#+begin_src matlab :exports none
Gm_iff = {zeros(length(Ms), 1)};
#+end_src
The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
initializeNanoHexapod('actuator', 'amplified');
#+end_src
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
Gm_iff(i) = {G_iff};
end
#+end_src
2020-09-01 13:51:12 +02:00
*** Controller Design
The loop gain for IFF is shown in Figure [[fig:amplified_piezo_iff_loop_gain ]].
The corresponding root locus is shown in Figure [[fig:amplified_piezo_iff_root_locus ]].
Finally, the damping as function of the gain is display in Figure [[fig:amplified_piezo_iff_damping_gain ]].
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_loop_gain.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_loop_gain
#+caption : Dynamics for the Integral Force Feedback for three payload masses
#+RESULTS :
[[file:figs/amplified_piezo_iff_loop_gain.png ]]
#+begin_src matlab :exports none
figure;
2020-09-01 13:51:12 +02:00
gains = logspace(2, 4, 200);
2020-05-20 16:41:34 +02:00
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gm_iff{i})), imag(pole(Gm_iff{i})), 'x', ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gm_iff{i})), imag(tzero(Gm_iff{i})), 'o', ...
'HandleVisibility', 'off');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-400, 10]); ylim([0, 500]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name : fig:amplified_piezo_iff_root_locus
#+caption : Root Locus for the IFF control for three payload masses
#+RESULTS :
[[file:figs/amplified_piezo_iff_root_locus.png ]]
#+begin_src matlab :exports none
c1 = [ 0 0.4470 0.7410]; % Blue
c2 = [0.8500 0.3250 0.0980]; % Orange
c3 = [0.9290 0.6940 0.1250]; % Yellow
c4 = [0.4940 0.1840 0.5560]; % Purple
c5 = [0.4660 0.6740 0.1880]; % Green
c6 = [0.3010 0.7450 0.9330]; % Light Blue
c7 = [0.6350 0.0780 0.1840]; % Red
colors = [c1; c2; c3; c4; c5; c6; c7];
figure;
gains = logspace(2, 5, 100);
hold on;
for i = 1:length(Ms)
for k = 1:length(gains)
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
set(gca,'ColorOrderIndex',i);
plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
end
end
hold off;
xlabel('IFF Gain'); ylabel('Modal Damping');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylim([0, 1]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damping_gain.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_damping_gain
#+caption : Damping ratio of the poles as a function of the IFF gain
#+RESULTS :
[[file:figs/amplified_piezo_iff_damping_gain.png ]]
2020-09-01 13:51:12 +02:00
The following controller for the Decentralized Integral Force Feedback is used:
2020-05-20 16:41:34 +02:00
#+begin_src matlab
Kiff = -1e4/s*eye(6);
#+end_src
2020-09-01 13:51:12 +02:00
*** Effect of the Low Authority Control on the Primary Plant
**** Introduction :ignore:
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
#+end_src
2020-09-01 13:51:12 +02:00
**** Identification of the undamped plant :ignore:
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
2020-05-20 16:55:50 +02:00
Kiff_backup = Kiff;
Kiff = tf(zeros(6));
2020-05-20 16:41:34 +02:00
#+end_src
#+begin_src matlab :exports none
G_x = {zeros(length(Ms), 1)};
G_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_x(i) = {Gx};
Gl = -nano_hexapod.kinematics.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
G_l(i) = {Gl};
end
#+end_src
#+begin_src matlab :exports none
2020-05-20 16:55:50 +02:00
Kiff = Kiff_backup;
2020-05-20 16:41:34 +02:00
#+end_src
2020-09-01 13:51:12 +02:00
**** Identification of the damped plant :ignore:
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
Gm_x = {zeros(length(Ms), 1)};
Gm_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gm_x(i) = {Gx};
Gl = -nano_hexapod.kinematics.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
Gm_l(i) = {Gl};
end
#+end_src
2020-09-01 13:51:12 +02:00
**** Effect of the Damping on the plant diagonal dynamics :ignore:
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/ \mathcal{F}_y$')
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_z/\mathcal{F}_z$')
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
2020-05-20 16:55:50 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_plant_damped_X.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_plant_damped_X
#+caption : Primary plant in the task space with (dashed) and without (solid) IFF
#+RESULTS :
[[file:figs/amplified_piezo_iff_plant_damped_X.png ]]
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
#+end_src
2020-05-20 16:55:50 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damped_plant_L.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_damped_plant_L
#+caption : Primary plant in the space of the legs with (dashed) and without (solid) IFF
#+RESULTS :
[[file:figs/amplified_piezo_iff_damped_plant_L.png ]]
2020-09-01 13:51:12 +02:00
**** Effect of the Damping on the coupling dynamics :ignore:
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G_x{1}(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
plot(freqs, abs(squeeze(freqresp(Gm_x{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_x{1}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gm_x{1}(1, 1), freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, inf]);
#+end_src
2020-05-20 16:55:50 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damped_coupling_X.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_damped_coupling_X
#+caption : Coupling in the primary plant in the task with (dashed) and without (solid) IFF
#+RESULTS :
[[file:figs/amplified_piezo_iff_damped_coupling_X.png ]]
2020-05-20 16:41:34 +02:00
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G_l{1}(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
plot(freqs, abs(squeeze(freqresp(Gm_l{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_l{1}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gm_l{1}(1, 1), freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, inf]);
#+end_src
2020-05-20 16:55:50 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damped_coupling_L.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_damped_coupling_L
#+caption : Coupling in the primary plant in the space of the legs with (dashed) and without (solid) IFF
#+RESULTS :
[[file:figs/amplified_piezo_iff_damped_coupling_L.png ]]
2020-09-01 13:51:12 +02:00
*** Effect of the Low Authority Control on the Sensibility to Disturbances
**** Introduction :ignore:
2020-05-20 16:55:50 +02:00
2020-09-01 13:51:12 +02:00
**** Identification :ignore:
2020-05-20 16:55:50 +02:00
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Micro-Hexapod
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
Kiff_backup = Kiff;
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab :exports none
Gd = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gd(i) = {G};
end
#+end_src
#+begin_src matlab :exports none
Kiff = Kiff_backup;
#+end_src
#+begin_src matlab :exports none
Gd_iff = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gd_iff(i) = {G};
end
#+end_src
2020-09-01 13:51:12 +02:00
**** Results :ignore:
2020-05-20 16:55:50 +02:00
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
subplot(2, 2, 1);
title('$D_{w,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');
subplot(2, 2, 2);
title('$F_{dz}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');
subplot(2, 2, 3);
title('$F_{T_y,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
subplot(2, 2, 4);
title('$F_{R_z,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_disturbances.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_iff_disturbances
#+caption : Norm of the transfer function from vertical disturbances to vertical position error with (dashed) and without (solid) Integral Force Feedback applied
#+RESULTS :
[[file:figs/amplified_piezo_iff_disturbances.png ]]
*** Conclusion :ignore:
#+begin_important
#+end_important
2020-05-20 17:14:38 +02:00
** Optimal Stiffnesses
2020-09-01 13:51:12 +02:00
*** Introduction :ignore:
Based on the analytical analysis, we can determine the parameters of the amplified piezoelectric actuator in order to be able to add a lots of damping using IFF:
- $k_1$ should be minimized.
- $k_e \approx k_a \approx 10^5 - 10^6\,[N/m]$
However, this might not be realizable.
*** Low Authority Controller
**** Identification
The nano-hexapod is initialized with the following parameters:
#+begin_src matlab
initializeNanoHexapod('actuator', 'amplified', ...
'k1', 1e4, ...
'ke', 1e6, ...
'ka', 1e6);
#+end_src
#+begin_src matlab :exports none
initializeController('type', 'hac-iff');
K = tf(zeros(6));
Kiff = tf(zeros(6));
Ms = [1, 10, 50];
Gm_iff = {zeros(length(Ms), 1)};
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
Gm_iff(i) = {G_iff};
end
#+end_src
The obtain plan for the IFF control is shown in Figure [[fig:amplified_piezo_opt_stiff_iff_plant ]].
The associated Root Locus is shown in Figure [[fig:amplified_piezo_opt_stiff_iff_root_locus ]].
Based on that, the following IFF gain is chosen:
#+begin_src matlab
Kiff = -1e3/s*eye(6);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_opt_stiff_iff_plant.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_opt_stiff_iff_plant
#+caption : Plant dynamics for IFF with the amplified piezoelectric stack actuator
#+RESULTS :
[[file:figs/amplified_piezo_opt_stiff_iff_plant.png ]]
#+begin_src matlab :exports none
figure;
gains = logspace(1, 4, 400);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gm_iff{i})), imag(pole(Gm_iff{i})), 'x', ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gm_iff{i})), imag(tzero(Gm_iff{i})), 'o', ...
'HandleVisibility', 'off');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-400, 10]); ylim([0, 410]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_opt_stiff_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name : fig:amplified_piezo_opt_stiff_iff_root_locus
#+caption : Root Locus for IFF with the amplified piezoelectric stack actuator
#+RESULTS :
[[file:figs/amplified_piezo_opt_stiff_iff_root_locus.png ]]
#+begin_src matlab :exports none
c1 = [ 0 0.4470 0.7410]; % Blue
c2 = [0.8500 0.3250 0.0980]; % Orange
c3 = [0.9290 0.6940 0.1250]; % Yellow
c4 = [0.4940 0.1840 0.5560]; % Purple
c5 = [0.4660 0.6740 0.1880]; % Green
c6 = [0.3010 0.7450 0.9330]; % Light Blue
c7 = [0.6350 0.0780 0.1840]; % Red
colors = [c1; c2; c3; c4; c5; c6; c7];
figure;
gains = logspace(2, 5, 100);
hold on;
for i = 1:length(Ms)
for k = 1:length(gains)
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
set(gca,'ColorOrderIndex',i);
plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
end
end
hold off;
xlabel('IFF Gain'); ylabel('Modal Damping');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylim([0, 1]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_opt_stiff_gain_damping.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_opt_stiff_gain_damping
#+caption : Damping of the modes as a function of the IFF gain
#+RESULTS :
[[file:figs/amplified_piezo_opt_stiff_gain_damping.png ]]
**** Effect of the Low Authority Control on the Primary Plant
***** Introduction :ignore:
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
#+end_src
***** Identification of the undamped plant :ignore:
#+begin_src matlab :exports none
Kiff_backup = Kiff;
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab :exports none
G_x = {zeros(length(Ms), 1)};
G_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_x(i) = {Gx};
Gl = -nano_hexapod.kinematics.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
G_l(i) = {Gl};
end
#+end_src
#+begin_src matlab :exports none
Kiff = Kiff_backup;
#+end_src
***** Identification of the damped plant :ignore:
#+begin_src matlab :exports none
Gm_x = {zeros(length(Ms), 1)};
Gm_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gm_x(i) = {Gx};
Gl = -nano_hexapod.kinematics.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
Gm_l(i) = {Gl};
end
#+end_src
***** Effect of the Damping on the plant diagonal dynamics :ignore:
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/ \mathcal{F}_y$')
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_z/\mathcal{F}_z$')
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
#+end_src
**** Effect of the Low Authority Control on the Sensibility to Disturbances
***** Introduction :ignore:
***** Identification :ignore:
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Micro-Hexapod
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
Kiff_backup = Kiff;
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab :exports none
Gd = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gd(i) = {G};
end
#+end_src
#+begin_src matlab :exports none
Kiff = Kiff_backup;
#+end_src
#+begin_src matlab :exports none
Gd_iff = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gd_iff(i) = {G};
end
#+end_src
***** Results :ignore:
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
subplot(2, 2, 1);
title('$D_{w,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');
subplot(2, 2, 2);
title('$F_{dz}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');
subplot(2, 2, 3);
title('$F_{T_y,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
subplot(2, 2, 4);
title('$F_{R_z,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_opt_stiff_iff_dist.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name : fig:amplified_piezo_opt_stiff_iff_dist
#+caption : Effect of disturbance with and without IFF
#+RESULTS :
[[file:figs/amplified_piezo_opt_stiff_iff_dist.png ]]
***** Conclusion :ignore:
#+begin_important
#+end_important
*** High Authority Controller
**** Introduction :ignore:
**** Controller Design
#+begin_src matlab
h = 2.5;
Kl = 5e6 * eye(6) * ...
1/h*(s/ (2*pi*40/h) + 1)/ (s/(2*pi*40*h) + 1) * ...
1/h*(s/ (2*pi*100/h) + 1)/ (s/(2*pi*100*h) + 1) * ...
(s/2/pi/50 + 1)/ (s/2/pi/50) * ...
(s/2/pi/10 + 1)/ (s/2/pi/10) * ...
1/(1 + s/2/pi/200);
#+end_src
#+begin_src matlab
Kl = 3e10 * eye(6) * ...
1/s * ...
(s+0.8)/s * ...
(s+50)/(s+0.01) * ...
(s+120)/(s+1000) * ...
(s+150)/(s+1000);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
for j = 1:6
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
for j = 1:6
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
end
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');
#+end_src
Finally, we include the Jacobian in the control and we ignore the measurement of the vertical rotation as for the real system.
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
K = Kl*nano_hexapod.kinematics.J*diag([1, 1, 1, 1, 1, 0]);
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(nano_hexapod.kinematics.J\Gm_l{i}*K, eye(6), -1))
end
#+end_src
**** Sensibility to Disturbances and Noise Budget
***** Identification :ignore:
We identify the transfer function from disturbances to the position error of the sample when the HAC-LAC control is applied.
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Micro-Hexapod
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
Gd_L = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gd_L(i) = {G};
end
#+end_src
***** Obtained Sensibility to Disturbances :ignore:
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
subplot(2, 2, 1);
title('$D_{w,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');
subplot(2, 2, 2);
title('$F_{dz}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');
subplot(2, 2, 3);
title('$F_{T_y,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
subplot(2, 2, 4);
title('$F_{R_z,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src
**** Simulations of Tomography Experiment
Let's now simulate a tomography experiment.
To do so, we include all disturbances except vibrations of the translation stage.
#+begin_src matlab
initializeDisturbances();
initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab :exports none
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
And we run the simulation for all three payload Masses.
#+begin_src matlab :exports none
hac_dvf_L = {zeros(length(Ms)), 1};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i));
sim('nass_model');
hac_iff_L(i) = {simout};
end
#+end_src
#+begin_src matlab :exports none
save('./mat/tomo_exp_hac_iff_opt_stiff.mat', 'hac_dvf_L', 'Ms');
#+end_src
**** Results
#+begin_src matlab :exports none
load('./mat/experiment_tomography.mat', 'scans_rz_align_dist');
load('./mat/tomo_exp_hac_iff_opt_stiff.mat', 'hac_iff_L', 'Ms');
#+end_src
#+begin_src matlab :exports none
n_av = 4;
han_win = hanning(ceil(length(hac_iff_L{1}.Em.En.Data(:,1))/n_av));
t = hac_iff_L{1}.Em.En.Time;
Ts = t(2)-t(1);
[pxx_ol, f] = pwelch(scans_rz_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
pxx_iff_L = zeros(length(f), 6, length(Ms));
for i = 1:length(Ms)
[pxx, ~] = pwelch(hac_iff_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
pxx_iff_L(:, :, i) = pxx;
end
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(pxx_ol(:, 1)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_iff_L(:, 1, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(pxx_ol(:, 2)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_iff_L(:, 2, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(pxx_ol(:, 3)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_iff_L(:, 3, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(pxx_ol(:, 4)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_iff_L(:, 4, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(pxx_ol(:, 5)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_iff_L(:, 5, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(pxx_ol(:, 6)), 'k-', 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_iff_L(:, 6, i)), ...
'DisplayName', sprintf('HAC-IFF $m = %.0f kg$', Ms(i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_iff_L(:, 1, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_x$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_iff_L(:, 2, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_y$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_iff_L(:, 3, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_z$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_iff_L(:, 4, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_x$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_iff_L(:, 5, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_y$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'k-', 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_iff_L(:, 6, i))))), ...
'DisplayName', sprintf('HAC-IFF $m = %.0f kg$', Ms(i)));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_z$ [$rad$]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 1), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 1));
end
hold off;
xlabel('Time [s]');
ylabel('Dx [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 2), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 2));
end
hold off;
xlabel('Time [s]');
ylabel('Dy [m]');
ax3 = subplot(2, 3, 3);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 3), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 3));
end
hold off;
xlabel('Time [s]');
ylabel('Dz [m]');
ax4 = subplot(2, 3, 4);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 4));
end
hold off;
xlabel('Time [s]');
ylabel('Rx [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 5));
end
hold off;
xlabel('Time [s]');
ylabel('Ry [rad]');
ax6 = subplot(2, 3, 6);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6), 'k-', ...
'DisplayName', '$\mu$-Station');
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 6), ...
'DisplayName', sprintf('HAC-IFF $m = %.0f kg$', Ms(i)));
end
hold off;
xlabel('Time [s]');
ylabel('Rz [rad]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
** Direct Velocity Feedback with Amplified Actuators
Lack of collocation.
#+begin_src matlab
initializeController('type', 'hac-dvf');
K = tf(zeros(6));
Kdvf = tf(zeros(6));
#+end_src
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
#+end_src
#+begin_src matlab :exports none
Gm_dvf = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab
initializeNanoHexapod('actuator', 'amplified', ...
'k1', 1e4, ...
'ke', 1e6, ...
'ka', 1e6);
#+end_src
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
Gm_dvf(i) = {G_dvf};
end
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 4, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none
figure;
gains = logspace(2, 4, 100);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gm_dvf{i})), imag(pole(Gm_dvf{i})), 'x', ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gm_dvf{i})), imag(tzero(Gm_dvf{i})), 'o', ...
'HandleVisibility', 'off');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-400, 10]); ylim([0, 500]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
#+end_src
* APA300ML
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
#+begin_src matlab
open('nass_model.slx')
#+end_src
** Initialization
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'hac-dvf');
#+end_src
We set the stiffness of the payload fixation:
#+begin_src matlab
Kp = 1e8; % [N/m]
#+end_src
** Identification
#+begin_src matlab
K = tf(zeros(6));
Kdvf = tf(zeros(6));
#+end_src
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
#+end_src
#+begin_src matlab :exports none
Gm_dvf = {zeros(length(Ms), 1)};
#+end_src
The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
initializeNanoHexapod('actuator', 'amplified', 'k1', 0.4e6, 'ka', 43e6, 'ke', 1.5e6);
#+end_src
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
Gm_dvf(i) = {G_dvf};
end
#+end_src
** Controller Design
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none
figure;
gains = logspace(2, 6, 100);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gm_dvf{i})), imag(pole(Gm_dvf{i})), 'x', ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gm_dvf{i})), imag(tzero(Gm_dvf{i})), 'o', ...
'HandleVisibility', 'off');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-1e3, 10]); ylim([0, 1e3]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
#+end_src
Damping as function of the gain
#+begin_src matlab :exports none
c1 = [ 0 0.4470 0.7410]; % Blue
c2 = [0.8500 0.3250 0.0980]; % Orange
c3 = [0.9290 0.6940 0.1250]; % Yellow
c4 = [0.4940 0.1840 0.5560]; % Purple
c5 = [0.4660 0.6740 0.1880]; % Green
c6 = [0.3010 0.7450 0.9330]; % Light Blue
c7 = [0.6350 0.0780 0.1840]; % Red
colors = [c1; c2; c3; c4; c5; c6; c7];
figure;
gains = logspace(4, 7, 100);
hold on;
for i = 1:length(Ms)
for k = 1:length(gains)
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
set(gca,'ColorOrderIndex',i);
plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
end
end
hold off;
xlabel('DVF Gain'); ylabel('Modal Damping');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylim([0, 1]);
#+end_src
Finally, we use the following controller for the Decentralized Direct Velocity Feedback:
#+begin_src matlab
Kdvf = 5e5*s/(1+s/2/pi/1e3)*eye(6);
#+end_src
** Effect of the Low Authority Control on the Primary Plant
*** Introduction :ignore:
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
#+end_src
*** Identification of the undamped plant :ignore:
#+begin_src matlab :exports none
Kdvf_backup = Kdvf;
Kdvf = tf(zeros(6));
#+end_src
#+begin_src matlab :exports none
G_x = {zeros(length(Ms), 1)};
G_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_x(i) = {Gx};
Gl = -nano_hexapod.kinematics.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
G_l(i) = {Gl};
end
#+end_src
#+begin_src matlab :exports none
Kdvf = Kdvf_backup;
#+end_src
*** Identification of the damped plant :ignore:
#+begin_src matlab :exports none
Gm_x = {zeros(length(Ms), 1)};
Gm_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gm_x(i) = {Gx};
Gl = -nano_hexapod.kinematics.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
Gm_l(i) = {Gl};
end
#+end_src
*** Effect of the Damping on the plant diagonal dynamics :ignore:
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/ \mathcal{F}_y$')
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_z/\mathcal{F}_z$')
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
#+end_src
** Control in the leg space
We design a diagonal controller with all the same diagonal elements.
The requirements for the controller are:
- Crossover frequency of around 100Hz
- Stable for all the considered payload masses
- Sufficient phase and gain margin
- Integral action at low frequency
The design controller is as follows:
- Lead centered around the crossover
- An integrator below 10Hz
- A low pass filter at 250Hz
#+begin_src matlab
h = 2.0;
Kl = 1e9 * eye(6) * ...
1/h*(s/ (2*pi*100/h) + 1)/ (s/(2*pi*100*h) + 1) * ...
1/h*(s/ (2*pi*200/h) + 1)/ (s/(2*pi*200*h) + 1) * ...
(s/2/pi/10 + 1)/ (s/2/pi/10) * ...
1/(1 + s/2/pi/300);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
for j = 1:6
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
for j = 1:6
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
end
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');
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
K = Kl*nano_hexapod.kinematics.J*diag([1, 1, 1, 1, 1, 0]);
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(nano_hexapod.kinematics.J\Gm_l{i}*K, eye(6), -1))
end
#+end_src
** Sensibility to Disturbances and Noise Budget
*** Identification :ignore:
We identify the transfer function from disturbances to the position error of the sample when the HAC-LAC control is applied.
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'nass_model';
%% Micro-Hexapod
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
Gd_L = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gd_L(i) = {G};
end
#+end_src
*** Obtained Sensibility to Disturbances :ignore:
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
subplot(2, 2, 1);
title('$D_{w,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
% set(gca,'ColorOrderIndex',i);
% plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ...
% 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');
subplot(2, 2, 2);
title('$F_{dz}$ to $E_z$');
hold on;
for i = 1:length(Ms)
% set(gca,'ColorOrderIndex',i);
% plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz'))));
% set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');
subplot(2, 2, 3);
title('$F_{T_y,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
% set(gca,'ColorOrderIndex',i);
% plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz'))));
% set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
subplot(2, 2, 4);
title('$F_{R_z,z}$ to $E_z$');
hold on;
for i = 1:length(Ms)
% set(gca,'ColorOrderIndex',i);
% plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz'))));
% set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src
*** Noise Budgeting :ignore:
#+begin_src matlab :exports none
load('./mat/dist_psd.mat', 'dist_f');
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
plot(dist_f.f, sqrt(dist_f.psd_gm).*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Dwz' ), dist_f.f, 'Hz'))), 'DisplayName', '$D_w$')
plot(dist_f.f, sqrt(dist_f.psd_rz).*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))), 'DisplayName', '$F_ {R_z}$')
hold off;
xlabel('Frequency [Hz]');
ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
legend('location', 'southwest');
xlim([1, 500]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_psd_dist.pdf', 'width', 'full', 'height', 'tall')
#+end_src
#+name : fig:opt_stiff_primary_control_L_psd_dist
#+caption : Amplitude Spectral Density of the vertical position error of the sample when the HAC-DVF control is applied due to both the ground motion and spindle vibrations
#+RESULTS :
[[file:figs/opt_stiff_primary_control_L_psd_dist.png ]]
#+begin_src matlab :exports none
figure;
hold on;
plot(dist_f.f, sqrt(dist_f.psd_gm.*abs(squeeze(freqresp(Gd{1}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ...
dist_f.psd_rz.*abs(squeeze(freqresp(Gd{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2), 'DisplayName', 'Open-Loop')
plot(dist_f.f, sqrt(dist_f.psd_gm.*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ...
dist_f.psd_rz.*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2), 'DisplayName', 'HAC-DVF')
hold off;
xlabel('Frequency [Hz]');
ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
legend('location', 'northeast');
xlim([1, 500]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_psd_tot.pdf', 'width', 'full', 'height', 'tall')
#+end_src
#+name : fig:opt_stiff_primary_control_L_psd_tot
#+caption : Amplitude Spectral Density of the vertical position error of the sample in Open-Loop and when the HAC-DVF control is applied
#+RESULTS :
[[file:figs/opt_stiff_primary_control_L_psd_tot.png ]]
#+begin_src matlab :exports none
figure;
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(dist_f.f, sqrt(flip(-cumtrapz(flip(dist_f.f), flip(dist_f.psd_gm.*abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ...
dist_f.psd_rz.*abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2)))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(dist_f.f, sqrt(flip(-cumtrapz(flip(dist_f.f), flip(dist_f.psd_gm.*abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ...
dist_f.psd_rz.*abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2)))), '--', ...
'HandleVisibility', 'off')
end
hold off;
xlabel('Frequency [Hz]');
ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
legend('location', 'southwest');
xlim([0.1, 500]); ylim([1e-12, 1e-6]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'height', 'tall')
#+end_src
#+name : fig:opt_stiff_primary_control_L_cas_tot
#+caption : Cumulative Amplitude Spectrum of the vertical position error of the sample in Open-Loop and when the HAC-DVF control is applied
#+RESULTS :
[[file:figs/opt_stiff_primary_control_L_cas_tot.png ]]
** Simulations of Tomography Experiment
Let's now simulate a tomography experiment.
To do so, we include all disturbances except vibrations of the translation stage.
#+begin_src matlab
initializeDisturbances();
initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab :exports none
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
And we run the simulation for all three payload Masses.
#+begin_src matlab :exports none
hac_dvf_L = {zeros(length(Ms)), 1};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i));
sim('nass_model');
hac_dvf_L(i) = {simout};
end
#+end_src
#+begin_src matlab :exports none
save('./mat/tomo_exp_hac_dvf_ampl.mat', 'hac_dvf_L', 'Ms');
#+end_src
** Results
#+begin_src matlab :exports none
load('./mat/experiment_tomography.mat', 'scans_rz_align_dist');
load('./mat/tomo_exp_hac_dvf_ampl.mat', 'hac_dvf_L', 'Ms');
#+end_src
#+begin_src matlab :exports none
n_av = 4;
han_win = hanning(ceil(length(hac_dvf_L{1}.Em.En.Data(:,1))/n_av));
t = hac_dvf_L{1}.Em.En.Time;
Ts = t(2)-t(1);
[pxx_ol, f] = pwelch(scans_rz_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
pxx_dvf_L = zeros(length(f), 6, length(Ms));
for i = 1:length(Ms)
[pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
pxx_dvf_L(:, :, i) = pxx;
end
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(pxx_ol(:, 1)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 1, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(pxx_ol(:, 2)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 2, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(pxx_ol(:, 3)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 3, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(pxx_ol(:, 4)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 4, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(pxx_ol(:, 5)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 5, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(pxx_ol(:, 6)), 'k-', 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_x$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_y$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_z$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_x$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_y$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'k-', 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_z$ [$rad$]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 1), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
end
hold off;
xlabel('Time [s]');
ylabel('Dx [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 2), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
end
hold off;
xlabel('Time [s]');
ylabel('Dy [m]');
ax3 = subplot(2, 3, 3);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 3), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
end
hold off;
xlabel('Time [s]');
ylabel('Dz [m]');
ax4 = subplot(2, 3, 4);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
end
hold off;
xlabel('Time [s]');
ylabel('Rx [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
end
hold off;
xlabel('Time [s]');
ylabel('Ry [rad]');
ax6 = subplot(2, 3, 6);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6), 'k-', ...
'DisplayName', '$\mu$-Station');
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
end
hold off;
xlabel('Time [s]');
ylabel('Rz [rad]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
