tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Analysis of active damping techniques with simscape model

Browse Source 
Some flexibility is added to the sample
This commit is contained in:
Thomas Dehaeze
2019-10-25 16:02:23 +02:00
parent 9e41241e45
commit 8b13893099
37 changed files with 1953 additions and 491 deletions
Download Patch File Download Diff File Expand all files Collapse all files

706
active_damping/index.html
View File

File diff suppressed because it is too large Load Diff

714
active_damping/index.org
View File

@@ -25,7 +25,7 @@
#+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 matlab/modal_frf_coh.m
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:shell :eval no-export
@@ -41,79 +41,8 @@
#+PROPERTY: header-args:latex+ :output-dir figs
:END:
* Analysis of the nano-hexapod transfer functions :noexport:
** Introduction :ignore:
** 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
** Init
#+begin_src matlab
%% Initialize Ground
initializeGround();
%% Initialize Granite
initializeGranite(struct('rigid', false));
%% Initialize Translation stage
initializeTy(struct('rigid', false));
%% Initialize Tilt Stage
initializeRy(struct('rigid', false));
%% Initialize Spindle
initializeRz(struct('rigid', false));
%% Initialize Hexapod Symétrie
initializeMicroHexapod(struct('rigid', false));
%% Initialize Center of gravity compensation
initializeAxisc();
%% Initialize the mirror
initializeMirror();
%% Initialize the Nano Hexapod
initializeNanoHexapod(struct('actuator', 'piezo'));
%% Initialize the Sample
initializeSample(struct('mass', 50));
#+end_src
#+begin_src matlab
K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
#+end_src
** Identification
#+begin_src matlab
G = identifyPlant();
#+end_src
** Force Sensor
#+begin_src matlab
figure;
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]');
#+end_src
* Introduction :ignore:
First, in section [[sec:undamped]], we will looked at the undamped system.
First, in section [[sec:undamped_system]], we will looked at the undamped system.
Then, we will compare three active damping techniques:
- In section [[sec:iff]]: the integral force feedback is used
@@ -130,7 +59,26 @@ The disturbances are:
- Motion errors of all the stages
* Undamped System
<<sec:undamped>>
:PROPERTIES:
:header-args:matlab+: :tangle matlab/undamped_system.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:undamped_system>>
** ZIP file containing the data and matlab files :ignore:
#+begin_src bash :exports none :results none
if [ matlab/undamped_system.m -nt data/undamped_system.zip ]; then
cp matlab/undamped_system.m undamped_system.m;
zip data/undamped_system \
undamped_system.m
rm undamped_system.m;
fi
#+end_src
#+begin_note
All the files (data and Matlab scripts) are accessible [[file:data/undamped_system.zip][here]].
#+end_note
** Introduction :ignore:
We first look at the undamped system.
The performance of this undamped system will be compared with the damped system using various techniques.
@@ -157,6 +105,7 @@ We initialize all the stages with the default parameters.
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
#+begin_src matlab
initializeInputs();
initializeGround();
initializeGranite();
initializeTy();
@@ -187,6 +136,11 @@ We identify the various transfer functions of the system
G = identifyPlant();
#+end_src
And we save it for further analysis.
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G', '-append');
#+end_src
** Sensitivity to disturbances
The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]].
@@ -225,6 +179,28 @@ The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undam
#+CAPTION: Undamped sensitivity to disturbances ([[./figs/sensitivity_dist_undamped.png][png]], [[./figs/sensitivity_dist_undamped.pdf][pdf]])
[[file:figs/sensitivity_dist_undamped.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_dist_stages.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_dist_stages
#+CAPTION: Sensitivity to force disturbances in various stages ([[./figs/sensitivity_dist_stages.png][png]], [[./figs/sensitivity_dist_stages.pdf][pdf]])
[[file:figs/sensitivity_dist_stages.png]]
** Undamped Plant
The "plant" (transfer function from forces applied by the nano-hexapod to the measured displacement of the sample with respect to the granite) bode plot is shown on figure [[fig:sensitivity_dist_undamped]].
@@ -266,19 +242,37 @@ The "plant" (transfer function from forces applied by the nano-hexapod to the me
#+CAPTION: Transfer Function from cartesian forces to displacement for the undamped plant ([[./figs/plant_undamped.png][png]], [[./figs/plant_undamped.pdf][pdf]])
[[file:figs/plant_undamped.png]]
** Save
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G', '-append');
* Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/iff.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:iff>>
** ZIP file containing the data and matlab files :ignore:
#+begin_src bash :exports none :results none
if [ matlab/iff.m -nt data/iff.zip ]; then
cp matlab/iff.m iff.m;
zip data/iff \
mat/plant.mat \
iff.m
rm iff.m;
fi
#+end_src
* Integral Force Feedback
<<sec:iff>>
#+begin_note
All the files (data and Matlab scripts) are accessible [[file:data/iff.zip][here]].
#+end_note
** Introduction :ignore:
Integral Force Feedback is applied.
In section [[sec:iff_1dof]], IFF is applied on a uni-axial system to understand its behavior.
Then, it is applied on the simscape model.
** One degree-of-freedom example
:PROPERTIES:
:header-args:matlab+: :tangle no
:END:
<<sec:iff_1dof>>
*** Equations
#+begin_src latex :file iff_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
@@ -560,9 +554,10 @@ The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
#+CAPTION: Loop Gain for the Integral Force Feedback ([[./figs/iff_open_loop.png][png]], [[./figs/iff_open_loop.pdf][pdf]])
[[file:figs/iff_open_loop.png]]
** Sensitivity to disturbances
** Identification of the damped plant
Let's initialize the system prior to identification.
#+begin_src matlab
initializeInputs();
initializeGround();
initializeGranite();
initializeTy();
@@ -592,6 +587,12 @@ We identify the system dynamics now that the IFF controller is ON.
G_iff = identifyPlant();
#+end_src
And we save the damped plant for further analysis
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_iff', '-append');
#+end_src
** Sensitivity to disturbances
As shown on figure [[fig:sensitivity_dist_iff]]:
- The top platform of the nano-hexapod how behaves as a "free-mass".
- The transfer function from direct forces $F_s$ to the relative displacement $D$ is equivalent to the one of an isolated mass.
@@ -664,7 +665,7 @@ As shown on figure [[fig:sensitivity_dist_iff]]:
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_iff.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
@@ -777,11 +778,6 @@ However, it increases coupling at low frequency (figure [[fig:plant_iff_coupling
#+CAPTION: Coupling induced by IFF ([[./figs/plant_iff_coupling.png][png]], [[./figs/plant_iff_coupling.pdf][pdf]])
[[file:figs/plant_iff_coupling.png]]
** Save
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_iff', '-append');
#+end_src
** Conclusion
#+begin_important
Integral Force Feedback:
@@ -791,11 +787,34 @@ Integral Force Feedback:
#+end_important
* Relative Motion Control
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rmc.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:rmc>>
** ZIP file containing the data and matlab files :ignore:
#+begin_src bash :exports none :results none
if [ matlab/rmc.m -nt data/rmc.zip ]; then
cp matlab/rmc.m rmc.m;
zip data/rmc \
mat/plant.mat \
rmc.m
rm rmc.m;
fi
#+end_src
#+begin_note
All the files (data and Matlab scripts) are accessible [[file:data/rmc.zip][here]].
#+end_note
** Introduction :ignore:
In the Relative Motion Control (RMC), a derivative feedback is applied between the measured actuator displacement to the actuator force input.
** One degree-of-freedom example
:PROPERTIES:
:header-args:matlab+: :tangle no
:END:
<<sec:rmc_1dof>>
*** Equations
#+begin_src latex :file rmc_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
@@ -1075,9 +1094,10 @@ The obtained loop gains are shown in figure [[fig:rmc_open_loop]].
#+CAPTION: Loop Gain for the Integral Force Feedback ([[./figs/rmc_open_loop.png][png]], [[./figs/rmc_open_loop.pdf][pdf]])
[[file:figs/rmc_open_loop.png]]
** Sensitivity to disturbances
** Identification of the damped plant
Let's initialize the system prior to identification.
#+begin_src matlab
initializeInputs();
initializeGround();
initializeGranite();
initializeTy();
@@ -1107,6 +1127,12 @@ We identify the system dynamics now that the RMC controller is ON.
G_rmc = identifyPlant();
#+end_src
And we save the damped plant for further analysis.
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_rmc', '-append');
#+end_src
** Sensitivity to disturbances
As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering the sensitivity to disturbances near resonance of the system.
#+begin_src matlab :exports none
@@ -1126,7 +1152,7 @@ As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering
plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
legend('location', 'southeast');
subplot(2, 1, 2);
title('$F_s$ to $D$');
@@ -1170,7 +1196,7 @@ As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_rmc.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_rmc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
@@ -1252,11 +1278,6 @@ As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering
#+CAPTION: Damped Plant after RMC is applied ([[./figs/plant_rmc_damped.png][png]], [[./figs/plant_rmc_damped.pdf][pdf]])
[[file:figs/plant_rmc_damped.png]]
** Save
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_rmc', '-append');
#+end_src
** Conclusion
#+begin_important
Relative Motion Control:
@@ -1264,11 +1285,34 @@ Relative Motion Control:
#+end_important
* Direct Velocity Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/dvf.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:dvf>>
** ZIP file containing the data and matlab files :ignore:
#+begin_src bash :exports none :results none
if [ matlab/dvf.m -nt data/dvf.zip ]; then
cp matlab/dvf.m dvf.m;
zip data/dvf \
mat/plant.mat \
dvf.m
rm dvf.m;
fi
#+end_src
#+begin_note
All the files (data and Matlab scripts) are accessible [[file:data/dvf.zip][here]].
#+end_note
** Introduction :ignore:
In the Relative Motion Control (RMC), a feedback is applied between the measured velocity of the platform to the actuator force input.
** One degree-of-freedom example
:PROPERTIES:
:header-args:matlab+: :tangle no
:END:
<<sec:dvf_1dof>>
*** Equations
#+begin_src latex :file dvf_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
@@ -1481,8 +1525,6 @@ Let's load the undamped plant:
Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]).
The plant looks to complicated to be reasonably controlled.
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
@@ -1520,15 +1562,241 @@ The plant looks to complicated to be reasonably controlled.
#+CAPTION: Transfer function from forces applied in the legs to leg velocity sensor ([[./figs/dvf_plant.png][png]], [[./figs/dvf_plant.pdf][pdf]])
[[file:figs/dvf_plant.png]]
The controller is defined below and the obtained loop gain is shown in figure [[fig:dvf_open_loop_gain]].
#+begin_src matlab
K_dvf = tf(3e4);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/dvf_open_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:dvf_open_loop_gain
#+CAPTION: Loop Gain for DVF ([[./figs/dvf_open_loop_gain.png][png]], [[./figs/dvf_open_loop_gain.pdf][pdf]])
[[file:figs/dvf_open_loop_gain.png]]
** Identification of the damped plant
Let's initialize the system prior to identification.
#+begin_src matlab
initializeInputs();
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
#+end_src
And initialize the controllers.
#+begin_src matlab
K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_rmc = tf(zeros(6));
save('./mat/controllers.mat', 'K_rmc', '-append');
K_dvf = -K_dvf*eye(6);
save('./mat/controllers.mat', 'K_dvf', '-append');
#+end_src
We identify the system dynamics now that the RMC controller is ON.
#+begin_src matlab
G_dvf = identifyPlant();
#+end_src
And we save the damped plant for further analysis.
#+begin_src matlab
save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
#+end_src
** Sensitivity to disturbances
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
subplot(2, 1, 1);
title('$D_g$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
subplot(2, 1, 2);
title('$F_s$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_dist_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_dist_dvf
#+CAPTION: Sensitivity to disturbance once the DVF controller is applied to the system ([[./figs/sensitivity_dist_dvf.png][png]], [[./figs/sensitivity_dist_dvf.pdf][pdf]])
[[file:figs/sensitivity_dist_dvf.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_dist_stages_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_dist_stages_dvf
#+CAPTION: Sensitivity to force disturbances in various stages when DVF is applied ([[./figs/sensitivity_dist_stages_dvf.png][png]], [[./figs/sensitivity_dist_stages_dvf.pdf][pdf]])
[[file:figs/sensitivity_dist_stages_dvf.png]]
** Damped Plant
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/plant_dvf_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:plant_dvf_damped
#+CAPTION: Damped Plant after DVF is applied ([[./figs/plant_dvf_damped.png][png]], [[./figs/plant_dvf_damped.pdf][pdf]])
[[file:figs/plant_dvf_damped.png]]
** Conclusion
#+begin_important
Direct Velocity Feedback:
- Not usable
#+end_important
* Comparison
<<sec:comparison>>
** Introduction :ignore:
** 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>>
@@ -1542,36 +1810,254 @@ Direct Velocity Feedback:
cd('../');
#+end_src
** Comparison
** Load the plants
#+begin_src matlab
load('./active_damping/mat/plants.mat', 'G', 'G_iff', 'G_rmc');
load('./active_damping/mat/plants.mat', 'G', 'G_iff', 'G_rmc', 'G_dvf');
#+end_src
** Sensitivity to Disturbance
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$D_{g,z}$ to $D_z$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm( 'Dz', 'Dgz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_comp_ground_motion_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_comp_ground_motion_z
#+CAPTION: caption ([[./figs/sensitivity_comp_ground_motion_z.png][png]], [[./figs/sensitivity_comp_ground_motion_z.pdf][pdf]])
[[file:figs/sensitivity_comp_ground_motion_z.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$F_{s,z}$ to $D_z$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs( 'Dz', 'Fsz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_comp_direct_forces_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_comp_direct_forces_z
#+CAPTION: caption ([[./figs/sensitivity_comp_direct_forces_z.png][png]], [[./figs/sensitivity_comp_direct_forces_z.pdf][pdf]])
[[file:figs/sensitivity_comp_direct_forces_z.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$F_{rz,z}$ to $D_z$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist( 'Dz', 'Frzz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_comp_spindle_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_comp_spindle_z
#+CAPTION: caption ([[./figs/sensitivity_comp_spindle_z.png][png]], [[./figs/sensitivity_comp_spindle_z.pdf][pdf]])
[[file:figs/sensitivity_comp_spindle_z.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$F_{ty,z}$ to $D_z$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist( 'Dz', 'Ftyz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_comp_ty_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_comp_ty_z
#+CAPTION: caption ([[./figs/sensitivity_comp_ty_z.png][png]], [[./figs/sensitivity_comp_ty_z.pdf][pdf]])
[[file:figs/sensitivity_comp_ty_z.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$F_{ty,x}$ to $D_x$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist( 'Dx', 'Ftyx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/sensitivity_comp_ty_x.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:sensitivity_comp_ty_x
#+CAPTION: caption ([[./figs/sensitivity_comp_ty_x.png][png]], [[./figs/sensitivity_comp_ty_x.pdf][pdf]])
[[file:figs/sensitivity_comp_ty_x.png]]
** Damped Plant
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$F_{n,z}$ to $D_z$');
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart( 'Dz', 'Fnz'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'northeast');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart ('Dz', 'Fnz'), freqs, 'Hz'))), 'k-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k:');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k-.');
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
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/plant_comp_damping_z.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:plant_comp_damping_z
#+CAPTION: Plant for the $z$ direction for different active damping technique used ([[./figs/plant_comp_damping_z.png][png]], [[./figs/plant_comp_damping_z.pdf][pdf]])
[[file:figs/plant_comp_damping_z.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
subplot(2, 1, 1);
title('$D_g$ to $D$');
title('$F_{n,z}$ to $D_z$');
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm( 'Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G.G_cart( 'Dx', 'Fnx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'northeast');
subplot(2, 1, 2);
title('$F_s$ to $D$');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs( 'Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'RMC');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart ('Dx', 'Fnx'), freqs, 'Hz'))), 'k-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k:');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k-.');
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
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/plant_comp_damping_x.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:plant_comp_damping_x
#+CAPTION: Plant for the $x$ direction for different active damping technique used ([[./figs/plant_comp_damping_x.png][png]], [[./figs/plant_comp_damping_x.pdf][pdf]])
[[file:figs/plant_comp_damping_x.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
title('$F_{n,x}$ to $R_z$');
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart( 'Rz', 'Fnx'), freqs, 'Hz'))), 'k-' , 'DisplayName', 'Undamped');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k:' , 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'RMC');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k-.', 'DisplayName', 'DVF');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'northeast');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart ('Ry', 'Fnx'), freqs, 'Hz'))), 'k-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k:');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k--');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Fnx'), freqs, 'Hz'))), 'k-.');
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
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/plant_comp_damping_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:plant_comp_damping_coupling
#+CAPTION: Comparison of one off-diagonal plant for different damping technique applied ([[./figs/plant_comp_damping_coupling.png][png]], [[./figs/plant_comp_damping_coupling.pdf][pdf]])
[[file:figs/plant_comp_damping_coupling.png]]
* Conclusion
<<sec:conclusion>>

241
active_damping/matlab/dvf.m Normal file
View File

@@ -0,0 +1,241 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
open 'simscape/sim_nano_station_id.slx'
% Control Design
% Let's load the undamped plant:
load('./active_damping/mat/plants.mat', 'G');
% Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]).
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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');
% #+NAME: fig:dvf_plant
% #+CAPTION: Transfer function from forces applied in the legs to leg velocity sensor ([[./figs/dvf_plant.png][png]], [[./figs/dvf_plant.pdf][pdf]])
% [[file:figs/dvf_plant.png]]
% The controller is defined below and the obtained loop gain is shown in figure [[fig:dvf_open_loop_gain]].
K_dvf = tf(3e4);
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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');
% Identification of the damped plant
% Let's initialize the system prior to identification.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
% And initialize the controllers.
K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_rmc = tf(zeros(6));
save('./mat/controllers.mat', 'K_rmc', '-append');
K_dvf = -K_dvf*eye(6);
save('./mat/controllers.mat', 'K_dvf', '-append');
% We identify the system dynamics now that the RMC controller is ON.
G_dvf = identifyPlant();
% And we save the damped plant for further analysis.
save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
% Sensitivity to disturbances
freqs = logspace(0, 3, 1000);
figure;
subplot(2, 1, 1);
title('$D_g$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
subplot(2, 1, 2);
title('$F_s$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
% #+NAME: fig:sensitivity_dist_dvf
% #+CAPTION: Sensitivity to disturbance once the DVF controller is applied to the system ([[./figs/sensitivity_dist_dvf.png][png]], [[./figs/sensitivity_dist_dvf.pdf][pdf]])
% [[file:figs/sensitivity_dist_dvf.png]]
freqs = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
% Damped Plant
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
linkaxes([ax1,ax2,ax3,ax4],'x');

281
active_damping/matlab/iff.m Normal file
View File

@@ -0,0 +1,281 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
open 'simscape/sim_nano_station_id.slx'
% Control Design
% Let's load the undamped plant:
load('./active_damping/mat/plants.mat', 'G');
% Let's look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure [[fig:iff_plant]]).
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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');
% #+NAME: fig:iff_plant
% #+CAPTION: Transfer function from forces applied in the legs to force sensor ([[./figs/iff_plant.png][png]], [[./figs/iff_plant.pdf][pdf]])
% [[file:figs/iff_plant.png]]
% The controller for each pair of actuator/sensor is:
K_iff = -1000/s;
% The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(K_iff*G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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');
% Identification of the damped plant
% Let's initialize the system prior to identification.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
% All the controllers are set to 0.
K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_iff = -K_iff*eye(6);
save('./mat/controllers.mat', 'K_iff', '-append');
K_rmc = tf(zeros(6));
save('./mat/controllers.mat', 'K_rmc', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');
% We identify the system dynamics now that the IFF controller is ON.
G_iff = identifyPlant();
% And we save the damped plant for further analysis
save('./active_damping/mat/plants.mat', 'G_iff', '-append');
% Sensitivity to disturbances
% As shown on figure [[fig:sensitivity_dist_iff]]:
% - The top platform of the nano-hexapod how behaves as a "free-mass".
% - The transfer function from direct forces $F_s$ to the relative displacement $D$ is equivalent to the one of an isolated mass.
% - The transfer function from ground motion $D_g$ to the relative displacement $D$ tends to the transfer function from $D_g$ to the displacement of the granite (the sample is being isolated thanks to IFF).
% However, as the goal is to make the relative displacement $D$ as small as possible (e.g. to make the sample motion follows the granite motion), this is not a good thing.
freqs = logspace(0, 3, 1000);
figure;
subplot(2, 1, 1);
title('$D_g$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
subplot(2, 1, 2);
title('$F_s$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
% #+NAME: fig:sensitivity_dist_iff
% #+CAPTION: Sensitivity to disturbance once the IFF controller is applied to the system ([[./figs/sensitivity_dist_iff.png][png]], [[./figs/sensitivity_dist_iff.pdf][pdf]])
% [[file:figs/sensitivity_dist_iff.png]]
% #+begin_warning
% The order of the models are very high and thus the plots may be wrong.
% For instance, the plots are not the same when using =minreal=.
% #+end_warning
freqs = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(minreal(prescale(G_iff.G_dist('Dz', 'Frzz'), {2*pi, 2*pi*1e3})), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dz', 'Ftyz')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dx', 'Ftyx')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
% Damped Plant
% Now, look at the new damped plant to control.
% It damps the plant (resonance of the nano hexapod as well as other resonances) as shown in figure [[fig:plant_iff_damped]].
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
% #+NAME: fig:plant_iff_damped
% #+CAPTION: Damped Plant after IFF is applied ([[./figs/plant_iff_damped.png][png]], [[./figs/plant_iff_damped.pdf][pdf]])
% [[file:figs/plant_iff_damped.png]]
% However, it increases coupling at low frequency (figure [[fig:plant_iff_coupling]]).
freqs = logspace(0, 3, 1000);
figure;
for ix = 1:6
for iy = 1:6
subplot(6, 6, (ix-1)*6 + iy);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart(ix, iy), freqs, 'Hz'))), 'k-');
plot(freqs, abs(squeeze(freqresp(G_iff.G_cart(ix, iy), freqs, 'Hz'))), 'k--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-12, 1e-5]);
end
end

246
active_damping/matlab/rmc.m Normal file
View File

@@ -0,0 +1,246 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
open 'simscape/sim_nano_station_id.slx'
% Control Design
% Let's load the undamped plant:
load('./active_damping/mat/plants.mat', 'G');
% Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure [[fig:rmc_plant]]).
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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');
% #+NAME: fig:rmc_plant
% #+CAPTION: Transfer function from forces applied in the legs to leg displacement sensor ([[./figs/rmc_plant.png][png]], [[./figs/rmc_plant.pdf][pdf]])
% [[file:figs/rmc_plant.png]]
% The Relative Motion Controller is defined below.
% A Low pass Filter is added to make the controller transfer function proper.
K_rmc = s*50000/(1 + s/2/pi/10000);
% The obtained loop gains are shown in figure [[fig:rmc_open_loop]].
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i=1:6
plot(freqs, abs(squeeze(freqresp(K_rmc*G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(K_rmc*G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
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');
% Identification of the damped plant
% Let's initialize the system prior to identification.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
% And initialize the controllers.
K = tf(zeros(6));
save('./mat/controllers.mat', 'K', '-append');
K_iff = tf(zeros(6));
save('./mat/controllers.mat', 'K_iff', '-append');
K_rmc = -K_rmc*eye(6);
save('./mat/controllers.mat', 'K_rmc', '-append');
K_dvf = tf(zeros(6));
save('./mat/controllers.mat', 'K_dvf', '-append');
% We identify the system dynamics now that the RMC controller is ON.
G_rmc = identifyPlant();
% And we save the damped plant for further analysis.
save('./active_damping/mat/plants.mat', 'G_rmc', '-append');
% Sensitivity to disturbances
% As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering the sensitivity to disturbances near resonance of the system.
freqs = logspace(0, 3, 1000);
figure;
subplot(2, 1, 1);
title('$D_g$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
subplot(2, 1, 2);
title('$F_s$ to $D$');
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
% #+NAME: fig:sensitivity_dist_rmc
% #+CAPTION: Sensitivity to disturbance once the RMC controller is applied to the system ([[./figs/sensitivity_dist_rmc.png][png]], [[./figs/sensitivity_dist_rmc.pdf][pdf]])
% [[file:figs/sensitivity_dist_rmc.png]]
freqs = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
% Damped Plant
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
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]);
legend('location', 'northwest');
linkaxes([ax1,ax2,ax3,ax4],'x');