3141 lines
114 KiB
Org Mode
3141 lines
114 KiB
Org Mode
#+TITLE: Active Damping applied on the Simscape Model
|
|
:DRAWER:
|
|
#+STARTUP: overview
|
|
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ../index.html
|
|
#+HTML_LINK_UP: ../index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
|
|
|
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
|
|
|
#+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/}{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 raw replace :buffer no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports both
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
:END:
|
|
|
|
* Introduction :ignore:
|
|
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
|
|
- In section [[sec:dvf]]: the direct velocity feedback is used
|
|
- In section [[sec:ine]]: inertial control is used
|
|
|
|
For each of the active damping technique, we will:
|
|
- Look at the damped plant
|
|
- Simulate tomography experiments
|
|
- Compare the sensitivity from disturbances
|
|
|
|
The disturbances are:
|
|
- Ground motion
|
|
- Motion errors of all the stages
|
|
|
|
* Undamped System
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/undamped_system.m
|
|
:header-args:matlab+: :comments none :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.
|
|
|
|
** 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
|
|
addpath('active_damping/src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
|
#+end_src
|
|
|
|
** Identification of the dynamics for Active Damping
|
|
*** Initialize the Simulation
|
|
We initialize all the stages with the default parameters.
|
|
#+begin_src matlab
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeAxisc();
|
|
initializeMirror();
|
|
#+end_src
|
|
|
|
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
|
|
#+begin_src matlab
|
|
initializeNanoHexapod('actuator', 'piezo');
|
|
initializeSample('mass', 50);
|
|
#+end_src
|
|
|
|
We set the references to zero.
|
|
#+begin_src matlab
|
|
initializeReferences();
|
|
#+end_src
|
|
|
|
And all the controllers are set to 0.
|
|
#+begin_src matlab
|
|
K = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K', '-append');
|
|
K_ine = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
|
K_iff = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
K_dvf = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
|
#+end_src
|
|
|
|
*** Identification
|
|
First, we identify the dynamics of the system using the =linearize= function.
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vlm'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
|
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
|
|
'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'};
|
|
#+end_src
|
|
|
|
We then create transfer functions corresponding to the active damping plants.
|
|
#+begin_src matlab
|
|
G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
|
G_dvf = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
|
G_ine = minreal(G({'Vnlm1', 'Vnlm2', 'Vnlm3', 'Vnlm4', 'Vnlm5', 'Vnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
|
#+end_src
|
|
|
|
And we save them for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/undamped_plants.mat', 'G_iff', 'G_dvf', 'G_ine');
|
|
#+end_src
|
|
|
|
*** Obtained Plants for Active Damping
|
|
#+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(G_iff(['Fnlm', num2str(i)], ['Fnl', 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_iff(['Fnlm', num2str(i)], ['Fnl', 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/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_active_damping_iff_plant
|
|
#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]])
|
|
[[file:figs/nass_active_damping_iff_plant.png]]
|
|
|
|
#+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(G_dvf(['Dnlm', num2str(i)], ['Fnl', 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_dvf(['Dnlm', num2str(i)], ['Fnl', 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/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_active_damping_dvf_plant
|
|
#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]])
|
|
[[file:figs/nass_active_damping_ine_plant.png]]
|
|
|
|
#+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(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', 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/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_active_damping_inertial_plant
|
|
#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]])
|
|
[[file:figs/nass_active_damping_inertial_plant.png]]
|
|
|
|
** Tomography Experiment
|
|
*** Simulation
|
|
We initialize elements for the tomography experiment.
|
|
#+begin_src matlab
|
|
prepareTomographyExperiment();
|
|
#+end_src
|
|
|
|
We change the simulation stop time.
|
|
#+begin_src matlab
|
|
load('mat/conf_simscape.mat');
|
|
set_param(conf_simscape, 'StopTime', '3');
|
|
#+end_src
|
|
|
|
And we simulate the system.
|
|
#+begin_src matlab
|
|
sim('sim_nass_active_damping');
|
|
#+end_src
|
|
|
|
Finally, we save the simulation results for further analysis
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/tomo_exp.mat', 'En', 'Eg', '-append');
|
|
#+end_src
|
|
|
|
*** Results
|
|
We load the results of tomography experiments.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/tomo_exp.mat', 'En');
|
|
t = linspace(0, 3, length(En(:,1)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
|
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
|
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
|
hold off;
|
|
legend();
|
|
xlabel('Time [s]'); ylabel('Position Error [m]');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_undamped_sim_tomo_trans
|
|
#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_undamped_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_trans.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_undamped_sim_tomo_trans.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
|
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
|
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
|
hold off;
|
|
xlim([0.5,inf]);
|
|
legend();
|
|
xlabel('Time [s]'); ylabel('Position Error [rad]');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_undamped_sim_tomo_rot.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_undamped_sim_tomo_rot
|
|
#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_undamped_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_rot.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_undamped_sim_tomo_rot.png]]
|
|
|
|
* Integral Force Feedback
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/iff.m
|
|
:header-args:matlab+: :comments none :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
|
|
|
|
#+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 on the simscape model.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
simulinkproject('../');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
addpath('active_damping/src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
|
#+end_src
|
|
|
|
** Control Design
|
|
*** Plant
|
|
Let's load the previously indentified undamped plant:
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/undamped_plants.mat', 'G_iff');
|
|
#+end_src
|
|
|
|
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]]).
|
|
|
|
#+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(G_iff(['Fnlm', num2str(i)], ['Fnl', 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_iff(['Fnlm', num2str(i)], ['Fnl', 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/iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+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]]
|
|
|
|
*** Control Design
|
|
The controller for each pair of actuator/sensor is:
|
|
#+begin_src matlab
|
|
K_iff = 1000/s;
|
|
#+end_src
|
|
|
|
The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
|
|
|
|
#+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_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', 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_iff(['Fnlm', num2str(i)], ['Fnl', 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/iff_open_loop.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: 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]]
|
|
|
|
*** Diagonal Controller
|
|
We create the diagonal controller and we add a minus sign as we have a positive
|
|
feedback architecture.
|
|
#+begin_src matlab
|
|
K_iff = -K_iff*eye(6);
|
|
#+end_src
|
|
|
|
We save the controller for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/K_iff.mat', 'K_iff');
|
|
#+end_src
|
|
|
|
*** IFF with High Pass Filter
|
|
#+begin_src matlab
|
|
w_hpf = 2*pi*10; % Cut-off frequency for the high pass filter [rad/s]
|
|
w_lpf = 2*pi*200; % Cut-off frequency for the low pass filter [rad/s]
|
|
|
|
K_iff = 2*pi*200/s * (s/w_hpf)/(s/w_hpf + 1) * 1/(s/w_lpf + 1);
|
|
#+end_src
|
|
|
|
The corresponding loop gains are shown in figure [[fig:iff_hpf_open_loop]].
|
|
#+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_iff*G_iff(['Fnlm', num2str(i)], ['Fnl', 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_iff(['Fnlm', num2str(i)], ['Fnl', 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/iff_hpf_open_loop.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:iff_hpf_open_loop
|
|
#+CAPTION: Loop Gain for the Integral Force Feedback with an High pass filter ([[./figs/iff_hpf_open_loop.png][png]], [[./figs/iff_hpf_open_loop.pdf][pdf]])
|
|
[[file:figs/iff_hpf_open_loop.png]]
|
|
|
|
We create the diagonal controller and we add a minus sign as we have a positive
|
|
feedback architecture.
|
|
#+begin_src matlab
|
|
K_iff = -K_iff*eye(6);
|
|
#+end_src
|
|
|
|
We save the controller for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/K_iff_hpf.mat', 'K_iff');
|
|
#+end_src
|
|
|
|
** TODO Identification of the damped plant :noexport:
|
|
*** Initialize the Simulation
|
|
We initialize all the stages with the default parameters.
|
|
#+begin_src matlab
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeAxisc();
|
|
initializeMirror();
|
|
#+end_src
|
|
|
|
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
|
|
#+begin_src matlab
|
|
initializeNanoHexapod('actuator', 'piezo');
|
|
initializeSample('mass', 50);
|
|
#+end_src
|
|
|
|
We set the references to zero.
|
|
#+begin_src matlab
|
|
initializeReferences();
|
|
#+end_src
|
|
|
|
And all the controllers are set to 0 except for the IFF.
|
|
#+begin_src matlab
|
|
K = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K', '-append');
|
|
K_ine = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
|
K_iff = K_iff;
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
K_dvf = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
|
#+end_src
|
|
|
|
*** Identification
|
|
First, we identify the dynamics of the system using the =linearize= function.
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
|
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
|
|
#+end_src
|
|
|
|
We then create transfer functions corresponding to the active damping plants.
|
|
#+begin_src matlab
|
|
G_iff = minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
|
% G_rmc = minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}));
|
|
#+end_src
|
|
|
|
And we save them for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/plants.mat', 'G_iff', '-append');
|
|
#+end_src
|
|
|
|
*** TODO 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.
|
|
|
|
#+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_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');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/sensitivity_dist_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+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
|
|
|
|
#+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(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');
|
|
#+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-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:sensitivity_dist_stages_iff
|
|
#+CAPTION: Sensitivity to force disturbances in various stages when IFF is applied ([[./figs/sensitivity_dist_stages_iff.png][png]], [[./figs/sensitivity_dist_stages_iff.pdf][pdf]])
|
|
[[file:figs/sensitivity_dist_stages_iff.png]]
|
|
|
|
*** TODO 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]].
|
|
|
|
#+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_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');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/plant_iff_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+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]]).
|
|
#+begin_src matlab :exports none
|
|
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
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/plant_iff_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: 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]]
|
|
|
|
** Tomography Experiment
|
|
*** Simulation with IFF Controller
|
|
We initialize elements for the tomography experiment.
|
|
#+begin_src matlab
|
|
prepareTomographyExperiment();
|
|
#+end_src
|
|
|
|
We set the IFF controller.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/K_iff.mat', 'K_iff');
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
#+end_src
|
|
|
|
We change the simulation stop time.
|
|
#+begin_src matlab
|
|
load('mat/conf_simscape.mat');
|
|
set_param(conf_simscape, 'StopTime', '3');
|
|
#+end_src
|
|
|
|
And we simulate the system.
|
|
#+begin_src matlab
|
|
sim('sim_nass_active_damping');
|
|
#+end_src
|
|
|
|
Finally, we save the simulation results for further analysis
|
|
#+begin_src matlab
|
|
En_iff = En;
|
|
Eg_iff = Eg;
|
|
save('./active_damping/mat/tomo_exp.mat', 'En_iff', 'Eg_iff', '-append');
|
|
#+end_src
|
|
|
|
*** Simulation with IFF Controller with added High Pass Filter
|
|
We initialize elements for the tomography experiment.
|
|
#+begin_src matlab
|
|
prepareTomographyExperiment();
|
|
#+end_src
|
|
|
|
We set the IFF controller with the High Pass Filter.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/K_iff_hpf.mat', 'K_iff');
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
#+end_src
|
|
|
|
We change the simulation stop time.
|
|
#+begin_src matlab
|
|
load('mat/conf_simscape.mat');
|
|
set_param(conf_simscape, 'StopTime', '3');
|
|
#+end_src
|
|
|
|
And we simulate the system.
|
|
#+begin_src matlab
|
|
sim('sim_nass_active_damping');
|
|
#+end_src
|
|
|
|
Finally, we save the simulation results for further analysis
|
|
#+begin_src matlab
|
|
En_iff_hpf = En;
|
|
Eg_iff_hpf = Eg;
|
|
save('./active_damping/mat/tomo_exp.mat', 'En_iff_hpf', 'Eg_iff_hpf', '-append');
|
|
#+end_src
|
|
|
|
*** Compare with Undamped system
|
|
We load the results of tomography experiments.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_iff', 'En_iff_hpf');
|
|
t = linspace(0, 3, length(En(:,1)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(En(:,1), En(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL')
|
|
plot(En_iff(:,1), En_iff(:,2), 'DisplayName', '$\epsilon_{x,y}$ - IFF')
|
|
plot(En_iff_hpf(:,1), En_iff_hpf(:,2), 'DisplayName', '$\epsilon_{x,y}$ - IFF + HPF')
|
|
xlabel('X Motion [m]'); ylabel('Y Motion [m]');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_iff_sim_tomo_xy.pdf" :var figsize="small-normal" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_iff_sim_tomo_xy
|
|
#+CAPTION: Position Error during tomography experiment - XY Motion ([[./figs/nass_act_damp_iff_sim_tomo_xy.png][png]], [[./figs/nass_act_damp_iff_sim_tomo_xy.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_iff_sim_tomo_xy.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
|
plot(t, En_iff(:,1), 'DisplayName', '$\epsilon_{x}$ - IFF')
|
|
plot(t, En_iff_hpf(:,1), 'DisplayName', '$\epsilon_{x}$ - IFF + HPF')
|
|
legend('location', 'southwest');
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
|
plot(t, En_iff(:,2), 'DisplayName', '$\epsilon_{y}$ - IFF')
|
|
plot(t, En_iff_hpf(:,2), 'DisplayName', '$\epsilon_{y}$ - IFF + HPF')
|
|
legend('location', 'southwest');
|
|
ylabel('Position Error [m]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
|
plot(t, En_iff(:,3), 'DisplayName', '$\epsilon_{z}$ - IFF')
|
|
plot(t, En_iff_hpf(:,3), 'DisplayName', '$\epsilon_{z}$ - IFF + HPF')
|
|
legend('location', 'northwest');
|
|
xlabel('Time [s]');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_iff_sim_tomo_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_iff_sim_tomo_trans
|
|
#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_iff_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_iff_sim_tomo_trans.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_iff_sim_tomo_trans.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
|
plot(t, En_iff(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - IFF')
|
|
plot(t, En_iff_hpf(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - IFF + HPF')
|
|
legend('location', 'northwest');
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
|
plot(t, En_iff(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - IFF')
|
|
plot(t, En_iff_hpf(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - IFF + HPF')
|
|
legend('location', 'southwest');
|
|
ylabel('Position Error [rad]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
|
plot(t, En_iff(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - IFF')
|
|
plot(t, En_iff_hpf(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - IFF + HPF')
|
|
legend();
|
|
xlabel('Time [s]');
|
|
|
|
linkaxes([ax1,ax2,ax3],'x');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_iff_sim_tomo_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_iff_sim_tomo_rot
|
|
#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_iff_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_iff_sim_tomo_rot.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_iff_sim_tomo_rot.png]]
|
|
|
|
** Conclusion
|
|
#+begin_important
|
|
Integral Force Feedback:
|
|
- Robust (guaranteed stability)
|
|
- Acceptable Damping
|
|
- Increase the sensitivity to disturbances at low frequencies
|
|
#+end_important
|
|
|
|
* Direct Velocity Feedback
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dvf.m
|
|
:header-args:matlab+: :comments none :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 Direct Velocity Feedback (DVF), a derivative feedback is applied between the measured actuator displacement to the actuator force input.
|
|
The actuator displacement can be measured with a capacitive sensor for instance.
|
|
|
|
** 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
|
|
addpath('active_damping/src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
|
#+end_src
|
|
|
|
** Control Design
|
|
*** Plant
|
|
Let's load the undamped plant:
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/undamped_plants.mat', 'G_dvf');
|
|
#+end_src
|
|
|
|
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:dvf_plant]]).
|
|
|
|
#+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(G_dvf(['Dnlm', num2str(i)], ['Fnl', 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_dvf(['Dnlm', num2str(i)], ['Fnl', 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_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:dvf_plant
|
|
#+CAPTION: Transfer function from forces applied in the legs to leg displacement sensor ([[./figs/dvf_plant.png][png]], [[./figs/dvf_plant.pdf][pdf]])
|
|
[[file:figs/dvf_plant.png]]
|
|
|
|
*** Control Design
|
|
The Direct Velocity Feedback is defined below.
|
|
A Low pass Filter is added to make the controller transfer function proper.
|
|
#+begin_src matlab
|
|
K_dvf = s*20000/(1 + s/2/pi/10000);
|
|
#+end_src
|
|
|
|
The obtained loop gains are shown in figure [[fig:dvf_open_loop]].
|
|
|
|
#+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_dvf(['Dnlm', num2str(i)], ['Fnl', 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_dvf(['Dnlm', num2str(i)], ['Fnl', 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.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:dvf_open_loop
|
|
#+CAPTION: Loop Gain for the Integral Force Feedback ([[./figs/dvf_open_loop.png][png]], [[./figs/dvf_open_loop.pdf][pdf]])
|
|
[[file:figs/dvf_open_loop.png]]
|
|
|
|
*** Diagonal Controller
|
|
We create the diagonal controller and we add a minus sign as we have a positive feedback architecture.
|
|
#+begin_src matlab
|
|
K_dvf = -K_dvf*eye(6);
|
|
#+end_src
|
|
|
|
We save the controller for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
|
#+end_src
|
|
|
|
** TODO Identification of the damped plant :noexport:
|
|
*** Initialize the Simulation
|
|
Let's initialize the system prior to identification.
|
|
#+begin_src matlab
|
|
initializeReferences();
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeAxisc();
|
|
initializeMirror();
|
|
initializeNanoHexapod('actuator', 'piezo');
|
|
initializeSample('mass', 50);
|
|
#+end_src
|
|
|
|
And initialize the controllers.
|
|
#+begin_src matlab
|
|
K = tf(zeros(6));
|
|
K_ine = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
|
save('./mat/controllers.mat', 'K', '-append');
|
|
K_iff = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
K_dvf = K_dvf;
|
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
|
#+end_src
|
|
|
|
*** Identification
|
|
We identify the system dynamics now that the DVF controller is ON.
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
|
|
'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
|
|
#+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
|
|
As shown in figure [[fig:sensitivity_dist_dvf]], DVF control succeed in lowering the sensitivity to disturbances near resonance of the system.
|
|
|
|
#+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', 'southeast');
|
|
|
|
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]]
|
|
|
|
** Tomography Experiment
|
|
*** Initialize the Simulation
|
|
We initialize elements for the tomography experiment.
|
|
#+begin_src matlab
|
|
prepareTomographyExperiment();
|
|
#+end_src
|
|
|
|
We set the DVF controller.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/K_dvf.mat', 'K_dvf');
|
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
|
#+end_src
|
|
|
|
*** Simulation
|
|
We change the simulation stop time.
|
|
#+begin_src matlab
|
|
load('mat/conf_simscape.mat');
|
|
set_param(conf_simscape, 'StopTime', '3');
|
|
#+end_src
|
|
|
|
And we simulate the system.
|
|
#+begin_src matlab
|
|
sim('sim_nass_active_damping');
|
|
#+end_src
|
|
|
|
Finally, we save the simulation results for further analysis
|
|
#+begin_src matlab
|
|
En_dvf = En;
|
|
Eg_dvf = Eg;
|
|
save('./active_damping/mat/tomo_exp.mat', 'En_dvf', 'Eg_dvf', '-append');
|
|
#+end_src
|
|
|
|
*** Compare with Undamped system
|
|
We load the results of tomography experiments.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_dvf');
|
|
t = linspace(0, 3, length(En(:,1)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(En(:,1), En(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL')
|
|
plot(En_dvf(:,1), En_dvf(:,2), 'DisplayName', '$\epsilon_{x,y}$ - DVF')
|
|
xlabel('X Motion [m]'); ylabel('Y Motion [m]');
|
|
legend();
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_dvf_sim_tomo_xy.pdf" :var figsize="small-normal" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_dvf_sim_tomo_xy
|
|
#+CAPTION: Position Error during tomography experiment - XY Motion ([[./figs/nass_act_damp_dvf_sim_tomo_xy.png][png]], [[./figs/nass_act_damp_dvf_sim_tomo_xy.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_dvf_sim_tomo_xy.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
|
plot(t, En_dvf(:,1), 'DisplayName', '$\epsilon_{x}$ - DVF')
|
|
legend();
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
|
plot(t, En_dvf(:,2), 'DisplayName', '$\epsilon_{y}$ - DVF')
|
|
legend();
|
|
ylabel('Position Error [m]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
|
plot(t, En_dvf(:,3), 'DisplayName', '$\epsilon_{z}$ - DVF')
|
|
legend();
|
|
xlabel('Time [s]');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_dvf_sim_tomo_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_dvf_sim_tomo_trans
|
|
#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_dvf_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_dvf_sim_tomo_trans.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_dvf_sim_tomo_trans.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
|
plot(t, En_dvf(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - DVF')
|
|
legend();
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
|
plot(t, En_dvf(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - DVF')
|
|
legend();
|
|
ylabel('Position Error [rad]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
|
plot(t, En_dvf(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - DVF')
|
|
legend();
|
|
xlabel('Time [s]');
|
|
|
|
linkaxes([ax1,ax2,ax3],'x');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_dvf_sim_tomo_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_dvf_sim_tomo_rot
|
|
#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_dvf_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_dvf_sim_tomo_rot.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_dvf_sim_tomo_rot.png]]
|
|
|
|
** Conclusion
|
|
#+begin_important
|
|
Direct Velocity Feedback:
|
|
-
|
|
#+end_important
|
|
|
|
* Inertial Control
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/ine.m
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
|
:END:
|
|
<<sec:ine>>
|
|
|
|
** ZIP file containing the data and matlab files :ignore:
|
|
#+begin_src bash :exports none :results none
|
|
if [ matlab/ine.m -nt data/ine.zip ]; then
|
|
cp matlab/ine.m ine.m;
|
|
zip data/ine \
|
|
mat/plant.mat \
|
|
ine.m
|
|
rm ine.m;
|
|
fi
|
|
#+end_src
|
|
|
|
#+begin_note
|
|
All the files (data and Matlab scripts) are accessible [[file:data/ine.zip][here]].
|
|
#+end_note
|
|
|
|
** Introduction :ignore:
|
|
In Inertial Control, a feedback is applied between the measured *absolute* motion (velocity or acceleration) of the platform to the actuator force input.
|
|
|
|
** 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
|
|
addpath('active_damping/src/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
open('active_damping/matlab/sim_nass_active_damping.slx')
|
|
#+end_src
|
|
|
|
** Control Design
|
|
*** Plant
|
|
Let's load the undamped plant:
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/undamped_plants.mat', 'G_ine');
|
|
#+end_src
|
|
|
|
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:ine_plant]]).
|
|
|
|
#+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(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [$\frac{m/s^2}{N}$]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
for i=1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', 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/ine_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:ine_plant
|
|
#+CAPTION: Transfer function from forces applied in the legs to leg velocity sensor ([[./figs/ine_plant.png][png]], [[./figs/ine_plant.pdf][pdf]])
|
|
[[file:figs/ine_plant.png]]
|
|
|
|
*** Control Design
|
|
The controller is defined below and the obtained loop gain is shown in figure [[fig:ine_open_loop_gain]].
|
|
|
|
#+begin_src matlab
|
|
K_ine = 1e4/(1+s/(2*pi*100));
|
|
#+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_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', 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_ine*G_ine(['Vnlm', num2str(i)], ['Fnl', 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/ine_open_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:ine_open_loop_gain
|
|
#+CAPTION: Loop Gain for Inertial Control ([[./figs/ine_open_loop_gain.png][png]], [[./figs/ine_open_loop_gain.pdf][pdf]])
|
|
[[file:figs/ine_open_loop_gain.png]]
|
|
|
|
*** Diagonal Controller
|
|
We create the diagonal controller and we add a minus sign as we have a positive feedback architecture.
|
|
#+begin_src matlab
|
|
K_ine = -K_ine*eye(6);
|
|
#+end_src
|
|
|
|
We save the controller for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/K_ine.mat', 'K_ine');
|
|
#+end_src
|
|
|
|
** TODO Identification of the damped plant :noexport:
|
|
*** Initialize the Simulation
|
|
Let's initialize the system prior to identification.
|
|
#+begin_src matlab
|
|
initializeReferences();
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeAxisc();
|
|
initializeMirror();
|
|
initializeNanoHexapod('actuator', 'piezo');
|
|
initializeSample('mass', 50);
|
|
#+end_src
|
|
|
|
And initialize the controllers.
|
|
#+begin_src matlab
|
|
K = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K', '-append');
|
|
K_ine = -K_ine*eye(6);
|
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
|
K_iff = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
K_dvf = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
|
#+end_src
|
|
|
|
*** Identification
|
|
We identify the system dynamics now that the Inertial controller is ON.
|
|
#+begin_src matlab
|
|
G_ine = identifyPlant();
|
|
#+end_src
|
|
|
|
And we save the damped plant for further analysis.
|
|
#+begin_src matlab
|
|
save('./active_damping/mat/plants.mat', 'G_ine', '-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_ine.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.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_ine.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.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_ine.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:sensitivity_dist_ine
|
|
#+CAPTION: Sensitivity to disturbance once the INE controller is applied to the system ([[./figs/sensitivity_dist_ine.png][png]], [[./figs/sensitivity_dist_ine.pdf][pdf]])
|
|
[[file:figs/sensitivity_dist_ine.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_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.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_ine.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:sensitivity_dist_stages_ine
|
|
#+CAPTION: Sensitivity to force disturbances in various stages when INE is applied ([[./figs/sensitivity_dist_stages_ine.png][png]], [[./figs/sensitivity_dist_stages_ine.pdf][pdf]])
|
|
[[file:figs/sensitivity_dist_stages_ine.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_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.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_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
|
|
plot(freqs, abs(squeeze(freqresp(G_ine.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_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.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_ine.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine.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_ine_damped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:plant_ine_damped
|
|
#+CAPTION: Damped Plant after INE is applied ([[./figs/plant_ine_damped.png][png]], [[./figs/plant_ine_damped.pdf][pdf]])
|
|
[[file:figs/plant_ine_damped.png]]
|
|
|
|
** Tomography Experiment
|
|
*** Initialize the Simulation
|
|
We initialize elements for the tomography experiment.
|
|
#+begin_src matlab
|
|
prepareTomographyExperiment();
|
|
#+end_src
|
|
|
|
We set the Inertial controller.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/K_ine.mat', 'K_ine');
|
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
|
#+end_src
|
|
|
|
*** Simulation
|
|
We change the simulation stop time.
|
|
#+begin_src matlab
|
|
load('mat/conf_simscape.mat');
|
|
set_param(conf_simscape, 'StopTime', '3');
|
|
#+end_src
|
|
|
|
And we simulate the system.
|
|
#+begin_src matlab
|
|
sim('sim_nass_active_damping');
|
|
#+end_src
|
|
|
|
Finally, we save the simulation results for further analysis
|
|
#+begin_src matlab
|
|
En_ine = En;
|
|
Eg_ine = Eg;
|
|
save('./active_damping/mat/tomo_exp.mat', 'En_ine', 'Eg_ine', '-append');
|
|
#+end_src
|
|
|
|
*** Compare with Undamped system
|
|
We load the results of tomography experiments.
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_ine');
|
|
t = linspace(0, 3, length(En_ine(:,1)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(En(:,1), En(:,2), 'DisplayName', '$\epsilon_{x,y}$ - OL')
|
|
plot(En_ine(:,1), En_ine(:,2), 'DisplayName', '$\epsilon_{x,y}$ - Inertial')
|
|
xlabel('X Motion [m]'); ylabel('Y Motion [m]');
|
|
legend();
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_ine_sim_tomo_xy.pdf" :var figsize="small-normal" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_ine_sim_tomo_xy
|
|
#+CAPTION: Position Error during tomography experiment - XY Motion ([[./figs/nass_act_damp_ine_sim_tomo_xy.png][png]], [[./figs/nass_act_damp_ine_sim_tomo_xy.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_ine_sim_tomo_xy.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
|
|
plot(t, En_ine(:,1), 'DisplayName', '$\epsilon_{x}$ - Inertial')
|
|
legend();
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
|
|
plot(t, En_ine(:,2), 'DisplayName', '$\epsilon_{y}$ - Inertial')
|
|
legend();
|
|
ylabel('Position Error [m]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
|
|
plot(t, En_ine(:,3), 'DisplayName', '$\epsilon_{z}$ - Inertial')
|
|
legend();
|
|
xlabel('Time [s]');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_ine_sim_tomo_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_ine_sim_tomo_trans
|
|
#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_ine_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_ine_sim_tomo_trans.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_ine_sim_tomo_trans.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
ax1 = subplot(3, 1, 1);
|
|
hold on;
|
|
plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
|
|
plot(t, En_ine(:,4), 'DisplayName', '$\epsilon_{\theta_x}$ - Inertial')
|
|
legend();
|
|
|
|
ax2 = subplot(3, 1, 2);
|
|
hold on;
|
|
plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
|
|
plot(t, En_ine(:,5), 'DisplayName', '$\epsilon_{\theta_y}$ - Inertial')
|
|
legend();
|
|
ylabel('Position Error [rad]');
|
|
|
|
ax3 = subplot(3, 1, 3);
|
|
hold on;
|
|
plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
|
|
plot(t, En_ine(:,6), 'DisplayName', '$\epsilon_{\theta_z}$ - Inertial')
|
|
legend();
|
|
xlabel('Time [s]');
|
|
|
|
linkaxes([ax1,ax2,ax3],'x');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/nass_act_damp_ine_sim_tomo_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:nass_act_damp_ine_sim_tomo_rot
|
|
#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_ine_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_ine_sim_tomo_rot.pdf][pdf]])
|
|
[[file:figs/nass_act_damp_ine_sim_tomo_rot.png]]
|
|
|
|
** Conclusion
|
|
#+begin_important
|
|
Inertial Control:
|
|
#+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>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
cd('../');
|
|
#+end_src
|
|
|
|
** Load the plants
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/plants.mat', 'G', 'G_iff', 'G_ine', '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_ine.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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: Sensitivity to ground motion in the Z direction on the Z motion error ([[./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_ine.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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: Compliance in the Z direction: Sensitivity of direct forces applied on the sample in the Z direction on the Z motion error ([[./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_ine.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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: Sensitivity to forces applied in the Z direction by the Spindle on the Z motion error ([[./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_ine.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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: Sensitivity to forces applied in the Z direction by the Y translation stage on the Z motion error ([[./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_ine.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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: Sensitivity to forces applied in the X direction by the Y translation stage on the X motion error ([[./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_ine.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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_ine.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;
|
|
|
|
title('$F_{n,z}$ to $D_z$');
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
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_ine.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
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_ine.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_ine.G_cart('Rz', 'Fnx'), freqs, 'Hz'))), 'k--', 'DisplayName', 'INE');
|
|
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_ine.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]]
|
|
|
|
** Tomography Experiment
|
|
*** Load the Simulation Data
|
|
#+begin_src matlab
|
|
load('./active_damping/mat/tomo_exp.mat', 'En', 'En_iff_hpf', 'En_dvf', 'En_ine');
|
|
En_iff = En_iff_hpf;
|
|
t = linspace(0, 3, length(En(:,1)));
|
|
#+end_src
|
|
|
|
*** Frequency Domain Analysis
|
|
Window used for =pwelch= function.
|
|
#+begin_src matlab
|
|
n_av = 8;
|
|
han_win = hanning(ceil(length(En(:, 1))/n_av));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
Ts = t(2)-t(1); % Sample Time for the Data [s]
|
|
|
|
[pxx, f] = pwelch(En(:, 1), han_win, [], [], 1/Ts);
|
|
[pxx_ine, ~] = pwelch(En_ine(:, 1), han_win, [], [], 1/Ts);
|
|
[pxx_dvf, ~] = pwelch(En_dvf(:, 1), han_win, [], [], 1/Ts);
|
|
[pxx_iff, ~] = pwelch(En_iff(:, 1), han_win, [], [], 1/Ts);
|
|
|
|
[pyy, ~] = pwelch(En(:, 2), han_win, [], [], 1/Ts);
|
|
[pyy_ine, ~] = pwelch(En_ine(:, 2), han_win, [], [], 1/Ts);
|
|
[pyy_dvf, ~] = pwelch(En_dvf(:, 2), han_win, [], [], 1/Ts);
|
|
[pyy_iff, ~] = pwelch(En_iff(:, 2), han_win, [], [], 1/Ts);
|
|
|
|
[pzz, ~] = pwelch(En(:, 3), han_win, [], [], 1/Ts);
|
|
[pzz_ine, ~] = pwelch(En_ine(:, 3), han_win, [], [], 1/Ts);
|
|
[pzz_dvf, ~] = pwelch(En_dvf(:, 3), han_win, [], [], 1/Ts);
|
|
[pzz_iff, ~] = pwelch(En_iff(:, 3), han_win, [], [], 1/Ts);
|
|
|
|
[prx, ~] = pwelch(En(:, 4), han_win, [], [], 1/Ts);
|
|
[prx_ine, ~] = pwelch(En_ine(:, 4), han_win, [], [], 1/Ts);
|
|
[prx_dvf, ~] = pwelch(En_dvf(:, 4), han_win, [], [], 1/Ts);
|
|
[prx_iff, ~] = pwelch(En_iff(:, 4), han_win, [], [], 1/Ts);
|
|
|
|
[pry, ~] = pwelch(En(:, 5), han_win, [], [], 1/Ts);
|
|
[pry_ine, ~] = pwelch(En_ine(:, 5), han_win, [], [], 1/Ts);
|
|
[pry_dvf, ~] = pwelch(En_dvf(:, 5), han_win, [], [], 1/Ts);
|
|
[pry_iff, ~] = pwelch(En_iff(:, 5), han_win, [], [], 1/Ts);
|
|
|
|
[prz, ~] = pwelch(En(:, 6), han_win, [], [], 1/Ts);
|
|
[prz_ine, ~] = pwelch(En_ine(:, 6), han_win, [], [], 1/Ts);
|
|
[prz_dvf, ~] = pwelch(En_dvf(:, 6), han_win, [], [], 1/Ts);
|
|
[prz_iff, ~] = pwelch(En_iff(:, 6), han_win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(f, pxx_ine, 'DisplayName', 'Inertial')
|
|
plot(f, pxx_dvf, 'DisplayName', 'DVF')
|
|
plot(f, pxx_iff, 'DisplayName', 'IFF')
|
|
plot(f, pxx, 'k--', 'DisplayName', 'Undamped')
|
|
hold off;
|
|
xlabel('Frequency [Hz]');
|
|
ylabel('Power Spectral Density [$m^2/Hz$]');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
legend('location', 'northeast');
|
|
xlim([2, 500]);
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/act_damp_tomo_exp_comp_psd_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:act_damp_tomo_exp_comp_psd_trans
|
|
#+CAPTION: PSD of the translation errors for applied Active Damping techniques ([[./figs/act_damp_tomo_exp_comp_psd_trans.png][png]], [[./figs/act_damp_tomo_exp_comp_psd_trans.pdf][pdf]])
|
|
[[file:figs/act_damp_tomo_exp_comp_psd_trans.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(f, prx_ine, 'DisplayName', 'Inertial')
|
|
plot(f, prx_dvf, 'DisplayName', 'DVF')
|
|
plot(f, prx_iff, 'DisplayName', 'IFF')
|
|
plot(f, prx, 'k--', 'DisplayName', 'Undamped')
|
|
hold off;
|
|
xlabel('Frequency [Hz]');
|
|
ylabel('Power Spectral Density [$m^2/Hz$]');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
legend('location', 'northeast');
|
|
xlim([2, 500]);
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/act_damp_tomo_exp_comp_psd_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:act_damp_tomo_exp_comp_psd_rot
|
|
#+CAPTION: PSD of the rotation errors for applied Active Damping techniques ([[./figs/act_damp_tomo_exp_comp_psd_rot.png][png]], [[./figs/act_damp_tomo_exp_comp_psd_rot.pdf][pdf]])
|
|
[[file:figs/act_damp_tomo_exp_comp_psd_rot.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(f, flip(-cumtrapz(flip(f), flip(pxx_ine))), 'DisplayName', 'Inertial')
|
|
plot(f, flip(-cumtrapz(flip(f), flip(pxx_dvf))), 'DisplayName', 'DVF')
|
|
plot(f, flip(-cumtrapz(flip(f), flip(pxx_iff))), 'DisplayName', 'IFF')
|
|
plot(f, flip(-cumtrapz(flip(f), flip(pxx))), 'k--', 'DisplayName', 'Undamped')
|
|
hold off;
|
|
xlabel('Frequency [Hz]');
|
|
ylabel('Power Spectral Density [$m^2/Hz$]');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
legend('location', 'northeast');
|
|
xlim([2, 500]);
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/act_damp_tomo_exp_comp_cps_trans.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:act_damp_tomo_exp_comp_cps_trans
|
|
#+CAPTION: CPS of the translation errors for applied Active Damping techniques ([[./figs/act_damp_tomo_exp_comp_cps_trans.png][png]], [[./figs/act_damp_tomo_exp_comp_cps_trans.pdf][pdf]])
|
|
[[file:figs/act_damp_tomo_exp_comp_cps_trans.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(f, flip(-cumtrapz(flip(f), flip(prx_ine))), 'DisplayName', 'Inertial')
|
|
plot(f, flip(-cumtrapz(flip(f), flip(prx_dvf))), 'DisplayName', 'DVF')
|
|
plot(f, flip(-cumtrapz(flip(f), flip(prx_iff))), 'DisplayName', 'IFF')
|
|
plot(f, flip(-cumtrapz(flip(f), flip(prx))), 'k--', 'DisplayName', 'Undamped')
|
|
hold off;
|
|
xlabel('Frequency [Hz]');
|
|
ylabel('Power Spectral Density [$m^2/Hz$]');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
legend('location', 'northeast');
|
|
xlim([2, 500]);
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/act_damp_tomo_exp_comp_cps_rot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:act_damp_tomo_exp_comp_cps_rot
|
|
#+CAPTION: CPS of the rotation errors for applied Active Damping techniques ([[./figs/act_damp_tomo_exp_comp_cps_rot.png][png]], [[./figs/act_damp_tomo_exp_comp_cps_rot.pdf][pdf]])
|
|
[[file:figs/act_damp_tomo_exp_comp_cps_rot.png]]
|
|
|
|
* Useful Functions
|
|
** prepareTomographyExperiment
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle src/prepareTomographyExperiment.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:prepareTomographyExperiment>>
|
|
|
|
This Matlab function is accessible [[file:src/prepareTomographyExperiment.m][here]].
|
|
|
|
*** Function Description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
function [] = prepareTomographyExperiment(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.nass_actuator char {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
|
|
args.sample_mass (1,1) double {mustBeNumeric, mustBePositive} = 50
|
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
end
|
|
#+end_src
|
|
|
|
*** Initialize the Simulation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
We initialize all the stages with the default parameters.
|
|
#+begin_src matlab
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeAxisc();
|
|
initializeMirror();
|
|
#+end_src
|
|
|
|
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
|
|
#+begin_src matlab
|
|
initializeNanoHexapod('actuator', args.nass_actuator);
|
|
initializeSample('mass', args.sample_mass);
|
|
#+end_src
|
|
|
|
We set the references to zero.
|
|
#+begin_src matlab
|
|
initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Ry_period);
|
|
#+end_src
|
|
|
|
And all the controllers are set to 0.
|
|
#+begin_src matlab
|
|
K = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K', '-append');
|
|
K_ine = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_ine', '-append');
|
|
K_iff = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
K_dvf = tf(zeros(6));
|
|
save('./mat/controllers.mat', 'K_dvf', '-append');
|
|
#+end_src
|
|
|
|
* TODO Order :noexport:
|
|
** Undamped
|
|
*** Identification of the transfer function from disturbance to position error
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwx'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwy'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Dwx', 'Dwy', 'Dwz'};
|
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
|
#+end_src
|
|
|
|
*** Identification of the plant
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
lzoad('mat/stages.mat', 'nano_hexapod');
|
|
G_cart = G*inv(nano_hexapod.J');
|
|
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$T_{y}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$T_{z}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest')
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edx', 'Fnx'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edy', 'Fny'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Edz', 'Fnz'), freqs, 'Hz'))));
|
|
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 :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest')
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
|
|
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
|
|
|
|
*** TODO test
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nass_active_damping';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Dy'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Compute Error in NASS base'], 2, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Dy'};
|
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
|
#+end_src
|
|
|
|
#+begin_important
|
|
Why is the transfer function from Ty displacement to position error is equal to
|
|
1 at all frequencies?
|
|
Why don't we see any resonance?
|
|
#+end_important
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest')
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
|
|
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
|
|
|
|
*** TODO test on hexapod
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'test_nano_hexapod';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fnl'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
|
G.OutputName = {'x', 'y', 'z'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'test_nano_hexapod';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fx'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fx'};
|
|
G.OutputName = {'x', 'y', 'z'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))), 'DisplayName', '$T_{x}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest')
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('Edy', 'Dy(1)'), freqs, 'Hz'))));
|
|
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 :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$R_{x}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$R_{y}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$R_{z}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest')
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erx', 'Mnx'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Ery', 'Mny'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart('Erz', 'Mnz'), freqs, 'Hz'))));
|
|
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
|
|
|
|
*** Sensitivity to disturbances
|
|
The sensitivity to disturbances are shown on figure [[fig:sensitivity_dist_undamped]].
|
|
|
|
#+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, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
|
legend('location', 'southeast');
|
|
|
|
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, '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_undamped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:sensitivity_dist_undamped
|
|
#+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]].
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 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'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$D_x / F_x$');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$D_y / F_y$');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$D_z / F_z$');
|
|
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', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/plant_undamped.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:plant_undamped
|
|
#+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]]
|
|
|
|
|
|
** Direct Velocity Feedback
|
|
*** 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
|
|
\begin{tikzpicture}
|
|
% Ground
|
|
\draw (-1, 0) -- (1, 0);
|
|
|
|
% Ground Displacement
|
|
\draw[dashed] (-1, 0) -- ++(-0.5, 0) coordinate(w);
|
|
\draw[->] (w) -- ++(0, 0.5) node[left]{$w$};
|
|
|
|
% Mass
|
|
\draw[fill=white] (-1, 1) rectangle ++(2, 0.8) node[pos=0.5]{$m$};
|
|
|
|
% Displacement of the mass
|
|
\draw[dashed] (-1, 1.8) -- ++(-0.5, 0) coordinate(x);
|
|
\draw[->] (x) -- ++(0, 0.5) node[left]{$x$};
|
|
|
|
% Spring, Damper, and Actuator
|
|
\draw[spring] (-0.8, 0) -- (-0.8, 1) node[midway, left=0.1]{$k$};
|
|
\draw[damper] (0, 0) -- (0, 1) node[midway, left=0.2]{$c$};
|
|
\draw[actuator={0.4}{0.2}] (0.8, 0) -- (0.8, 1) coordinate[midway, right=0.1](F);
|
|
|
|
% Measured deformation
|
|
\draw[dashed] (1, 0) -- ++(2, 0) coordinate(d_bot);
|
|
\draw[dashed] (1, 1) -- ++(2, 0) coordinate(d_top);
|
|
\draw[<->] (d_bot) --coordinate[midway](d) (d_top);
|
|
|
|
% Displacements
|
|
\node[block={0.8cm}{0.6cm}, right=0.6 of F] (Krmc) {$K$};
|
|
\draw[->] (Krmc.west) -- (F) node[above right]{$F$};
|
|
\draw[->] (d)node[above left]{$d$} -- (Krmc.east);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:rmc_1dof
|
|
#+caption: Relative Motion Control applied to a 1dof system
|
|
#+RESULTS:
|
|
[[file:figs/rmc_1dof.png]]
|
|
|
|
The dynamic of the system is:
|
|
\begin{equation}
|
|
ms^2x = F_d - kx - csx + kw + csw + F
|
|
\end{equation}
|
|
In terms of the stage deformation $d = x - w$:
|
|
\begin{equation}
|
|
(ms^2 + cs + k) d = -ms^2 w + F_d + F
|
|
\end{equation}
|
|
The relative motion control law is:
|
|
\begin{equation}
|
|
K = -g s
|
|
\end{equation}
|
|
Thus, the applied force is:
|
|
\begin{equation}
|
|
F = -g s d
|
|
\end{equation}
|
|
And the new dynamics will be:
|
|
\begin{equation}
|
|
d = w \frac{-ms^2}{ms^2 + (c + g)s + k} + F_d \frac{1}{ms^2 + (c + g)s + k} + F \frac{1}{ms^2 + (c + g)s + k}
|
|
\end{equation}
|
|
|
|
And thus damping is added.
|
|
|
|
If critical damping is wanted:
|
|
\begin{equation}
|
|
\xi = \frac{1}{2}\frac{c + g}{\sqrt{km}} = \frac{1}{2}
|
|
\end{equation}
|
|
This corresponds to a gain:
|
|
\begin{equation}
|
|
g = \sqrt{km} - c
|
|
\end{equation}
|
|
|
|
**** 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
|
|
|
|
**** Matlab Example
|
|
Let define the system parameters.
|
|
#+begin_src matlab
|
|
m = 50; % [kg]
|
|
k = 1e6; % [N/m]
|
|
c = 1e3; % [N/(m/s)]
|
|
#+end_src
|
|
|
|
The state space model of the system is defined below.
|
|
#+begin_src matlab
|
|
A = [-c/m -k/m;
|
|
1 0];
|
|
|
|
B = [1/m 1/m -1;
|
|
0 0 0];
|
|
|
|
C = [ 0 1;
|
|
-c -k];
|
|
|
|
D = [0 0 0;
|
|
1 0 0];
|
|
|
|
sys = ss(A, B, C, D);
|
|
sys.InputName = {'F', 'Fd', 'wddot'};
|
|
sys.OutputName = {'d', 'Fm'};
|
|
sys.StateName = {'ddot', 'd'};
|
|
#+end_src
|
|
|
|
The controller $K_\text{RMC}$ is:
|
|
#+begin_src matlab
|
|
Krmc = -(sqrt(k*m)-c)*s;
|
|
Krmc.InputName = {'d'};
|
|
Krmc.OutputName = {'F'};
|
|
#+end_src
|
|
|
|
And the closed loop system is computed below.
|
|
#+begin_src matlab
|
|
sys_rmc = feedback(sys, Krmc, 'name', +1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
|
|
subplot(2, 2, 1);
|
|
title('Fd to d')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(sys('d', 'Fd'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(sys_rmc('d', 'Fd'), freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
subplot(2, 2, 3);
|
|
title('Fd to x')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(sys('d', 'Fd'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(sys_rmc('d', 'Fd'), freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
subplot(2, 2, 2);
|
|
title('w to d')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(sys('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(sys_rmc('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
subplot(2, 2, 4);
|
|
title('w to x')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(1+sys('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(1+sys_rmc('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/rmc_1dof_sensitivitiy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:rmc_1dof_sensitivitiy
|
|
#+CAPTION: Sensitivity to disturbance when RMC is applied on the 1dof system ([[./figs/rmc_1dof_sensitivitiy.png][png]], [[./figs/rmc_1dof_sensitivitiy.pdf][pdf]])
|
|
[[file:figs/rmc_1dof_sensitivitiy.png]]
|
|
** Inertial Control
|
|
*** One degree-of-freedom example
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle no
|
|
:END:
|
|
<<sec:ine_1dof>>
|
|
**** Equations
|
|
#+begin_src latex :file ine_1dof.pdf :post pdf2svg(file=*this*, ext="png") :exports results
|
|
\begin{tikzpicture}
|
|
% Ground
|
|
\draw (-1, 0) -- (1, 0);
|
|
|
|
% Ground Displacement
|
|
\draw[dashed] (-1, 0) -- ++(-0.5, 0) coordinate(w);
|
|
\draw[->] (w) -- ++(0, 0.5) node[left]{$w$};
|
|
|
|
% Mass
|
|
\draw[fill=white] (-1, 1) rectangle ++(2, 0.8) node[pos=0.5]{$m$};
|
|
|
|
% Velocity Sensor
|
|
\node[inertialsensor={0.3}] (velg) at (1, 1.8){};
|
|
\node[above] at (velg.north) {$\dot{x}$};
|
|
|
|
% Displacement of the mass
|
|
\draw[dashed] (-1, 1.8) -- ++(-0.5, 0) coordinate(x);
|
|
\draw[->] (x) -- ++(0, 0.5) node[left]{$x$};
|
|
|
|
% Spring, Damper, and Actuator
|
|
\draw[spring] (-0.8, 0) -- (-0.8, 1) node[midway, left=0.1]{$k$};
|
|
\draw[damper] (0, 0) -- (0, 1) node[midway, left=0.2]{$c$};
|
|
\draw[actuator={0.4}{0.2}] (0.8, 0) -- (0.8, 1) coordinate[midway, right=0.1](F);
|
|
|
|
% Control
|
|
\node[block={0.8cm}{0.6cm}, right=0.6 of F] (Kine) {$K$};
|
|
\draw[->] (Kine.west) -- (F) node[above right]{$F$};
|
|
\draw[<-] (Kine.east) -- ++(0.5, 0) |- (velg.east);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:ine_1dof
|
|
#+caption: Direct Velocity Feedback applied to a 1dof system
|
|
#+RESULTS:
|
|
[[file:figs/ine_1dof.png]]
|
|
|
|
The dynamic of the system is:
|
|
\begin{equation}
|
|
ms^2x = F_d - kx - csx + kw + csw + F
|
|
\end{equation}
|
|
In terms of the stage deformation $d = x - w$:
|
|
\begin{equation}
|
|
(ms^2 + cs + k) d = -ms^2 w + F_d + F
|
|
\end{equation}
|
|
The direct velocity feedback law shown in figure [[fig:ine_1dof]] is:
|
|
\begin{equation}
|
|
K = -g
|
|
\end{equation}
|
|
Thus, the applied force is:
|
|
\begin{equation}
|
|
F = -g \dot{x}
|
|
\end{equation}
|
|
And the new dynamics will be:
|
|
\begin{equation}
|
|
d = w \frac{-ms^2 - gs}{ms^2 + (c + g)s + k} + F_d \frac{1}{ms^2 + (c + g)s + k} + F \frac{1}{ms^2 + (c + g)s + k}
|
|
\end{equation}
|
|
|
|
And thus damping is added.
|
|
|
|
If critical damping is wanted:
|
|
\begin{equation}
|
|
\xi = \frac{1}{2}\frac{c + g}{\sqrt{km}} = \frac{1}{2}
|
|
\end{equation}
|
|
This corresponds to a gain:
|
|
\begin{equation}
|
|
g = \sqrt{km} - c
|
|
\end{equation}
|
|
|
|
**** 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
|
|
|
|
**** Matlab Example
|
|
Let define the system parameters.
|
|
#+begin_src matlab
|
|
m = 50; % [kg]
|
|
k = 1e6; % [N/m]
|
|
c = 1e3; % [N/(m/s)]
|
|
#+end_src
|
|
|
|
The state space model of the system is defined below.
|
|
#+begin_src matlab
|
|
A = [-c/m -k/m;
|
|
1 0];
|
|
|
|
B = [1/m 1/m -1;
|
|
0 0 0];
|
|
|
|
C = [1 0;
|
|
0 1;
|
|
0 0];
|
|
|
|
D = [0 0 0;
|
|
0 0 0;
|
|
0 0 1];
|
|
|
|
sys = ss(A, B, C, D);
|
|
sys.InputName = {'F', 'Fd', 'wddot'};
|
|
sys.OutputName = {'ddot', 'd', 'wddot'};
|
|
sys.StateName = {'ddot', 'd'};
|
|
#+end_src
|
|
|
|
Because we need $\dot{x}$ for feedback, we compute it from the outputs
|
|
#+begin_src matlab
|
|
G_xdot = [1, 0, 1/s;
|
|
0, 1, 0];
|
|
G_xdot.InputName = {'ddot', 'd', 'wddot'};
|
|
G_xdot.OutputName = {'xdot', 'd'};
|
|
#+end_src
|
|
|
|
Finally, the system is described by =sys= as defined below.
|
|
#+begin_src matlab
|
|
sys = series(sys, G_xdot, [1 2 3], [1 2 3]);
|
|
#+end_src
|
|
|
|
The controller $K_\text{INE}$ is:
|
|
#+begin_src matlab
|
|
Kine = tf(-(sqrt(k*m)-c));
|
|
Kine.InputName = {'xdot'};
|
|
Kine.OutputName = {'F'};
|
|
#+end_src
|
|
|
|
And the closed loop system is computed below.
|
|
#+begin_src matlab
|
|
sys_ine = feedback(sys, Kine, 'name', +1);
|
|
#+end_src
|
|
|
|
The obtained sensitivity to disturbances is shown in figure [[fig:ine_1dof_sensitivitiy]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
|
|
subplot(2, 2, 1);
|
|
title('Fd to d')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(sys('d', 'Fd'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(sys_ine('d', 'Fd'), freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
subplot(2, 2, 3);
|
|
title('Fd to x')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(sys('xdot', 'Fd')/s, freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(sys_ine('xdot', 'Fd')/s, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
subplot(2, 2, 2);
|
|
title('w to d')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(sys('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(sys_ine('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
subplot(2, 2, 4);
|
|
title('w to x')
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(1+sys('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(1+sys_ine('d', 'wddot')*s^2, freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
|
#+begin_src matlab :var filepath="figs/ine_1dof_sensitivitiy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
|
<<plt-matlab>>
|
|
#+end_src
|
|
|
|
#+NAME: fig:ine_1dof_sensitivitiy
|
|
#+CAPTION: Sensitivity to disturbance when INE is applied on the 1dof system ([[./figs/ine_1dof_sensitivitiy.png][png]], [[./figs/ine_1dof_sensitivitiy.pdf][pdf]])
|
|
[[file:figs/ine_1dof_sensitivitiy.png]]
|