1633 lines
58 KiB
Org Mode
1633 lines
58 KiB
Org Mode
#+TITLE: Nano-Hexapod - Test Bench
|
|
:DRAWER:
|
|
#+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="https://research.tdehaeze.xyz/css/style.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
|
|
#+BIND: org-latex-image-default-option "scale=1"
|
|
#+BIND: org-latex-image-default-width ""
|
|
|
|
#+LaTeX_CLASS: scrreprt
|
|
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full]
|
|
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :noweb yes
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results file raw replace
|
|
#+PROPERTY: header-args:latex+ :buffer no
|
|
#+PROPERTY: header-args:latex+ :tangle no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports results
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
|
:END:
|
|
|
|
#+begin_export html
|
|
<hr>
|
|
<p>This report is also available as a <a href="./test-bench-nano-hexapod.pdf">pdf</a>.</p>
|
|
<hr>
|
|
#+end_export
|
|
|
|
#+latex: \clearpage
|
|
|
|
* Introduction :ignore:
|
|
This document is dedicated to the experimental study of the nano-hexapod shown in Figure [[fig:picture_bench_granite_nano_hexapod]].
|
|
|
|
#+name: fig:picture_bench_granite_nano_hexapod
|
|
#+caption: Nano-Hexapod
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/IMG_20210608_152917.jpg]]
|
|
|
|
#+begin_note
|
|
Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure [[fig:picture_bench_granite_overview]]):
|
|
- Voltage Amplifier: PiezoDrive [[file:doc/PD200-V7-R1.pdf][PD200]]
|
|
- Amplified Piezoelectric Actuator: Cedrat [[file:doc/APA300ML.pdf][APA300ML]]
|
|
- DAC/ADC: Speedgoat [[file:doc/IO131-OEM-Datasheet.pdf][IO313]]
|
|
- Encoder: Renishaw [[file:doc/L-9517-9678-05-A_Data_sheet_VIONiC_series_en.pdf][Vionic]] and used [[file:doc/L-9517-9862-01-C_Data_sheet_RKLC_EN.pdf][Ruler]]
|
|
- Interferometers: Attocube
|
|
#+end_note
|
|
|
|
#+name: fig:picture_bench_granite_overview
|
|
#+caption: Nano-Hexapod and the control electronics
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/IMG_20210608_154722.jpg]]
|
|
|
|
In Figure [[fig:nano_hexapod_signals]] is shown a block diagram of the experimental setup.
|
|
When possible, the notations are consistent with this diagram and summarized in Table [[tab:list_signals]].
|
|
|
|
#+begin_src latex :file nano_hexapod_signals.pdf
|
|
\definecolor{instrumentation}{rgb}{0, 0.447, 0.741}
|
|
\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098}
|
|
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={4.0cm}{3.0cm}, fill=mechanics!20!white] (nano_hexapod) {Mechanics};
|
|
\coordinate[] (inputF) at (nano_hexapod.west);
|
|
\coordinate[] (outputL) at ($(nano_hexapod.south east)!0.8!(nano_hexapod.north east)$);
|
|
\coordinate[] (outputF) at ($(nano_hexapod.south east)!0.2!(nano_hexapod.north east)$);
|
|
|
|
\node[block, left= 0.8 of inputF, fill=instrumentation!20!white, align=center] (F_stack) {\tiny Actuator \\ \tiny stacks};
|
|
\node[block, left= 0.8 of F_stack, fill=instrumentation!20!white] (PD200) {PD200};
|
|
\node[DAC, left= 0.8 of PD200, fill=instrumentation!20!white] (F_DAC) {DAC};
|
|
\node[block, right=0.8 of outputF, fill=instrumentation!20!white, align=center] (Fm_stack){\tiny Sensor \\ \tiny stack};
|
|
\node[ADC, right=0.8 of Fm_stack,fill=instrumentation!20!white] (Fm_ADC) {ADC};
|
|
\node[block, right=0.8 of outputL, fill=instrumentation!20!white] (encoder) {\tiny Encoder};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(F_DAC.west)+(-0.8,0)$) node[above right]{$\bm{u}$} node[below right]{$[V]$} -- node[sloped]{$/$} (F_DAC.west);
|
|
\draw[->] (F_DAC.east) -- node[midway, above]{$\tilde{\bm{u}}$}node[midway, below]{$[V]$} (PD200.west);
|
|
\draw[->] (PD200.east) -- node[midway, above]{$\bm{u}_a$}node[midway, below]{$[V]$} (F_stack.west);
|
|
\draw[->] (F_stack.east) -- (inputF) node[above left]{$\bm{\tau}$}node[below left]{$[N]$};
|
|
|
|
\draw[->] (outputF) -- (Fm_stack.west) node[above left]{$\bm{\epsilon}$} node[below left]{$[m]$};
|
|
\draw[->] (Fm_stack.east) -- node[midway, above]{$\tilde{\bm{\tau}}_m$}node[midway, below]{$[V]$} (Fm_ADC.west);
|
|
\draw[->] (Fm_ADC.east) -- node[sloped]{$/$} ++(0.8, 0)coordinate(end) node[above left]{$\bm{\tau}_m$}node[below left]{$[V]$};
|
|
|
|
\draw[->] (outputL) -- (encoder.west) node[above left]{$d\bm{\mathcal{L}}$} node[below left]{$[m]$};
|
|
\draw[->] (encoder.east) -- node[sloped]{$/$} (encoder-|end) node[above left]{$d\bm{\mathcal{L}}_m$}node[below left]{$[m]$};
|
|
|
|
% Nano-Hexapod
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(F_stack.west|-nano_hexapod.south) (Fm_stack.east|-nano_hexapod.north)}, fill=black!20!white, draw, inner sep=2pt] (system) {};
|
|
\node[above] at (system.north) {Nano-Hexapod};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_signals
|
|
#+caption: Block diagram of the system with named signals
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nano_hexapod_signals.png]]
|
|
|
|
#+name: tab:list_signals
|
|
#+caption: List of signals
|
|
#+attr_latex: :environment tabularx :width \linewidth :align Xllll
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| | *Unit* | *Matlab* | *Vector* | *Elements* |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Control Input (wanted DAC voltage) | =[V]= | =u= | $\bm{u}$ | $u_i$ |
|
|
| DAC Output Voltage | =[V]= | =u= | $\tilde{\bm{u}}$ | $\tilde{u}_i$ |
|
|
| PD200 Output Voltage | =[V]= | =ua= | $\bm{u}_a$ | $u_{a,i}$ |
|
|
| Actuator applied force | =[N]= | =tau= | $\bm{\tau}$ | $\tau_i$ |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Strut motion | =[m]= | =dL= | $d\bm{\mathcal{L}}$ | $d\mathcal{L}_i$ |
|
|
| Encoder measured displacement | =[m]= | =dLm= | $d\bm{\mathcal{L}}_m$ | $d\mathcal{L}_{m,i}$ |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Force Sensor strain | =[m]= | =epsilon= | $\bm{\epsilon}$ | $\epsilon_i$ |
|
|
| Force Sensor Generated Voltage | =[V]= | =taum= | $\tilde{\bm{\tau}}_m$ | $\tilde{\tau}_{m,i}$ |
|
|
| Measured Generated Voltage | =[V]= | =taum= | $\bm{\tau}_m$ | $\tau_{m,i}$ |
|
|
|------------------------------------+-----------+-----------+-----------------------+----------------------|
|
|
| Motion of the top platform | =[m,rad]= | =dX= | $d\bm{\mathcal{X}}$ | $d\mathcal{X}_i$ |
|
|
| Metrology measured displacement | =[m,rad]= | =dXm= | $d\bm{\mathcal{X}}_m$ | $d\mathcal{X}_{m,i}$ |
|
|
|
|
This document is divided in the following sections:
|
|
- Section [[sec:encoders_struts]]: the encoders are fixed to the struts
|
|
- Section [[sec:encoders_plates]]: the encoders are fixed to the plates
|
|
|
|
* Encoders fixed to the Struts
|
|
<<sec:encoders_struts>>
|
|
|
|
** Introduction
|
|
In this section, the encoders are fixed to the struts.
|
|
|
|
It is divided in the following sections:
|
|
- Section [[sec:enc_struts_plant_id]]: the transfer function matrix from the actuators to the force sensors and to the encoders is experimentally identified.
|
|
- Section [[sec:enc_struts_comp_simscape]]: the obtained FRF matrix is compared with the dynamics of the simscape model
|
|
- Section [[sec:enc_struts_iff]]: decentralized Integral Force Feedback (IFF) is applied and its performances are evaluated.
|
|
- Section [[sec:enc_struts_modal_analysis]]: a modal analysis of the nano-hexapod is performed
|
|
|
|
** 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
|
|
addpath('./matlab/mat/');
|
|
addpath('./matlab/src/');
|
|
addpath('./matlab/');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('./mat/');
|
|
addpath('./src/');
|
|
#+end_src
|
|
|
|
** Identification of the dynamics
|
|
<<sec:enc_struts_plant_id>>
|
|
*** Load Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_data_lf = {};
|
|
|
|
for i = 1:6
|
|
meas_data_lf(i) = {load(sprintf('mat/frf_data_exc_strut_%i_noise_lf.mat', i), 't', 'Va', 'Vs', 'de')};
|
|
meas_data_hf(i) = {load(sprintf('mat/frf_data_exc_strut_%i_noise_hf.mat', i), 't', 'Va', 'Vs', 'de')};
|
|
end
|
|
#+end_src
|
|
|
|
*** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
|
|
|
|
i_lf = f < 250; % Points for low frequency excitation
|
|
i_hf = f > 250; % Points for high frequency excitation
|
|
#+end_src
|
|
|
|
*** DVF Plant
|
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_dvf_coh]]).
|
|
|
|
#+begin_src matlab
|
|
%% Coherence
|
|
coh_dvf_lf = zeros(length(f), 6, 6);
|
|
coh_dvf_hf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
coh_dvf_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
|
coh_dvf_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Coherence for the transfer function from u to dLm
|
|
figure;
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), coh_dvf_lf(i_lf, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), coh_dvf_hf(i_hf, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), coh_dvf_lf(i_lf,i, i), ...
|
|
'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), coh_dvf_hf(i_hf,i, i), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f(i_lf), coh_dvf_lf(i_lf, 1, 2), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{dvf}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
xlim([20, 2e3]); ylim([0, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_coh
|
|
#+caption: Obtained coherence for the DVF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_coh.png]]
|
|
|
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_dvf_frf]]).
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_dvf_lf = zeros(length(f), 6, 6);
|
|
G_dvf_hf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_dvf_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
|
G_dvf_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ...
|
|
'DisplayName', sprintf('$G_{dvf}(%i,%i)$', i, i));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{dvf}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_frf
|
|
#+caption: Measured FRF for the DVF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_frf.png]]
|
|
|
|
|
|
*** IFF Plant
|
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_iff_coh]]).
|
|
|
|
#+begin_src matlab
|
|
%% Coherence for the IFF plant
|
|
coh_iff_lf = zeros(length(f), 6, 6);
|
|
coh_iff_hf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
coh_iff_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
|
|
coh_iff_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
|
|
end
|
|
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Coherence of the IFF Plant (transfer function from u to taum)
|
|
figure;
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), coh_iff_lf(i_lf, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), coh_iff_hf(i_hf, i, j), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), coh_iff_lf(i_lf,i, i), ...
|
|
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), coh_iff_hf(i_hf,i, i), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f(i_lf), coh_iff_lf(i_lf, 1, 2), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{iff}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
xlim([20, 2e3]); ylim([0, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_coh
|
|
#+caption: Obtained coherence for the IFF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_coh.png]]
|
|
|
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_iff_frf]]).
|
|
#+begin_src matlab
|
|
%% IFF Plant
|
|
G_iff_lf = zeros(length(f), 6, 6);
|
|
G_iff_hf = zeros(length(f), 6, 6);
|
|
|
|
for i = 1:6
|
|
G_iff_lf(:, :, i) = tfestimate(meas_data_lf{i}.Va, meas_data_lf{i}.Vs, win, [], [], 1/Ts);
|
|
G_iff_hf(:, :, i) = tfestimate(meas_data_hf{i}.Va, meas_data_hf{i}.Vs, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the IFF Plant (transfer function from u to taum)
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), ...
|
|
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$G_{iff}(i,j)$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
ylim([1e-3, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_frf
|
|
#+caption: Measured FRF for the IFF plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_frf.png]]
|
|
|
|
** Jacobian :noexport:
|
|
*** Introduction :ignore:
|
|
The Jacobian is used to transform the excitation force in the cartesian frame as well as the displacements.
|
|
|
|
Consider the plant shown in Figure [[fig:schematic_jacobian_in_out]] with:
|
|
- $\tau$ the 6 input voltages (going to the PD200 amplifier and then to the APA)
|
|
- $d\mathcal{L}$ the relative motion sensor outputs (encoders)
|
|
- $\bm{\tau}_m$ the generated voltage of the force sensor stacks
|
|
- $J_a$ and $J_s$ the Jacobians for the actuators and sensors
|
|
|
|
#+begin_src latex :file schematic_jacobian_in_out.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at (P.west);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputF) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, left= of inputF] (Ja) {$\bm{J}^{-T}_a$};
|
|
\node[block, right= of outputL] (Js) {$\bm{J}^{-1}_s$};
|
|
\node[block, right= of outputF] (Jf) {$\bm{J}^{-1}_s$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1,0)$) -- (Ja.west) node[above left]{$\bm{\mathcal{F}}$};
|
|
\draw[->] (Ja.east) -- (inputF) node[above left]{$\bm{\tau}$};
|
|
\draw[->] (outputL) -- (Js.west) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] (Js.east) -- ++(1, 0) node[above left]{$d\bm{\mathcal{X}}$};
|
|
\draw[->] (outputF) -- (Jf.west) node[above left]{$\bm{\tau}_m$};
|
|
\draw[->] (Jf.east) -- ++(1, 0) node[above left]{$\bm{\mathcal{F}}_m$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_in_out
|
|
#+caption: Plant in the cartesian Frame
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_in_out.png]]
|
|
|
|
First, we load the Jacobian matrix (same for the actuators and sensors).
|
|
#+begin_src matlab
|
|
load('jacobian.mat', 'J');
|
|
#+end_src
|
|
|
|
*** DVF Plant
|
|
The transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}}$ is computed and shown in Figure [[fig:enc_struts_dvf_cart_frf]].
|
|
|
|
#+begin_src matlab
|
|
G_dvf_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_lf, [2 3 1]), inv(J'))), [3 1 2]);
|
|
G_dvf_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_hf, [2 3 1]), inv(J'))), [3 1 2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), abs(G_dvf_J_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), abs(G_dvf_J_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), abs(G_dvf_J_lf(i_lf,i, i)), ...
|
|
'DisplayName', labels{i});
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), abs(G_dvf_J_hf(i_hf,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f(i_lf), abs(G_dvf_J_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_i/F_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-7, 1e-1]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), 180/pi*angle(G_dvf_J_lf(i_lf,i, i)));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), 180/pi*angle(G_dvf_J_hf(i_hf,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_cart_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_cart_frf
|
|
#+caption: Measured FRF for the DVF plant in the cartesian frame
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_cart_frf.png]]
|
|
|
|
*** IFF Plant
|
|
The transfer function from $\bm{\mathcal{F}}$ to $\bm{\mathcal{F}}_m$ is computed and shown in Figure [[fig:enc_struts_iff_cart_frf]].
|
|
|
|
#+begin_src matlab
|
|
G_iff_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_lf, [2 3 1]), inv(J'))), [3 1 2]);
|
|
G_iff_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_hf, [2 3 1]), inv(J'))), [3 1 2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
labels = {'$F_{m,x}/F_{x}$', '$F_{m,y}/F_{y}$', '$F_{m,z}/F_{z}$', '$M_{m,x}/M_{x}$', '$M_{m,y}/M_{y}$', '$M_{m,z}/M_{z}$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), abs(G_iff_J_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), abs(G_iff_J_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), abs(G_iff_J_lf(i_lf,i, i)), ...
|
|
'DisplayName', labels{i});
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), abs(G_iff_J_hf(i_hf,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f(i_lf), abs(G_iff_J_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_i/F_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e4]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_lf), 180/pi*angle(G_iff_J_lf(i_lf,i, i)));
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(f(i_hf), 180/pi*angle(G_iff_J_hf(i_hf,i, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_cart_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_cart_frf
|
|
#+caption: Measured FRF for the IFF plant in the cartesian frame
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_cart_frf.png]]
|
|
|
|
** Comparison with the Simscape Model
|
|
<<sec:enc_struts_comp_simscape>>
|
|
*** Introduction :ignore:
|
|
In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.
|
|
|
|
*** Initialize :noexport:
|
|
#+begin_src matlab :tangle no
|
|
%% Add all useful folders to the path
|
|
addpath('matlab/')
|
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('matlab/nass-simscape/STEPS/png/')
|
|
addpath('matlab/nass-simscape/src/')
|
|
addpath('matlab/nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add all useful folders to the path
|
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
|
addpath('nass-simscape/STEPS/png/')
|
|
addpath('nass-simscape/src/')
|
|
addpath('nass-simscape/mat/')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Open Simulink Model
|
|
mdl = 'nano_hexapod_simscape';
|
|
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
*** Dynamics from Actuator to Force Sensors
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]);
|
|
plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_comp_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_comp_simscape
|
|
#+caption: Diagonal elements of the IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_comp_simscape.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data (off-diagonal elements)
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
% Off diagonal terms
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-3, 1e2]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_comp_offdiag_simscape
|
|
#+caption: Off diagonal elements of the IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_comp_offdiag_simscape.png]]
|
|
|
|
*** Dynamics from Actuator to Encoder
|
|
#+begin_src matlab
|
|
%% Initialization of the Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
|
|
Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Diagonal elements of the DVF plant
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2]);
|
|
plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_comp_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_comp_simscape
|
|
#+caption: Diagonal elements of the DVF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_comp_simscape.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Off-diagonal elements of the DVF plant
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
% Off diagonal terms
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-8, 1e-3]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_dvf_comp_offdiag_simscape
|
|
#+caption: Off diagonal elements of the DVF Plant
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_dvf_comp_offdiag_simscape.png]]
|
|
|
|
** Integral Force Feedback
|
|
<<sec:enc_struts_iff>>
|
|
*** Root Locus and Decentralized Loop gain
|
|
#+begin_src matlab
|
|
%% IFF Controller
|
|
Kiff_g1 = (1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
|
|
(s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
|
|
(1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
|
|
eye(6); % Diagonal 6x6 controller
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Root Locus for IFF
|
|
gains = logspace(1, 4, 100);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
% Pure Integrator
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', '$g = 0$');
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(Giff, g*Kiff_g1*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
|
|
end
|
|
|
|
g = 4e2;
|
|
clpoles = pole(feedback(Giff, g*Kiff_g1*eye(6)));
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(real(clpoles), imag(clpoles), 'x', 'DisplayName', sprintf('$g=%.0f$', g));
|
|
hold off;
|
|
axis square;
|
|
xlim([-1250, 0]); ylim([0, 1250]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_root_locus
|
|
#+caption: Root Locus for the IFF control strategy
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_root_locus.png]]
|
|
|
|
Then the "optimal" IFF controller is:
|
|
#+begin_src matlab
|
|
%% IFF controller with Optimal gain
|
|
Kiff = g*Kiff_g1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the "decentralized loop gain"
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - FRF')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f(i_hf), abs(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f(i_lf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]);
|
|
plot(f(i_hf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2]);
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(1,1)*Giff(i,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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_opt_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_opt_loop_gain
|
|
#+caption: Bode plot of the "decentralized loop gain" $G_\text{iff}(i,i) \times K_\text{iff}(i,i)$
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_opt_loop_gain.png]]
|
|
|
|
*** Multiple Gains - Simulation
|
|
#+begin_src matlab
|
|
%% Tested IFF gains
|
|
iff_gains = [4, 10, 20, 40, 100, 200, 400];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize the Simscape model in closed loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'iff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
|
|
Gd_iff = {zeros(1, length(iff_gains))};
|
|
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
|
|
|
|
for i = 1:length(iff_gains)
|
|
Kiff = iff_gains(i)*Kiff_g1*eye(6); % IFF Controller
|
|
Gd_iff(i) = {exp(-s*Ts)*linearize(mdl, io, 0.0, options)};
|
|
|
|
isstable(Gd_iff{i})
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', sprintf('$g = %.0f$', iff_gains(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{i}(1,1), 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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/enc_struts_iff_gains_effect_dvf_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:enc_struts_iff_gains_effect_dvf_plant
|
|
#+caption: Effect of the IFF gain $g$ on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$
|
|
#+RESULTS:
|
|
[[file:figs/enc_struts_iff_gains_effect_dvf_plant.png]]
|
|
|
|
*** Experimental Results - Gains
|
|
**** Introduction :ignore:
|
|
Let's look at the damping introduced by IFF as a function of the IFF gain and compare that with the results obtained using the Simscape model.
|
|
|
|
**** Load Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_iff_gains = {};
|
|
|
|
for i = 1:length(iff_gains)
|
|
meas_iff_gains(i) = {load(sprintf('mat/iff_strut_1_noise_g_%i.mat', iff_gains(i)), 't', 'Vexc', 'Vs', 'de', 'u')};
|
|
end
|
|
#+end_src
|
|
|
|
**** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_iff_gains{1}.t(end) - (meas_iff_gains{1}.t(1)))/(length(meas_iff_gains{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_iff_gains{1}.Vexc, meas_iff_gains{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
**** DVF Plant
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_iff_gains = {};
|
|
|
|
for i = 1:length(iff_gains)
|
|
G_iff_gains{i} = tfestimate(meas_iff_gains{i}.Vexc, meas_iff_gains{i}.de(:,1), win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(iff_gains)
|
|
plot(f, abs(G_iff_gains{i}), '-', ...
|
|
'DisplayName', sprintf('$g_{iff} = %.0f$', iff_gains(i)));
|
|
end
|
|
set(gca,'ColorOrderIndex',1)
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}(1,1), freqs, 'Hz'))), '--', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:length(iff_gains)
|
|
plot(f, 180/pi*angle(G_iff_gains{i}), '-');
|
|
end
|
|
set(gca,'ColorOrderIndex',1)
|
|
for i = 1:length(iff_gains)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{i}(1,1), 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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_iff_gains_dvf_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_iff_gains_dvf_plant
|
|
#+caption: Transfer function from $u$ to $d\mathcal{L}_m$ for multiple values of the IFF gain
|
|
#+RESULTS:
|
|
[[file:figs/comp_iff_gains_dvf_plant.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
xlim([20, 200]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_iff_gains_dvf_plant_zoom.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_iff_gains_dvf_plant_zoom
|
|
#+caption: Transfer function from $u$ to $d\mathcal{L}_m$ for multiple values of the IFF gain (Zoom)
|
|
#+RESULTS:
|
|
[[file:figs/comp_iff_gains_dvf_plant_zoom.png]]
|
|
|
|
#+begin_important
|
|
The IFF control strategy is very effective for the damping of the suspension modes.
|
|
It however does not damp the modes at 200Hz, 300Hz and 400Hz (flexible modes of the APA).
|
|
This is very logical.
|
|
|
|
Also, the experimental results and the models obtained from the Simscape model are in agreement.
|
|
#+end_important
|
|
|
|
**** Experimental Results - Comparison of the un-damped and fully damped system
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Un Damped measurement
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), ...
|
|
'DisplayName', 'Un-Damped')
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f(i_hf), abs(G_dvf_hf(i_hf,1, 1)), ...
|
|
'HandleVisibility', 'off');
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% IFF Damped measurement
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff_opt{1}(:,1)), ...
|
|
'DisplayName', 'Optimal gain')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, abs(G_iff_opt{i}(:,i)), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)));
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)));
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, 180/pi*angle(G_iff_opt{i}(:,i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_undamped_opt_iff_gain_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_undamped_opt_iff_gain_diagonal
|
|
#+caption: Comparison of the diagonal elements of the tranfer function from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ without active damping and with optimal IFF gain
|
|
#+RESULTS:
|
|
[[file:figs/comp_undamped_opt_iff_gain_diagonal.png]]
|
|
|
|
#+begin_question
|
|
A series of modes at around 205Hz are also damped.
|
|
|
|
Are these damped modes at 205Hz additional "suspension" modes or flexible modes of the struts?
|
|
#+end_question
|
|
|
|
*** Experimental Results - Damped Plant with Optimal gain
|
|
**** Introduction :ignore:
|
|
Let's now look at the $6 \times 6$ damped plant with the optimal gain $g = 400$.
|
|
|
|
**** Load Data
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_iff_struts = {};
|
|
|
|
for i = 1:6
|
|
meas_iff_struts(i) = {load(sprintf('mat/iff_strut_%i_noise_g_400.mat', i), 't', 'Vexc', 'Vs', 'de', 'u')};
|
|
end
|
|
#+end_src
|
|
|
|
**** Spectral Analysis - Setup
|
|
#+begin_src matlab
|
|
%% Setup useful variables
|
|
% Sampling Time [s]
|
|
Ts = (meas_iff_struts{1}.t(end) - (meas_iff_struts{1}.t(1)))/(length(meas_iff_struts{1}.t)-1);
|
|
|
|
% Sampling Frequency [Hz]
|
|
Fs = 1/Ts;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1*Fs));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_iff_struts{1}.Vexc, meas_iff_struts{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
**** DVF Plant
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_iff_opt = {};
|
|
|
|
for i = 1:6
|
|
G_iff_opt{i} = tfestimate(meas_iff_struts{i}.Vexc, meas_iff_struts{i}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Diagonal Elements FRF
|
|
plot(f, abs(G_iff_opt{1}(:,1)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
|
for i = 2:6
|
|
plot(f, abs(G_iff_opt{i}(:,i)), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Diagonal Elements Model
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
for i = 2:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_iff_opt{i}(:,i)), 'color', [0,0,0,0.2]);
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{end}(i,i), freqs, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/damped_iff_plant_comp_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:damped_iff_plant_comp_diagonal
|
|
#+caption: Comparison of the diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$
|
|
#+RESULTS:
|
|
[[file:figs/damped_iff_plant_comp_diagonal.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the transfer function from u to dLm
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
% Off diagonal FRF
|
|
plot(f, abs(G_iff_opt{1}(:,2)), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff_opt{i}(:,j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
|
|
% Off diagonal Model
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(1,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{end}(i,j), freqs, 'Hz'))), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/V_{exc}$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
% Off diagonal FRF
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, 180/pi*angle(G_iff_opt{i}(:,j)), 'color', [0, 0, 0, 0.2]);
|
|
end
|
|
end
|
|
|
|
% Off diagonal Model
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{end}(i,j), freqs, 'Hz'))));
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/damped_iff_plant_comp_off_diagonal.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:damped_iff_plant_comp_off_diagonal
|
|
#+caption: Comparison of the off-diagonal elements of the transfer functions from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ with active damping (IFF) applied with an optimal gain $g = 400$
|
|
#+RESULTS:
|
|
[[file:figs/damped_iff_plant_comp_off_diagonal.png]]
|
|
|
|
#+begin_important
|
|
With the IFF control strategy applied and the optimal gain used, the suspension modes are very well damped.
|
|
Remains the undamped flexible modes of the APA (200Hz, 300Hz, 400Hz), and the modes of the plates (700Hz).
|
|
|
|
The Simscape model and the experimental results are in very good agreement.
|
|
#+end_important
|
|
|
|
** Modal Analysis
|
|
<<sec:enc_struts_modal_analysis>>
|
|
*** Introduction :ignore:
|
|
Several 3-axis accelerometers are fixed on the top platform of the nano-hexapod as shown in Figure [[fig:compliance_vertical_comp_iff]].
|
|
|
|
#+name: fig:accelerometers_nano_hexapod
|
|
#+caption: Location of the accelerometers on top of the nano-hexapod
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/accelerometers_nano_hexapod.jpg]]
|
|
|
|
The top platform is then excited using an instrumented hammer as shown in Figure [[fig:hammer_excitation_compliance_meas]].
|
|
|
|
#+name: fig:hammer_excitation_compliance_meas
|
|
#+caption: Example of an excitation using an instrumented hammer
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/hammer_excitation_compliance_meas.jpg]]
|
|
|
|
*** Effectiveness of the IFF Strategy - Compliance
|
|
In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance.
|
|
|
|
The top plate is excited vertically using the instrumented hammer two times:
|
|
1. no control loop is used
|
|
2. decentralized IFF is used
|
|
|
|
The data is loaded.
|
|
#+begin_src matlab
|
|
frf_ol = load('Measurement_Z_axis.mat'); % Open-Loop
|
|
frf_iff = load('Measurement_Z_axis_damped.mat'); % IFF
|
|
#+end_src
|
|
|
|
The mean vertical motion of the top platform is computed by averaging all 5 accelerometers.
|
|
#+begin_src matlab
|
|
%% Multiply by 10 (gain in m/s^2/V) and divide by 5 (number of accelerometers)
|
|
d_frf_ol = 10/5*(frf_ol.FFT1_H1_4_1_RMS_Y_Mod + frf_ol.FFT1_H1_7_1_RMS_Y_Mod + frf_ol.FFT1_H1_10_1_RMS_Y_Mod + frf_ol.FFT1_H1_13_1_RMS_Y_Mod + frf_ol.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_ol.FFT1_H1_16_1_RMS_X_Val).^2;
|
|
d_frf_iff = 10/5*(frf_iff.FFT1_H1_4_1_RMS_Y_Mod + frf_iff.FFT1_H1_7_1_RMS_Y_Mod + frf_iff.FFT1_H1_10_1_RMS_Y_Mod + frf_iff.FFT1_H1_13_1_RMS_Y_Mod + frf_iff.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_iff.FFT1_H1_16_1_RMS_X_Val).^2;
|
|
#+end_src
|
|
|
|
The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure [[fig:compliance_vertical_comp_iff]].
|
|
|
|
#+begin_src matlab :exports none
|
|
figure;
|
|
hold on;
|
|
plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, 'DisplayName', 'OL');
|
|
plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, 'DisplayName', 'IFF');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]');
|
|
xlim([20, 2e3]); ylim([2e-9, 2e-5]);
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/compliance_vertical_comp_iff.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:compliance_vertical_comp_iff
|
|
#+caption: Measured vertical compliance with and without IFF
|
|
#+RESULTS:
|
|
[[file:figs/compliance_vertical_comp_iff.png]]
|
|
|
|
#+begin_important
|
|
From Figure [[fig:compliance_vertical_comp_iff]], it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapode.
|
|
It also has the effect of degrading (slightly) the vertical compliance at low frequency.
|
|
|
|
It also seems some damping can be added to the modes at around 205Hz which are flexible modes of the struts.
|
|
#+end_important
|
|
|
|
*** Comparison with the Simscape Model
|
|
Let's now compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model.
|
|
|
|
The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model.
|
|
#+begin_src matlab :exports none
|
|
%% Identify the IFF Plant (transfer function from u to taum)
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fz_ext'], 1, 'openinput'); io_i = io_i + 1; % External - Vertical force
|
|
io(io_i) = linio([mdl, '/Z_top_plat'], 1, 'openoutput'); io_i = io_i + 1; % Absolute vertical motion of top platform
|
|
|
|
%% Initialize Nano-Hexapod in Open Loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
|
|
G_compl_z_ol = linearize(mdl, io, 0.0, options);
|
|
|
|
%% Initialize Nano-Hexapod with IFF
|
|
Kiff = 400*(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
|
|
(s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
|
|
(1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
|
|
eye(6); % Diagonal 6x6 controller
|
|
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'iff');
|
|
|
|
G_compl_z_iff = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The comparison is done in Figure [[fig:compliance_vertical_comp_model_iff]].
|
|
Again, the model is quite accurate!
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the measured compliance and the one obtained from the model
|
|
freqs = 2*logspace(1,3,1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, '-', 'DisplayName', 'OL - Meas.');
|
|
plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, '-', 'DisplayName', 'IFF - Meas.');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, abs(squeeze(freqresp(G_compl_z_ol, freqs, 'Hz'))), '--', 'DisplayName', 'OL - Model')
|
|
plot(freqs, abs(squeeze(freqresp(G_compl_z_iff, freqs, 'Hz'))), '--', 'DisplayName', 'IFF - Model')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]');
|
|
xlim([20, 2e3]); ylim([2e-9, 2e-5]);
|
|
legend('location', 'northeast', 'FontSize', 8);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/compliance_vertical_comp_model_iff.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:compliance_vertical_comp_model_iff
|
|
#+caption: Measured vertical compliance with and without IFF
|
|
#+RESULTS:
|
|
[[file:figs/compliance_vertical_comp_model_iff.png]]
|
|
|
|
*** Obtained Mode Shapes
|
|
Then, several excitation are performed using the instrumented Hammer and the mode shapes are extracted.
|
|
|
|
We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure [[fig:mode_shapes_annotated]].
|
|
|
|
#+name: fig:mode_shapes_annotated
|
|
#+caption: Measured mode shapes for the first six modes
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/mode_shapes_annotated.gif]]
|
|
|
|
Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure [[fig:mode_shapes_annotated]]).
|
|
|
|
#+name: fig:mode_shapes_annotated
|
|
#+caption: First flexible mode at 692Hz
|
|
#+attr_latex: :width 0.3\linewidth
|
|
[[file:figs/ModeShapeFlex1_crop.gif]]
|
|
|
|
The obtained modes are summarized in Table [[tab:description_modes]].
|
|
|
|
#+name: tab:description_modes
|
|
#+caption: Description of the identified modes
|
|
#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccX
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| Mode | Freq. [Hz] | Description |
|
|
|------+------------+----------------------------------------------|
|
|
| 1 | 105 | Suspension Mode: Y-translation |
|
|
| 2 | 107 | Suspension Mode: X-translation |
|
|
| 3 | 131 | Suspension Mode: Z-translation |
|
|
| 4 | 161 | Suspension Mode: Y-tilt |
|
|
| 5 | 162 | Suspension Mode: X-tilt |
|
|
| 6 | 180 | Suspension Mode: Z-rotation |
|
|
| 7 | 692 | (flexible) Membrane mode of the top platform |
|
|
|
|
* Encoders fixed to the plates
|
|
<<sec:encoders_plates>>
|
|
|
|
** Introduction :ignore:
|