1575 lines
52 KiB
Org Mode
1575 lines
52 KiB
Org Mode
#+TITLE: Control of the NASS with optimal stiffness
|
|
:DRAWER:
|
|
#+STARTUP: overview
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
|
|
#+PROPERTY: header-args:shell :eval no-export
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results file raw replace
|
|
#+PROPERTY: header-args:latex+ :buffer no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports results
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
|
:END:
|
|
|
|
* Introduction :ignore:
|
|
|
|
* Low Authority Control - Decentralized Direct Velocity Feedback
|
|
<<sec:lac_dvf>>
|
|
** Introduction :ignore:
|
|
|
|
#+name: fig:control_architecture_dvf
|
|
#+caption: Low Authority Control: Decentralized Direct Velocity Feedback
|
|
#+RESULTS:
|
|
[[file:figs/control_architecture_dvf.png]]
|
|
|
|
** 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
|
|
load('mat/conf_simulink.mat');
|
|
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('k', 1e5, 'c', 2e2);
|
|
#+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
|
|
The obtain dynamics from actuators forces $\tau_i$ to the relative motion of the legs $d\mathcal{L}_i$ is shown in Figure [[fig:opt_stiff_dvf_plant]] for the three considered payload masses.
|
|
|
|
The Root Locus is shown in Figure [[fig:opt_stiff_dvf_root_locus]] and wee see that we have unconditional stability.
|
|
|
|
In order to choose the gain such that we obtain good damping for all the three payload masses, we plot the damping ration of the modes as a function of the gain for all three payload masses in Figure [[fig:opt_stiff_dvf_damping_gain]].
|
|
|
|
#+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 :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_dvf_plant.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_dvf_plant
|
|
#+caption: Dynamics for the Direct Velocity Feedback active damping for three payload masses
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_dvf_plant.png]]
|
|
|
|
#+begin_src matlab :exports none :post
|
|
figure;
|
|
|
|
gains = logspace(2, 5, 300);
|
|
|
|
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([-140, 10]); ylim([0, 150]);
|
|
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_dvf_root_locus
|
|
#+caption: Root Locus for the DVF controll for three payload masses
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_dvf_root_locus.png]]
|
|
|
|
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(1, 4, 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
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_dvf_damping_gain.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_dvf_damping_gain
|
|
#+caption: Damping ratio of the poles as a function of the DVF gain
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_dvf_damping_gain.png]]
|
|
|
|
Finally, we use the following controller for the Decentralized Direct Velocity Feedback:
|
|
#+begin_src matlab
|
|
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
|
|
#+end_src
|
|
|
|
** Effect of the Low Authority Control on the Primary Plant
|
|
*** Introduction :ignore:
|
|
Let's identify the dynamics from actuator forces $\bm{\tau}$ to displacement as measured by the metrology $\bm{\mathcal{X}}$:
|
|
\[ \bm{G}(s) = \frac{\bm{\mathcal{X}}}{\bm{\tau}} \]
|
|
We do so both when the DVF is applied and when it is not applied.
|
|
|
|
|
|
Then, we compute the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
|
|
\[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]
|
|
The obtained dynamics is shown in Figure [[fig:opt_stiff_primary_plant_damped_X]].
|
|
|
|
|
|
And we compute the transfer function from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
|
|
\[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]
|
|
The obtained dynamics is shown in Figure [[fig:opt_stiff_primary_plant_damped_L]].
|
|
|
|
#+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.J');
|
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
G_x(i) = {Gx};
|
|
|
|
Gl = -nano_hexapod.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.J');
|
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
Gm_x(i) = {Gx};
|
|
|
|
Gl = -nano_hexapod.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 :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_plant_damped_X.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_plant_damped_X
|
|
#+caption: Primary plant in the task space with (dashed) and without (solid) Direct Velocity Feedback
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_plant_damped_X.png]]
|
|
|
|
#+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
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_plant_damped_L.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_plant_damped_L
|
|
#+caption: Primary plant in the space of the legs with (dashed) and without (solid) Direct Velocity Feedback
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_plant_damped_L.png]]
|
|
|
|
*** Effect of the Damping on the coupling dynamics :ignore:
|
|
The coupling (off diagonal elements) of $\bm{G}_\mathcal{X}$ are shown in Figure [[fig:opt_stiff_primary_plant_damped_coupling_X]] both when DVF is applied and when it is not.
|
|
|
|
The coupling does not change a lot with DVF.
|
|
|
|
|
|
The coupling in the space of the legs $\bm{G}_\mathcal{L}$ are shown in Figure [[fig:opt_stiff_primary_plant_damped_coupling_L]].
|
|
The magnitude of the coupling around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
|
|
|
|
#+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
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_plant_damped_coupling_X.pdf', 'width', 'full', 'height', 'tall')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_plant_damped_coupling_X
|
|
#+caption: Coupling in the primary plant in the task with (dashed) and without (solid) Direct Velocity Feedback
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_plant_damped_coupling_X.png]]
|
|
|
|
#+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
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_plant_damped_coupling_L.pdf', 'width', 'full', 'height', 'tall')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_plant_damped_coupling_L
|
|
#+caption: Coupling in the primary plant in the space of the legs with (dashed) and without (solid) Direct Velocity Feedback
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_plant_damped_coupling_L.png]]
|
|
|
|
** Effect of the Low Authority Control on the Sensibility to Disturbances
|
|
*** Introduction :ignore:
|
|
We may now see how Decentralized Direct Velocity Feedback changes the sensibility to disturbances, namely:
|
|
- Ground motion
|
|
- Spindle and Translation stage vibrations
|
|
- Direct forces applied to the sample
|
|
|
|
To simplify the analysis, we here only consider the vertical direction, thus, we will look at the transfer functions:
|
|
- from vertical ground motion $D_{w,z}$ to the vertical position error of the sample $E_z$
|
|
- from vertical vibration forces of the spindle $F_{R_z,z}$ to $E_z$
|
|
- from vertical vibration forces of the translation stage $F_{T_y,z}$ to $E_z$
|
|
- from vertical direct forces (such as cable forces) $F_{d,z}$ to $E_z$
|
|
|
|
The norm of these transfer functions are shown in Figure [[fig:opt_stiff_sensibility_dist_dvf]].
|
|
|
|
*** 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
|
|
Kdvf_backup = Kdvf;
|
|
Kdvf = 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
|
|
Kdvf = Kdvf_backup;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
Gd_dvf = {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_dvf(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_dvf{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_dvf{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_dvf{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_dvf{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/opt_stiff_sensibility_dist_dvf.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_sensibility_dist_dvf
|
|
#+caption: Norm of the transfer function from vertical disturbances to vertical position error with (dashed) and without (solid) Direct Velocity Feedback applied
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_sensibility_dist_dvf.png]]
|
|
|
|
* Primary Control in the leg space
|
|
<<sec:primary_control_L>>
|
|
** Introduction :ignore:
|
|
In this section we implement the control architecture shown in Figure [[fig:control_architecture_hac_dvf_pos_L]] consisting of:
|
|
- an inner loop with a decentralized direct velocity feedback control
|
|
- an outer loop where the controller $\bm{K}_\mathcal{L}$ is designed in the frame of the legs
|
|
|
|
#+name: fig:control_architecture_hac_dvf_pos_L
|
|
#+caption: Cascade Control Architecture. The inner loop consist of a decentralized Direct Velocity Feedback. The outer loop consist of position control in the leg's space
|
|
[[file:figs/control_architecture_hac_dvf_pos_L.png]]
|
|
|
|
The controller for decentralized direct velocity feedback is the one designed in Section [[sec:lac_dvf]].
|
|
|
|
** Plant in the task space
|
|
We now loop at the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the design of $\bm{K}_\mathcal{L}$.
|
|
|
|
The diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses are shown in Figure [[fig:opt_stiff_primary_plant_L]].
|
|
|
|
The plant dynamics below $100\ [Hz]$ is only slightly dependent on the payload mass.
|
|
|
|
#+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), freqs, 'Hz'))));
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
title('Diagonal elements of the Plant');
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
for j = 1:6
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz')))));
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-270, 90]);
|
|
yticks([-360:90:360]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_plant_L
|
|
#+caption: Diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_plant_L.png]]
|
|
|
|
** 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
|
|
|
|
The loop gain is shown in Figure [[fig:opt_stiff_primary_loop_gain_L]].
|
|
|
|
#+begin_src matlab
|
|
h = 2.5;
|
|
Kl = 2e7 * eye(6) * ...
|
|
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
|
|
for i = 1:length(Ms)
|
|
isstable(feedback(Gm_l{i}(1,1)*Kl(1,1), 1, -1))
|
|
end
|
|
#+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('Amplitude [m/N]'); 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 :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_loop_gain_L.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_loop_gain_L
|
|
#+caption: Loop gain for the primary plant
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_loop_gain_L.png]]
|
|
|
|
#+begin_src matlab
|
|
load('mat/stages.mat', 'nano_hexapod');
|
|
K = Kl*nano_hexapod.J;
|
|
#+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:
|
|
We compare the norm of these transfer function for the vertical direction when no control is applied and when HAC-LAC control is applied: Figure [[fig:opt_stiff_primary_control_L_senbility_dist]].
|
|
|
|
#+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
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/opt_stiff_primary_control_L_senbility_dist.pdf', 'width', 'full', 'height', 'full')
|
|
#+end_src
|
|
|
|
#+name: fig:opt_stiff_primary_control_L_senbility_dist
|
|
#+caption: Sensibility to disturbances when the HAC-LAC control is applied
|
|
#+RESULTS:
|
|
[[file:figs/opt_stiff_primary_control_L_senbility_dist.png]]
|
|
|
|
*** Noise Budgeting :ignore:
|
|
Then, we load the Power Spectral Density of the perturbations and we look at the obtained PSD of the displacement error in the vertical direction due to the disturbances:
|
|
- Figure [[fig:opt_stiff_primary_control_L_psd_dist]]: Amplitude Spectral Density of the vertical position error due to both the vertical ground motion and the vertical vibrations of the spindle
|
|
- Figure [[fig:opt_stiff_primary_control_L_psd_tot]]: Comparison of the Amplitude Spectral Density of the vertical position error in Open Loop and with the HAC-DVF Control
|
|
- Figure [[fig:opt_stiff_primary_control_L_cas_tot]]: Comparison of the Cumulative Amplitude Spectrum of the vertical position error in Open Loop and with the HAC-DVF Control
|
|
|
|
#+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
|
|
#+begin_src matlab
|
|
initializeDisturbances('Fty_x', false, 'Fty_z', false);
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'all');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('mat/conf_simulink.mat');
|
|
set_param(conf_simulink, 'StopTime', '2');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
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
|
|
save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
|
|
#+end_src
|
|
|
|
** Results
|
|
#+begin_src matlab
|
|
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
n_av = 4;
|
|
han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
t = simout.Em.En.Time;
|
|
Ts = t(2)-t(1);
|
|
|
|
[pxx_ol, f] = pwelch(tomo_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)))
|
|
for i = 1:length(Ms)
|
|
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)))
|
|
for i = 1:length(Ms)
|
|
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)))
|
|
for i = 1:length(Ms)
|
|
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)))
|
|
for i = 1:length(Ms)
|
|
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)))
|
|
for i = 1:length(Ms)
|
|
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)), 'DisplayName', '$\mu$-Station')
|
|
for i = 1:length(Ms)
|
|
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))))))
|
|
for i = 1:length(Ms)
|
|
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))))))
|
|
for i = 1:length(Ms)
|
|
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))))))
|
|
for i = 1:length(Ms)
|
|
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))))))
|
|
for i = 1:length(Ms)
|
|
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))))))
|
|
for i = 1:length(Ms)
|
|
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))))), 'DisplayName', '$\mu$-Station')
|
|
for i = 1:length(Ms)
|
|
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
|
|
for i = 1:length(Ms)
|
|
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
|
|
for i = 1:length(Ms)
|
|
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
|
|
for i = 1:length(Ms)
|
|
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
|
|
for i = 1:length(Ms)
|
|
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
|
|
for i = 1:length(Ms)
|
|
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), ...
|
|
'DisplayName', '$\mu$-Station')
|
|
for i = 1:length(Ms)
|
|
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
|
|
* Primary Control in the task space
|
|
<<sec:primary_control_X>>
|
|
** Introduction :ignore:
|
|
|
|
** Plant in the task space
|
|
Let's look $\bm{G}_\mathcal{X}(s)$.
|
|
|
|
#+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(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(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(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(Gm_x{i}(3, 3), 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', 'southwest');
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 2, 1);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(4, 4), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(5, 5), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
|
|
title('$\mathcal{X}_{R_x}/\mathcal{M}_x$, $\mathcal{X}_{R_y}/\mathcal{M}_y$')
|
|
|
|
ax2 = subplot(2, 2, 2);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(6, 6), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
|
|
title('$\mathcal{X}_{R_z}/\mathcal{M}_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(Gm_x{i}(4, 4), freqs, 'Hz')))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(5, 5), 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(Gm_x{i}(6, 6), 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', 'southwest');
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
#+end_src
|
|
|
|
** Control in the task space
|
|
#+begin_src matlab
|
|
Kx = tf(zeros(6));
|
|
|
|
h = 2.5;
|
|
Kx(1,1) = 3e7 * ...
|
|
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
|
(s/2/pi/1 + 1)/(s/2/pi/1);
|
|
|
|
Kx(2,2) = Kx(1,1);
|
|
|
|
h = 2.5;
|
|
Kx(3,3) = 3e7 * ...
|
|
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
|
(s/2/pi/1 + 1)/(s/2/pi/1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
h = 1.5;
|
|
Kx(4,4) = 5e5 * ...
|
|
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
|
(s/2/pi/1 + 1)/(s/2/pi/1);
|
|
|
|
Kx(5,5) = Kx(4,4);
|
|
|
|
h = 1.5;
|
|
Kx(6,6) = 5e4 * ...
|
|
1/h*(s/(2*pi*30/h) + 1)/(s/(2*pi*30*h) + 1) * ...
|
|
(s/2/pi/1 + 1)/(s/2/pi/1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 2, 1);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
title('Loop Gain $x$ and $y$')
|
|
|
|
ax2 = subplot(2, 2, 2);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
title('Loop Gain $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(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(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(Gm_x{i}(3, 3)*Kx(3,3), 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', 'southwest');
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 2, 1);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(4, 4)*Kx(4,4), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(5, 5)*Kx(5,5), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
|
|
title('$\mathcal{X}_{R_x}/\mathcal{M}_x$, $\mathcal{X}_{R_y}/\mathcal{M}_y$')
|
|
|
|
ax2 = subplot(2, 2, 2);
|
|
hold on;
|
|
for i = 1:length(Ms)
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(6, 6)*Kx(6,6), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [rad/(N m)]'); set(gca, 'XTickLabel',[]);
|
|
title('$\mathcal{X}_{R_z}/\mathcal{M}_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(Gm_x{i}(4, 4)*Kx(4,4), freqs, 'Hz')))));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(5, 5)*Kx(5,5), 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(Gm_x{i}(6, 6)*Kx(6,6), 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', 'southwest');
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
#+end_src
|
|
|
|
*** Stability
|
|
#+begin_src matlab
|
|
for i = 1:length(Ms)
|
|
isstable(feedback(Gm_x{i}*Kx, eye(6), -1))
|
|
end
|
|
#+end_src
|
|
|
|
** Simulation
|