2690 lines
88 KiB
Org Mode
2690 lines
88 KiB
Org Mode
#+TITLE: Nano-Hexapod on top of a Spindle - 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, bibliography=totoc]
|
|
#+LATEX_HEADER: \input{preamble.tex}
|
|
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
|
|
#+LATEX_HEADER_EXTRA: \bibliography{test-bench-nass-spindle.bib}
|
|
|
|
#+BIND: org-latex-bib-compiler "biber"
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :exports none
|
|
#+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:matlab+ :tangle no
|
|
|
|
#+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:
|
|
|
|
#+latex: \clearpage
|
|
|
|
* Build :noexport:
|
|
#+NAME: startblock
|
|
#+BEGIN_SRC emacs-lisp :results none :tangle no
|
|
(add-to-list 'org-latex-classes
|
|
'("scrreprt"
|
|
"\\documentclass{scrreprt}"
|
|
("\\chapter{%s}" . "\\chapter*{%s}")
|
|
("\\section{%s}" . "\\section*{%s}")
|
|
("\\subsection{%s}" . "\\subsection*{%s}")
|
|
("\\paragraph{%s}" . "\\paragraph*{%s}")
|
|
))
|
|
|
|
|
|
;; Remove automatic org heading labels
|
|
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
|
|
"Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
|
|
(when (org-export-derived-backend-p backend 'latex)
|
|
(replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
|
|
(add-to-list 'org-export-filter-headline-functions
|
|
'my-latex-filter-removeOrgAutoLabels)
|
|
|
|
;; Remove all org comments in the output LaTeX file
|
|
(defun delete-org-comments (backend)
|
|
(loop for comment in (reverse (org-element-map (org-element-parse-buffer)
|
|
'comment 'identity))
|
|
do
|
|
(setf (buffer-substring (org-element-property :begin comment)
|
|
(org-element-property :end comment))
|
|
"")))
|
|
(add-hook 'org-export-before-processing-hook 'delete-org-comments)
|
|
|
|
;; Use no package by default
|
|
(setq org-latex-packages-alist nil)
|
|
(setq org-latex-default-packages-alist nil)
|
|
|
|
;; Do not include the subtitle inside the title
|
|
(setq org-latex-subtitle-separate t)
|
|
(setq org-latex-subtitle-format "\\subtitle{%s}")
|
|
|
|
(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
|
|
org-ref-acronyms-before-parsing))
|
|
#+END_SRC
|
|
|
|
* Notes :noexport:
|
|
** Notes
|
|
Prefix is =test_rot=
|
|
*Goals*:
|
|
- Short stroke metrology with Interferometers to have more stroke
|
|
- Validation of IFF
|
|
- Validation of control architecture with rotation (kinematics)
|
|
- How to estimate Rz? (useful for kinematics): what happens if Rz is ignored (i.e. supposed to be zero)?
|
|
- Effect of rotation on the plant dynamics
|
|
*Notes*:
|
|
- Robustness will be checked on ID31
|
|
- Tests were performed in December 2022 and January/February 2023
|
|
- All that can be explained here before the ID31 tests is nice
|
|
- 20 pages maximum should be good enough
|
|
- Should I speak here about the Simscape model? (Maybe not useful)
|
|
|
|
- [ ] Explain that beam time is complex to have, so first tests are made in the lab.
|
|
- [ ] talk about the metrology system.
|
|
Comparison with LION precision system.
|
|
- [ ] Find pictures with additional mass with the metrology? (I don't find them)
|
|
|
|
** Alignment
|
|
|
|
Optimal distance for top Attocube: 179.67 (distance between reference surface and bottom surface of LION sphere)
|
|
|
|
Nouvelle mesure après alignement
|
|
|
|
aligné à mieux que 0.1mm en XYZ:
|
|
- XY tête du haut / axe LION
|
|
- XZ tête face / axe lion
|
|
- YZ tête droite / axe lion
|
|
|
|
* Introduction :ignore:
|
|
|
|
As the different beamlines are running 24/7
|
|
It is difficult to have access to the micro station to perform tests.
|
|
The only slot available is 3 weeks during the summer.
|
|
|
|
Before the tests on ID31:
|
|
- Development of a 5DoF metrology system
|
|
- Make sure all the kinematic is working properly
|
|
|
|
|
|
#+name: fig:picture_setup
|
|
#+caption: Setup with the Spindle, nano-hexapod and metrology
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/IMG_20221220_152429.jpg]]
|
|
|
|
* Test-Bench Description
|
|
** Introduction :ignore:
|
|
#+begin_note
|
|
Here are the documentation of the equipment used for this test bench:
|
|
- 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][IO131]]
|
|
- 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]]
|
|
- LION Precision [[file:doc/Catalog-CPL190290.pdf][CPL290]]
|
|
- Spindle: Lab Motion [[file:doc/RS250S.pdf][RT250S]] with [[file:doc/DB36_connections_english_V2.pdf][Drivebox 3.6]] controller
|
|
#+end_note
|
|
|
|
** 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 :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Alignment
|
|
|
|
Procedure:
|
|
1. Align bottom sphere with the spindle rotation axis (~ 10um)
|
|
2. Align top sphere with the spindle rotation axis (~ 10um)
|
|
|
|
** Short Range metrology system
|
|
|
|
There are 5 interferometers pointing at 2 spheres as shown in Figure ref:fig:LION_metrology_interferometers.
|
|
|
|
#+name: fig:picture_metrology
|
|
#+caption: Metrology system with LION sphere (1 inch diameter) and 5 interferometers fixed to their individual tip-tilts
|
|
#+attr_latex: :width 0.5\linewidth
|
|
[[file:figs/IMG_20221216_181305.jpg]]
|
|
|
|
| | Value |
|
|
|------------------------------+--------|
|
|
| Sphere Diameter | 25.4mm |
|
|
| Distance between the spheres | 76.2mm |
|
|
|
|
#+name: fig:LION_metrology_interferometers
|
|
#+caption: Schematic of the measurement system
|
|
#+attr_latex: :width 0.5\linewidth
|
|
[[file:figs/LION_metrology_interferometers.png]]
|
|
*Assumptions*:
|
|
- Interferometers are perfectly positioned / oriented
|
|
- Sphere is perfect
|
|
*Compute the Jacobian matrix*:
|
|
- From pure X-Y-Z-Rx-Ry small motions, compute the effect on the 5 measured distances
|
|
- Compute the matrix
|
|
- Inverse the matrix
|
|
- Verify that it is working with simple example (for example using Solidworkds)
|
|
|
|
We have the following set of equations:
|
|
\begin{align}
|
|
d_1 &= -D_y + l_2 R_x \\
|
|
d_2 &= -D_y - l_1 R_x \\
|
|
d_3 &= -D_x - l_2 R_y \\
|
|
d_4 &= -D_x + l_1 R_y \\
|
|
d_5 &= -D_z
|
|
\end{align}
|
|
|
|
That can be written as a linear transformation:
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
0 & -1 & 0 & l_2 & 0 \\
|
|
0 & -1 & 0 & -l_1 & 0 \\
|
|
-1 & 0 & 0 & 0 & -l_2 \\
|
|
-1 & 0 & 0 & 0 & l_1 \\
|
|
0 & 0 & -1 & 0 & 0
|
|
\end{bmatrix} \cdot \begin{bmatrix}
|
|
D_x \\ D_y \\ D_z \\ R_x \\ R_y
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
By inverting the matrix, we obtain the Jacobian relation:
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
D_x \\ D_y \\ D_z \\ R_x \\ R_y
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
0 & -1 & 0 & l_2 & 0 \\
|
|
0 & -1 & 0 & -l_1 & 0 \\
|
|
-1 & 0 & 0 & 0 & -l_2 \\
|
|
-1 & 0 & 0 & 0 & l_1 \\
|
|
0 & 0 & -1 & 0 & 0
|
|
\end{bmatrix}^{-1} \cdot \begin{bmatrix}
|
|
d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab
|
|
%% Parameters
|
|
H = 150e-3;
|
|
l1 = (150-38-52)*1e-3;
|
|
l2 = (52+38+76.2-150)*1e-3;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Transformation matrix
|
|
Hm = [ 0 -1 0 l2 0;
|
|
0 -1 0 -l1 0;
|
|
-1 0 0 0 -l2;
|
|
-1 0 0 0 l1;
|
|
0 0 -1 0 0];
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
|
data2orgtable(pinv(Hm), {'*Dx*', '*Dy*', '*Dz*', '*Rx*', '*Ry*'}, {'*d1*', '*d2*', '*d3*', '*d4*', '*d5*'}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+name: tab:jacobian_metrology
|
|
#+caption: Jacobian matrix for the metrology system
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| | *d1* | *d2* | *d3* | *d4* | *d5* |
|
|
|------+-------+--------+--------+-------+------|
|
|
| *Dx* | 0.0 | 0.0 | -0.79 | -0.21 | 0.0 |
|
|
| *Dy* | -0.79 | -0.21 | -0.0 | -0.0 | 0.0 |
|
|
| *Dz* | 0.0 | 0.0 | 0.0 | 0.0 | -1.0 |
|
|
| *Rx* | 13.12 | -13.12 | 0.0 | -0.0 | 0.0 |
|
|
| *Ry* | 0.0 | 0.0 | -13.12 | 13.12 | 0.0 |
|
|
|
|
** Spindle errors
|
|
*** Introduction :ignore:
|
|
|
|
The spindle is rotated at 60rpm during 10 turns.
|
|
The signal of all 5 interferometers are recorded.
|
|
|
|
#+begin_src matlab
|
|
data = load(sprintf('%s/spindle/mat/2022-12-20_15-47_sec_test.mat', data_dir));
|
|
#+end_src
|
|
|
|
*** Errors in $D_x$ and $D_y$
|
|
|
|
Because of the eccentricity of the reference surfaces (the spheres), we expect the motion in the X-Y plane to be a circle as a first approximation.
|
|
We can first see that in Figure ref:fig:dx_dy_motion_rotation that shows the measured $D_x$ and $D_y$ motion as a function of the $R_z$ angle.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Dz motion during the rotation
|
|
figure;
|
|
hold on;
|
|
plot(1/2/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dx_int, 0), 'DisplayName', '$D_x$')
|
|
plot(1/2/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dy_int, 0), 'DisplayName', '$D_y$')
|
|
hold off;
|
|
xlim([0, 10])
|
|
xlabel('Rotation [turn]'); ylabel('$D_z$ motion [$\mu$m]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dx_dy_motion_rotation.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:dx_dy_motion_rotation
|
|
#+caption: Dx and Dy motion during the rotation
|
|
#+RESULTS:
|
|
[[file:figs/dx_dy_motion_rotation.png]]
|
|
|
|
#+begin_src matlab
|
|
%% Circle Fit
|
|
[xc,yc,R,a] = circlefit(data.Dx_int, data.Dy_int);
|
|
#+end_src
|
|
|
|
A circle is fit, and the obtained radius of the circle (i.e. the excentricity) is estimated to be:
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('Error linked to excentricity = %.0f um', 1e6*R);
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: Error linked to excentricity = 19 um
|
|
|
|
The motion in the X-Y plane as well as the circle fit and the residual motion (circle fit subtracted from the measured motion) are shown in Figure ref:fig:dx_dy_spindle_rotation.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Dx and Dy motion during the spindle rotation
|
|
theta = linspace(0, 2*pi, 1000);
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(1e6*detrend(data.Dx_int, 0), 1e6*detrend(data.Dy_int, 0), 'DisplayName', 'Raw data')
|
|
plot(1e6*detrend(xc + R*cos(theta), 0), 1e6*detrend(yc + R*sin(theta), 0), '--', 'DisplayName', 'fit')
|
|
plot(1e6*(data.Dx_int - (xc + R*cos(data.Rz-0.07))), 1e6*(data.Dy_int - (yc + R*sin(data.Rz-0.07))), 'k.', 'DisplayName', 'Residual')
|
|
hold off;
|
|
xlabel('$D_x$ [$\mu$m]')
|
|
ylabel('$D_y$ [$\mu$m]')
|
|
axis equal
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile();
|
|
plot(1e6*(data.Dx_int - (xc + R*cos(data.Rz-0.07))), 1e6*(data.Dy_int - (yc + R*sin(data.Rz-0.07))), 'k.')
|
|
xlabel('$D_x$ [$\mu$m]')
|
|
ylabel('$D_y$ [$\mu$m]')
|
|
axis equal
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dx_dy_spindle_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:dx_dy_spindle_rotation
|
|
#+caption: Dx and Dy motion during the spindle rotation
|
|
#+RESULTS:
|
|
[[file:figs/dx_dy_spindle_rotation.png]]
|
|
|
|
Let's now analyse the frequency content in the signal.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Hanning window used for pwelch function
|
|
Ts = 1e-4;
|
|
win = hanning(5/Ts);
|
|
|
|
%% Compute PSD of initial motion error
|
|
[S_Dx, f] = pwelch(detrend(data.Dx_int, 0), win, 0, [], 1/Ts);
|
|
[S_Dy, ~] = pwelch(detrend(data.Dy_int, 0), win, 0, [], 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Amplitude Spectral Density of the measured Dx and Dy motion
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(S_Dx), 'DisplayName', '$D_x$');
|
|
plot(f, sqrt(S_Dy), 'DisplayName', '$D_y$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([0.1, 1e3]); ylim([1e-11, 1e-4]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dx_dy_spindle_rotation_asd.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:dx_dy_spindle_rotation_asd
|
|
#+caption: Amplitude Spectral Density of the measured Dx and Dy motion
|
|
#+RESULTS:
|
|
[[file:figs/dx_dy_spindle_rotation_asd.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Dx)))), 'DisplayName', '$D_x$');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Dy)))), 'DisplayName', '$D_y$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([0.1, 1e3]); ylim([1e-9, 2e-5]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dx_dy_spindle_rotation_cas.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:dx_dy_spindle_rotation_cas
|
|
#+caption: Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
|
#+RESULTS:
|
|
[[file:figs/dx_dy_spindle_rotation_cas.png]]
|
|
|
|
*** Errors in vertical motion $D_z$
|
|
|
|
The top interferometer is measuring the vertical motion of the sphere.
|
|
|
|
However, if the top sphere is not perfectly aligned with the spindle axis, there will also measure some vertical motion due to this excentricity.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Dz motion during the rotation
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
plot(1/2/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dz_int, 0), '-')
|
|
xlim([0, 10])
|
|
xlabel('Rotation [turn]'); ylabel('$D_z$ motion [$\mu$m]');
|
|
|
|
ax1 = nexttile();
|
|
plot(180/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dz_int, 0), '-')
|
|
xlim([0, 90])
|
|
xlabel('Rotation [deg]'); ylabel('$D_z$ motion [$\mu$m]');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dz_motion_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:dz_motion_rotation
|
|
#+caption: Dz motion during the rotation
|
|
#+RESULTS:
|
|
[[file:figs/dz_motion_rotation.png]]
|
|
|
|
Let's fit a sinus with a period of one turn.
|
|
|
|
#+begin_src matlab
|
|
%% Fit Sinus with period of one turn
|
|
x1 = data.Rz(data.t<1);
|
|
y1 = data.Dz_int(data.t<1);
|
|
|
|
fit = @(b,x) b(1).*sin(x + b(2)) + b(3); % Function to fit
|
|
fcn = @(b) sum((fit(b,x1) - y1).^2); % Least-Squares cost function
|
|
|
|
s1 = fminsearch(fcn, [1e-6; 30; 1.5e-6]) % Minimise Least-Squares
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('Errors linked to excentricity = %.0f [nm]', 1e9*s1(1));
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: Errors linked to excentricity = 410 [nm]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Effect of the excentricity and remaining Dz motion
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(180/pi*x1, 1e6*y1, '.', 'DisplayName', 'Raw')
|
|
plot(180/pi*x1, 1e6*fit(s1, x1), '.', 'DisplayName', 'Fit')
|
|
hold off;
|
|
xlabel('$R_z$ [deg]')
|
|
ylabel('$D_z$ [$\mu$m]')
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([0, 360]);
|
|
|
|
ax2 = nexttile();
|
|
plot(180/pi*x1, 1e9*(y1 - fit(s1, x1)), 'k.')
|
|
xlabel('$R_z$ [deg]')
|
|
ylabel('$D_z$ [$\mu$m]')
|
|
xlim([0, 360]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dz_motion_rotation_excentricity.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:dz_motion_rotation_excentricity
|
|
#+caption: Effect of the excentricity and remaining Dz motion
|
|
#+RESULTS:
|
|
[[file:figs/dz_motion_rotation_excentricity.png]]
|
|
|
|
If we look at the remaining motion after removing the effect of the eccentricity (Figure ref:fig:dz_motion_rotation_excentricity, right), we can see a signal with 20 periods every turn.
|
|
Let's fit this.
|
|
|
|
#+begin_src matlab
|
|
%% Fit Sinus with period of 18 degrees (one electrical period)
|
|
x2 = data.Rz(data.t<1)*20;
|
|
y2 = y1 - fit(s1, x1);
|
|
|
|
fit = @(b,x) b(1).*sin(x + b(2)) + b(3); % Function to fit
|
|
fcn = @(b) sum((fit(b,x2) - y2).^2); % Least-Squares cost function
|
|
|
|
s2 = fminsearch(fcn, [50e-9; 30; 0]) % Minimise Least-Squares
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('Errors linked to spindle motor = %.0f [nm]', 1e9*s2(1));
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: Errors linked to spindle motor = 58 [nm]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Effect of the poles and remaining Dz motion
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(180/pi*x1, 1e6*y2, '.', 'DisplayName', 'Raw')
|
|
plot(180/pi*x1, 1e6*fit(s2, x2), '.', 'DisplayName', 'Fit')
|
|
hold off;
|
|
xlabel('$R_z$ [deg]')
|
|
ylabel('$D_z$ [$\mu$m]')
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([0, 360]);
|
|
|
|
ax2 = nexttile();
|
|
plot(180/pi*x1, 1e9*(y2 - fit(s2, x2)), 'k.')
|
|
xlabel('$R_z$ [deg]')
|
|
ylabel('$D_z$ [$\mu$m]')
|
|
xlim([0, 360]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dz_motion_rotation_poles.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:dz_motion_rotation_poles
|
|
#+caption: Effect of the magnetic pole pairs and remaining Dz motion
|
|
#+RESULTS:
|
|
[[file:figs/dz_motion_rotation_poles.png]]
|
|
|
|
Let's look at the signal in the frequency domain.
|
|
|
|
On top of the peak at 1Hz (excentricity) and at 20Hz (number of pole pairs), we can observe a frequency of 126Hz (i.e. 126 periods per turn, approx 2.85 deg).
|
|
|
|
#+begin_quote
|
|
Could this be related to the air bearing system?
|
|
#+end_quote
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Hanning window used for pwelch function
|
|
Ts = 1e-4;
|
|
win = hanning(5/Ts);
|
|
|
|
%% Compute PSD of initial motion error
|
|
[S_Dz, f] = pwelch(detrend(data.Dz_int, 0), win, 0, [], 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Amplitude Spectral Density of the measured Dz motion
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(S_Dz));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('ASD of $D_z$ [$m/\sqrt{Hz}$]');
|
|
xlim([0.1, 1e3]); ylim([1e-11, 1e-4]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dz_spindle_rotation_asd.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:dz_spindle_rotation_asd
|
|
#+caption: Amplitude Spectral Density of the measured Dz motion
|
|
#+RESULTS:
|
|
[[file:figs/dz_spindle_rotation_asd.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Dx)))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('CAS of $D_z$ [$m$]');
|
|
xlim([0.1, 1e3]); ylim([1e-9, 2e-5]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/dz_spindle_rotation_cas.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:dz_spindle_rotation_cas
|
|
#+caption: Cumulative Amplitude Spectrum of the measured Dz motion
|
|
#+RESULTS:
|
|
[[file:figs/dz_spindle_rotation_cas.png]]
|
|
|
|
*** Angle errors in $R_x$ and $R_y$
|
|
|
|
#+begin_src matlab
|
|
[xc,yc,R,a] = circlefit(data.Rx_int, data.Ry_int);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('amplitude = %.0f urad', 1e6*R);
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: amplitude = 281 urad
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Rx and Ry motion during the spindle rotation
|
|
theta = linspace(0, 2*pi, 1000);
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(1e6*detrend(data.Rx_int, 0), 1e6*detrend(data.Ry_int, 0), 'DisplayName', 'Raw data')
|
|
plot(1e6*detrend(xc + R*cos(theta), 0), 1e6*detrend(yc + R*sin(theta), 0), '--', 'DisplayName', 'fit')
|
|
plot(1e6*(data.Rx_int - (xc + R*cos(data.Rz+2.97))), 1e6*(data.Ry_int - (yc + R*sin(data.Rz+2.97))), 'k.', 'DisplayName', 'Residual')
|
|
hold off;
|
|
xlabel('$R_x$ [$\mu$rad]')
|
|
ylabel('$R_y$ [$\mu$rad]')
|
|
axis equal
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile();
|
|
plot(1e6*(data.Rx_int - (xc + R*cos(data.Rz+2.97))), 1e6*(data.Ry_int - (yc + R*sin(data.Rz+2.97))), 'k.')
|
|
xlabel('$R_x$ [$\mu$rad]')
|
|
ylabel('$R_y$ [$\mu$rad]')
|
|
axis equal
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/rx_ry_spindle_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:rx_ry_spindle_rotation
|
|
#+caption: Rx and Ry motion during the spindle rotation
|
|
#+RESULTS:
|
|
[[file:figs/rx_ry_spindle_rotation.png]]
|
|
|
|
Let's now analyse the frequency content in the signal.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Hanning window used for pwelch function
|
|
Ts = 1e-4;
|
|
win = hanning(5/Ts);
|
|
|
|
%% Compute PSD of initial motion error
|
|
[S_Rx, f] = pwelch(detrend(data.Rx_int, 0), win, 0, [], 1/Ts);
|
|
[S_Ry, ~] = pwelch(detrend(data.Ry_int, 0), win, 0, [], 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Amplitude Spectral Density of the measured Rx and Ry motion
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(S_Rx), 'DisplayName', '$D_x$');
|
|
plot(f, sqrt(S_Ry), 'DisplayName', '$D_y$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('ASD [rad$/\sqrt{Hz}$]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([0.1, 1e3]); ylim([1e-10, 1e-3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/rx_ry_spindle_rotation_asd.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:rx_ry_spindle_rotation_asd
|
|
#+caption: Amplitude Spectral Density of the measured Rx and Ry motion
|
|
#+RESULTS:
|
|
[[file:figs/rx_ry_spindle_rotation_asd.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Cumulative Amplitude Spectrum of the measured Rx and Ry motion
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Rx)))), 'DisplayName', '$D_x$');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Ry)))), 'DisplayName', '$D_y$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [rad]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([0.1, 1e3]); ylim([5e-8, 1e-3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/rx_ry_spindle_rotation_cas.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:rx_ry_spindle_rotation_cas
|
|
#+caption: Cumulative Amplitude Spectrum of the measured Rx and Ry motion
|
|
#+RESULTS:
|
|
[[file:figs/rx_ry_spindle_rotation_cas.png]]
|
|
|
|
|
|
* Simscape Model
|
|
** Introduction :ignore:
|
|
|
|
A 3D view of the Simscape model is shown in Figure ref:fig:simscape_model_spindle_bench.
|
|
The Spindle is represented by a /Bushing joint/.
|
|
Axial, radial and tilt stiffnesses are taken from the Spindle datasheet (see Table).
|
|
|
|
#+name: tab:spindle_stiffnesses
|
|
#+caption: Spindle stiffnesses
|
|
#+attr_latex: :environment tabularx :width \linewidth :align lXX
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Stiffness* | *Value* | *Unit* |
|
|
|-------------+---------+-----------|
|
|
| Axial | 402 | $N/\mu m$ |
|
|
| Radial | 226 | $N/\mu m$ |
|
|
| Tilt | 2380 | $Nm/mrad$ |
|
|
|
|
The metrology system consists of 5 distance measurements (represented by the red lines in Figure ref:fig:simscape_model_spindle_bench).
|
|
|
|
#+name: fig:simscape_model_spindle_bench
|
|
#+caption: Screenshot of the 3D view of the Simscape model
|
|
#+attr_latex: :width 0.5\linewidth
|
|
[[file:figs/simscape_model_spindle_bench.jpg]]
|
|
|
|
** 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 :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Frequency vector for further use
|
|
freqs = logspace(0,3,1000);
|
|
|
|
%% Start angle of the Spindle
|
|
start_angle = 0;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
%% Add path for Nano-Hexapod Simscape model
|
|
addpath('./matlab/nass-simscape/STEPS/nano_hexapod')
|
|
addpath('./matlab/nass-simscape/matlab/nano_hexapod')
|
|
addpath('./matlab/nass-simscape/src')
|
|
addpath('./matlab/nass-simscape/mat')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Add path for Nano-Hexapod Simscape model
|
|
addpath('./nass-simscape/STEPS/nano_hexapod')
|
|
addpath('./nass-simscape/matlab/nano_hexapod')
|
|
addpath('./nass-simscape/src')
|
|
addpath('./nass-simscape/mat')
|
|
#+end_src
|
|
|
|
** Simscape model parameters
|
|
The nano-hexapod is initialized.
|
|
|
|
#+begin_src matlab
|
|
%% Initialize Nano-Hexapod
|
|
n_hexapod = initializeNanoHexapodFinal('MO_B', 150e-3, ...
|
|
'actuator_type', '2dof', ...
|
|
'motion_sensor_type', 'plates');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Initialize Spindle
|
|
spindle = struct();
|
|
|
|
%% Stiffnesses
|
|
spindle.kx = 226e6; % Radial stiffness in [N/m]
|
|
spindle.ky = 226e6; % Radial stiffness in [N/m]
|
|
spindle.kz = 402e6; % Axial stiffness in [N/m]
|
|
spindle.krx = 2.38e6; % Tilt stiffness in [Nm/rad]
|
|
spindle.kry = 2.38e6; % Tilt stiffness in [Nm/rad]
|
|
spindle.krz = 0; % Rotation stiffness in [Nm/rad]
|
|
|
|
%% Damping
|
|
spindle.cx = 1/(2*0.01)/(sqrt(spindle.kx*50)); % Radial damping in [N/(m/s)]
|
|
spindle.cy = 1/(2*0.01)/(sqrt(spindle.ky*50)); % Radial damping in [N/(m/s)]
|
|
spindle.cz = 1/(2*0.01)/(sqrt(spindle.kz*50)); % Axial damping in [N/(m/s)]
|
|
spindle.crx = 1/(2*0.01)/(sqrt(spindle.krx*50)); % Tilt damping in [Nm/(rad/s)]
|
|
spindle.cry = 1/(2*0.01)/(sqrt(spindle.kry*50)); % Tilt damping in [Nm/(rad/s)]
|
|
spindle.crz = 0; % Rotation damping in [Nm/(rad/s)]
|
|
#+end_src
|
|
|
|
The Jacobian matrix that computes the $[x, y, z, R_x, R_y]$ motion of the sample from the 5 interferometers is defined below.
|
|
|
|
#+begin_src matlab
|
|
% Sensor Jacobian (Interferometers to Cartesian motion)
|
|
J_int_to_X_ = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
|
-0.78740157480315 -0.21259842519685 0 0 0;
|
|
0 0 0 0 -1;
|
|
13.1233595800525 -13.1233595800525 0 0 0;
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Open the Simscape model
|
|
mdl = 'spindle_test_bench'
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
** Control Architecture
|
|
|
|
Let's note:
|
|
- $d\mathcal{L}_m = [d_{\mathcal{L}_1},\ d_{\mathcal{L}_2},\ d_{\mathcal{L}_3},\ d_{\mathcal{L}_4},\ d_{\mathcal{L}_5},\ d_{\mathcal{L}_6}]$ the measurement of the 6 encoders fixed to the nano-hexapod
|
|
- $\bm{\tau}_m = [\tau_{m_1},\ \tau_{m_2},\ \tau_{m_3},\ \tau_{m_4},\ \tau_{m_5},\ \tau_{m_6}]$ the voltages measured by the 6 force sensors
|
|
- $\bm{u} = [u_1,\ u_2,\ u_3,\ u_4,\ u_5,\ u_6]$ the voltages send to the voltage amplifiers for the 6 piezoelectric actuators
|
|
- $R_z$ the spindle measured angle (encoder)
|
|
- $\bm{d}_m = [d_1,\ d_2,\ d_3,\ d_4,\ d_5]$ the distances measured by the 5 interferometers (see Figure ref:fig:LION_metrology_interferometers_bis)
|
|
|
|
#+name: fig:LION_metrology_interferometers_bis
|
|
#+caption: Schematic of the measurement system
|
|
#+attr_latex: :width 0.5\linewidth
|
|
[[file:figs/LION_metrology_interferometers.png]]
|
|
|
|
** Computation of the strut errors from the external metrology
|
|
|
|
The following frames are defined:
|
|
- $\{ W \}$: the frame that represents the wanted pose of the sample
|
|
- $\{ M \}$: the frame that represents the measured pose of the sample (estimated from the 5 interferometers and the spindle encoder)
|
|
- $\{ G \}$: the frame fixed to the granite and positioned at the sample's center
|
|
- $\{ H \}$: the frame fixed to the the spindle rotor, and positioned at the sample's center
|
|
|
|
We can express several homogeneous transformation matrices.
|
|
|
|
Frame fixed to the spindle rotor (centered on the sample's position), expressed in the frame of the granite:
|
|
\begin{equation}
|
|
{}^{G}\bm{T}_H = \begin{bmatrix}
|
|
cos(R_z) & -sin(R_z) & 0 & 0 \\
|
|
sin(R_z) & cos(R_z) & 0 & 0 \\
|
|
0 & 0 & 1 & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
with $R_z$ the spindle encoder.
|
|
|
|
|
|
Wanted position expressed in the frame of the granite:
|
|
\begin{equation}
|
|
{}^{G}\bm{T}_W = \begin{bmatrix}
|
|
& & & r_{D_x} \\
|
|
& \bm{R}_x(r_{R_x}) \bm{R}_y(r_{R_y}) \bm{R}_z(r_{R_z}) & & r_{D_y} \\
|
|
& & & r_{D_z} \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
with $\bm{R}(r_{R_x}, r_{R_y}, r_{R_z})$ representing the wanted orientation of the sample with respect to the granite.
|
|
Typically, $r_{R_x} = 0$, $r_{R_y} = 0$ and $r_{R_z}$ corresponds to the spindle encoder $R_z$.
|
|
|
|
|
|
Measured position of the sample with respect to the granite:
|
|
\begin{equation}
|
|
{}^{G}\bm{T}_M = \begin{bmatrix}
|
|
& & & y_{D_x} \\
|
|
& \bm{R}_x(y_{R_x}) \bm{R}_y(y_{R_y}) \bm{R}_z(R_z) & & y_{D_y} \\
|
|
& & & y_{D_z} \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
with $R_z$ the spindle encoder, and $[y_{D_x},\ y_{D_y},\ y_{D_z},\ y_{R_x},\ y_{R_y}]$ are obtained from the 5 interferometers:
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
y_{D_x} \\ y_{D_y} \\ y_{D_z} \\ y_{R_x} \\ y_{R_y}
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
0 & -1 & 0 & l_2 & 0 \\
|
|
0 & -1 & 0 & -l_1 & 0 \\
|
|
-1 & 0 & 0 & 0 & -l_2 \\
|
|
-1 & 0 & 0 & 0 & l_1 \\
|
|
0 & 0 & -1 & 0 & 0
|
|
\end{bmatrix}^{-1} \cdot \begin{bmatrix}
|
|
d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
|
|
In order to have the *position error in the frame of the nano-hexapod*, we have to compute ${}^M\bm{T}_W$:
|
|
\begin{align}
|
|
{}^M\bm{T}_W &= {}^M\bm{T}_G \cdot {}^G\bm{T}_W \\
|
|
&= {{}^G\bm{T}_M}^{-1} \cdot {}^G\bm{T}_W
|
|
\end{align}
|
|
|
|
The *inverse of the transformation matrix* can be obtained by
|
|
\begin{equation}
|
|
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & \\
|
|
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
|
|
& & & \cr
|
|
\hline
|
|
0 & 0 & 0 & 1 \\
|
|
\end{array} \right]
|
|
\end{equation}
|
|
|
|
The position errors $\bm{\epsilon}_{\mathcal{X}} = [\epsilon_{D_x},\ \epsilon_{D_y},\ \epsilon_{D_z},\ \epsilon_{R_x},\ \epsilon_{R_y},\ \epsilon_{R_z}]$ expressed in a frame fixed to the nano-hexapod can be extracted from ${}^W\bm{T}_M$:
|
|
- $\epsilon_{D_x} = {}^M\bm{T}_W(1,4)$
|
|
- $\epsilon_{D_y} = {}^M\bm{T}_W(2,4)$
|
|
- $\epsilon_{D_z} = {}^M\bm{T}_W(3,4)$
|
|
- $\epsilon_{R_y} = \text{atan2}({}^M\bm{T}_W(1,3), \sqrt{{}^M\bm{T}_W(1,1)^2 + {}^M\bm{T}_W(1,2)^2})$
|
|
- $\epsilon_{R_x} = \text{atan2}(\frac{-{}^M\bm{T}_W(2,3)}{\cos(\epsilon_{R_y})}, \frac{{}^M\bm{T}_W(3,3)}{\cos(\epsilon_{R_y})})$
|
|
- $\epsilon_{R_z} = \text{atan2}(\frac{-{}^M\bm{T}_W(1,2)}{\cos(\epsilon_{R_y})}, \frac{{}^M\bm{T}_W(1,1)}{\cos(\epsilon_{R_y})})$
|
|
|
|
Finally, the strut errors $\bm{\epsilon}_{\mathcal{L}} = [\epsilon_{\matcal{L}_1},\ \epsilon_{\matcal{L}_2},\ \epsilon_{\matcal{L}_3},\ \epsilon_{\matcal{L}_4},\ \epsilon_{\matcal{L}_5},\ \epsilon_{\matcal{L}_6}]$ can be computed from:
|
|
\begin{equation}
|
|
\bm{\epsilon}_\mathcal{L} = \bm{J} \cdot \bm{\epsilon}_\mathcal{X}
|
|
\end{equation}
|
|
|
|
** IFF Plant
|
|
#+begin_src matlab
|
|
start_angle = 0; % [deg]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Identify the 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
|
|
|
|
%% Perform the model extraction
|
|
G_iff = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% IFF plant obtained on the Simscape model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_iff(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\\tau_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [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(freqs, 180/pi*angle(squeeze(freqresp(G_iff(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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
** DVF Plant
|
|
#+begin_src matlab
|
|
start_angle = 0; % [deg]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Identify the 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, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
|
|
|
%% Perform the model extraction
|
|
G_dvf = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% DVF plant obtained on the Simscape model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(freqs, abs(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$d\\mathcal{L}_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_dvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [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(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
** HAC Plant
|
|
The transfer functions from the 6 actuator inputs to the 6 estimated strut errors are extracted from the Simscape model.
|
|
|
|
#+begin_src matlab
|
|
start_angle = 0; % [deg]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Identify the 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, '/J_X_to_L'], 1, 'openoutput'); io_i = io_i + 1; % Estimated Strut Error
|
|
|
|
%% Perform the model extraction
|
|
G = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The obtained transfer functions are shown in Figure ref:fig:simscape_model_hac_plant.
|
|
|
|
We can see that the system is well decoupled at low frequency (i.e. below the first resonance of the Nano-Hexapod).
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% HAC plant obtained on the Simscape model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(freqs, abs(squeeze(freqresp(G(i, i), freqs, 'Hz'))), ...
|
|
'DisplayName', sprintf('$d\\mathcal{L}_%i/u_%i$', i, i));
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_i/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [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(freqs, 180/pi*angle(squeeze(freqresp(G(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([1e1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/simscape_model_hac_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:simscape_model_hac_plant
|
|
#+caption: HAC plant obtained on the Simscape model
|
|
#+RESULTS:
|
|
[[file:figs/simscape_model_hac_plant.png]]
|
|
|
|
** Save Plants :noexport:
|
|
#+begin_src matlab :tangle no
|
|
save('matlab/mat/simscape_plants.mat', 'G_iff', 'G_dvf', 'G');
|
|
#+end_src
|
|
|
|
* Control Experiment
|
|
** 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 :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Frequency vector for further use
|
|
freqs = logspace(0,3,1000);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
data_path = '/home/thomas/mnt/data_mel/NASS';
|
|
#+end_src
|
|
|
|
** IFF Plant
|
|
#+begin_src matlab
|
|
%% Load identification data
|
|
data = load(sprintf('%s/dynamics/2023-02-01_15-21_identification_new_matrices_long_bis.mat', data_path));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Load extracted plants from Simscape
|
|
model = load('simscape_plants.mat', 'G_iff', 'G_dvf', 'G');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
% Sampling Time [s]
|
|
Ts = 1e-4;
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1/Ts));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% IFF Plant (transfer function from u to taum)
|
|
G_iff = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
eL = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 1:6
|
|
plot(f, abs(G_iff(:,i, i)), 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\tau_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
% ylim([1e-8, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_iff(:,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);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/G_iif_exp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:G_iif_exp_no_rotation
|
|
#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
|
#+RESULTS:
|
|
[[file:figs/G_iif_exp_no_rotation.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison with the model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, abs(G_iff(:,i, i)), '-', 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
|
end
|
|
for i = 1:6
|
|
plot(f, abs(squeeze(freqresp(model.G_iff(i, i), f, 'Hz'))), '--', 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
% ylim([1e-8, 1e-3]);
|
|
% legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)), '-', 'color', colors(i,:));
|
|
end
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(squeeze(freqresp(model.G_iff(i, i), f, 'Hz'))), '--', 'color', colors(i,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/G_iif_exp_comp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:G_iif_exp_comp_no_rotation
|
|
#+caption: Comparison with the model
|
|
#+RESULTS:
|
|
[[file:figs/G_iif_exp_comp_no_rotation.png]]
|
|
|
|
** IFF Controller
|
|
#+begin_src matlab
|
|
%% IFF Controller
|
|
Kiff_g1 = -(1/(s + 2*pi*40))*... % LPF: provides integral action above 40Hz
|
|
(s/(s + 2*pi*30))*... % HPF: limit low frequency gain
|
|
(1/(1 + s/2/pi/500))*... % LPF: more robust to high frequency resonances
|
|
eye(6); % Diagonal 6x6 controller
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Root Locus for IFF
|
|
gains = logspace(1, 4, 100);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
plot(real(pole(model.G_iff)), imag(pole(model.G_iff)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(model.G_iff)), imag(tzero(model.G_iff)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(model.G_iff, g*Kiff_g1*eye(6), +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
g = 4e2;
|
|
clpoles = pole(feedback(model.G_iff, g*Kiff_g1*eye(6), +1));
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
|
|
'DisplayName', sprintf('$g=%.0f$', g));
|
|
hold off;
|
|
xlim([-1500, 0]); ylim([0, 1500]);
|
|
axis square;
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/root_locus_iff_no_payload.pdf', 'width', 'normal', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:root_locus_iff_no_payload
|
|
#+caption: Root Locus for IFF
|
|
#+RESULTS:
|
|
[[file:figs/root_locus_iff_no_payload.png]]
|
|
|
|
|
|
#+begin_src matlab :exports none
|
|
K = g*Kiff_g1(1,1);
|
|
K_order = order(K(1,1));
|
|
|
|
Kz = c2d(K(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
|
[num, den] = tfdata(Kz, 'v');
|
|
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
|
fileID = fopen('matlab/mat/K_iff_first_test.dat', 'w');
|
|
fprintf(fileID, formatSpec, [num; den]');
|
|
fclose(fileID);
|
|
#+end_src
|
|
|
|
** Open Loop Plant
|
|
Here the $R_z$ motion of the Hexapod is estimated from the encoders.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% IFF Plant (transfer function from u to taum)
|
|
G_hac = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
G_hac(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, 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', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 1:6
|
|
plot(f, abs(G_hac(:,i, i)), 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\epsilon_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(f, abs(G_hac(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\epsilon_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_hac(:,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);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/G_damp_exp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:G_damp_exp_no_rotation
|
|
#+caption: Obtained transfer function from generated voltages to estimated strut motion
|
|
#+RESULTS:
|
|
[[file:figs/G_damp_exp_no_rotation.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the open-loop plant measured experimentally and extracted from Simscape
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(G_hac(:,1, 1)), 'color', colors(1,:), ...
|
|
'DisplayName', 'Model');
|
|
for i = 1:6
|
|
plot(f, abs(G_hac(:,i, i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(squeeze(freqresp(model.G(i, j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(squeeze(freqresp(model.G(1, 1), f, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', 'Experiment');
|
|
for i = 1:6
|
|
plot(f, abs(squeeze(freqresp(model.G(i, i), f, 'Hz'))), 'color', colors(2,:), ...
|
|
'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', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', colors(1,:));
|
|
end
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(squeeze(freqresp(model.G(i, i), f, 'Hz'))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_hac_plant_exp_simscape.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_hac_plant_exp_simscape
|
|
#+caption: Comparison of the open-loop plant measured experimentally and extracted from Simscape
|
|
#+RESULTS:
|
|
[[file:figs/comp_hac_plant_exp_simscape.png]]
|
|
|
|
|
|
** Damped Plant
|
|
#+begin_src matlab
|
|
%% Load identification data for the damped plant
|
|
data = load(sprintf('%s/dynamics/2023-02-01_16-04_identification_damped_iff_long.mat', data_path));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% IFF Plant (transfer function from u to taum)
|
|
G_damp = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
G_damp(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, 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', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_damp(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 1:6
|
|
plot(f, abs(G_damp(:,i, i)), 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\epsilon_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(f, abs(G_damp(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\epsilon_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_damp(:,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);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/G_damp_damped_exp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:G_damp_damped_exp_no_rotation
|
|
#+caption: Obtained transfer function from generated voltages to estimated strut motion
|
|
#+RESULTS:
|
|
[[file:figs/G_damp_damped_exp_no_rotation.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the undamped and damped plant with IFF
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(G_hac(:, 1, 1)), 'color', colors(1,:), ...
|
|
'DisplayName', 'Undamped');
|
|
for i = 2:6
|
|
plot(f, abs(G_hac(:,i, i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_damp(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(G_damp(:, 1, 1)), 'color', colors(2,:), ...
|
|
'DisplayName', 'Damped');
|
|
for i = 2:6
|
|
plot(f, abs(G_damp(:,i, i)), 'color', colors(2,:), ...
|
|
'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', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', colors(1,:));
|
|
end
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_damp(:,i, i)), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_damped_undamped_plant.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_damped_undamped_plant
|
|
#+caption: Comparison of the undamped and damped plant with IFF
|
|
#+RESULTS:
|
|
[[file:figs/comp_damped_undamped_plant.png]]
|
|
|
|
** HAC Controller
|
|
#+begin_src matlab
|
|
%% Lead to increase phase margin
|
|
a = 4; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
wc = 2*pi*15; % Frequency with the maximum phase lead [rad/s]
|
|
|
|
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
|
|
|
%% Low Pass filter to increase robustness
|
|
H_lpf = 1/(1 + s/2/pi/60);
|
|
|
|
%% Notch at the top-plate resonance
|
|
gm = 0.02;
|
|
xi = 0.3;
|
|
wn = 2*pi*665;
|
|
|
|
H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
|
|
|
%% Decentralized HAC
|
|
Khac_iff_struts = (8e3) * ... % Gain
|
|
H_notch * ... % Notch
|
|
H_lpf * ... % LPF
|
|
(2*pi*100/s) * ... % Integrator
|
|
eye(6); % 6x6 Diagonal
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Loop Gain
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, abs(G_damp(:,i, i).*squeeze(freqresp(Khac_iff_struts(i,i), f, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
% ylim([1e-8, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(f, 180/pi*angle(G_damp(:,i, i).*squeeze(freqresp(Khac_iff_struts(i,i), f, 'Hz'))));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/first_hac_K_exp_loop_gain.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:first_hac_K_exp_loop_gain
|
|
#+caption: Loop gain for the HAC
|
|
#+RESULTS:
|
|
[[file:figs/first_hac_K_exp_loop_gain.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Compute the Eigenvalues of the loop gain
|
|
Ldet = zeros(6, length(f));
|
|
|
|
Lmimo = pagemtimes(permute(G_damp, [2,3,1]),squeeze(freqresp(Khac_iff_struts, f, 'Hz')));
|
|
for i_f = 2:length(f)
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot of the eigenvalues of L in the complex plane
|
|
figure;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
|
|
end
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/first_hac_K_exp_root_locus.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:first_hac_K_exp_root_locus
|
|
#+caption: Obtained Root Locus
|
|
#+RESULTS:
|
|
[[file:figs/first_hac_K_exp_root_locus.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Export to Bliss format
|
|
K = - Khac_iff_struts(1,1);
|
|
K_order = order(K(1,1));
|
|
|
|
Kz = c2d(K(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
|
[num, den] = tfdata(Kz, 'v');
|
|
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
|
fileID = fopen('matlab/mat/K_hac_first_test.dat', 'w');
|
|
fprintf(fileID, formatSpec, [num; den]');
|
|
fclose(fileID);
|
|
#+end_src
|
|
|
|
** Compare dynamics seen by interferometers and by encoders
|
|
#+begin_src matlab :exports none
|
|
%% IFF Plant (transfer function from u to taum)
|
|
G_dl = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
eL = -[data.(sprintf("uL%i", i_strut)).dL1 ; data.(sprintf("uL%i", i_strut)).dL2 ; data.(sprintf("uL%i", i_strut)).dL3 ; data.(sprintf("uL%i", i_strut)).dL4 ; data.(sprintf("uL%i", i_strut)).dL5 ; data.(sprintf("uL%i", i_strut)).dL6]';
|
|
|
|
G_dl(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the identified dynamic by the internal metrology (encoders) and by the external metrology (interferometers)
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_hac(:,1, 1)), '-', ...
|
|
'DisplayName', 'External Metrology');
|
|
plot(f, abs(G_dl(:,1, 1)), '-', ...
|
|
'DisplayName', 'Internal Metrology');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f, abs(G_dl(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
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', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_hac(:,1, 1)), '-');
|
|
plot(f, 180/pi*angle(G_dl(:,1, 1)), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_dynamics_int_ext_metrology.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_dynamics_int_ext_metrology
|
|
#+caption: Comparison of the identified dynamic by the internal metrology (encoders) and by the external metrology (interferometers)
|
|
#+RESULTS:
|
|
[[file:figs/comp_dynamics_int_ext_metrology.png]]
|
|
|
|
|
|
** Compare dynamics obtained with different Rz estimations
|
|
#+begin_src matlab :exports none
|
|
%% Load identification data
|
|
data = load(sprintf('%s/dynamics/2023-02-01_15-18_identification_new_matrices_long_Va.mat', data_path));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% IFF Plant (transfer function from u to taum)
|
|
G_hac_Va = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
G_hac_Va(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the obtained plant using the Encoders or using the output Voltages to estimate Rz
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_hac(:,1, 1)), '-', ...
|
|
'DisplayName', 'Encoders');
|
|
plot(f, abs(G_hac_Va(:,1, 1)), '-', ...
|
|
'DisplayName', 'Voltage');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f, abs(G_hac_Va(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
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', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_hac(:,1, 1)), '-');
|
|
plot(f, 180/pi*angle(G_hac_Va(:,1, 1)), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_plant_encoders_Va.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_plant_encoders_Va
|
|
#+caption: Comparison of the obtained plant using the Encoders or using the output Voltages to estimate Rz
|
|
#+RESULTS:
|
|
[[file:figs/comp_plant_encoders_Va.png]]
|
|
|
|
|
|
* Closed-Loop Results
|
|
** 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 :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Frequency vector for further use
|
|
freqs = logspace(0,3,1000);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
data_path = '/home/thomas/mnt/data_mel/NASS';
|
|
#+end_src
|
|
|
|
** Open and Closed loop results
|
|
#+begin_src matlab
|
|
data_ol = load(sprintf('%s/spindle/2022-12-20_15-43_sec_test.mat', data_path));
|
|
data_cl = load(sprintf('%s/spindle/2023-02-01_16-57_closed_loop.mat', data_path));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the Open-Loop and Closed-Loop spindle errors
|
|
figure;
|
|
hold on;
|
|
plot(detrend(1e6*data_ol.Dx_int, 0), detrend(1e6*data_ol.Dy_int, 0), 'DisplayName', sprintf('OL $\\epsilon_d = %.1f$ [$\\mu$m RMS]', 1e6*rms(sqrt(detrend(data_ol.Dx_int, 0).^2 + detrend(data_ol.Dy_int, 0).^2))))
|
|
hold off;
|
|
xlabel('$x$ motion [$\mu$m]');
|
|
ylabel('$y$ motion [$\mu$m]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
axis square
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/spindle_errors_1rpm_ol.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
[[file:figs/spindle_errors_1rpm_ol.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the Open-Loop and Closed-Loop spindle errors
|
|
figure;
|
|
hold on;
|
|
plot(detrend(1e6*data_ol.Dx_int, 0), detrend(1e6*data_ol.Dy_int, 0), 'DisplayName', sprintf('OL $\\epsilon_d = %.1f$ [$\\mu$m RMS]', 1e6*rms(sqrt(detrend(data_ol.Dx_int, 0).^2 + detrend(data_ol.Dy_int, 0).^2))))
|
|
plot(1e6*data_cl.Dx_int, 1e6*data_cl.Dy_int, 'DisplayName', sprintf('CL $\\epsilon_d = %.1f$ [nm RMS]', 1e9*rms(sqrt(data_cl.Dx_int.^2 + data_cl.Dy_int.^2))))
|
|
hold off;
|
|
xlabel('$x$ motion [$\mu$m]');
|
|
ylabel('$y$ motion [$\mu$m]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
axis square
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/spindle_errors_1rpm_op_cl.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:spindle_errors_1rpm_op_cl
|
|
#+caption: Comparison of the Open-Loop and Closed-Loop spindle errors
|
|
#+RESULTS:
|
|
[[file:figs/spindle_errors_1rpm_op_cl.png]]
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the Open-Loop and Closed-Loop spindle errors - Rotation
|
|
figure;
|
|
hold on;
|
|
plot(detrend(1e6*data_ol.Rx_int, 0), detrend(1e6*data_ol.Ry_int, 0), 'DisplayName', 'Open-Loop')
|
|
plot(1e6*data_cl.Rx_int, 1e6*data_cl.Ry_int, 'DisplayName', 'Closed-Loop')
|
|
hold off;
|
|
xlabel('$R_x$ motion [$\mu$rad]');
|
|
ylabel('$R_y$ motion [$\mu$rad]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
axis square
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/spindle_errors_1rpm_op_cl_rot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:spindle_errors_1rpm_op_cl_rot
|
|
#+caption: Comparison of the Open-Loop and Closed-Loop spindle errors - Rotation
|
|
#+RESULTS:
|
|
[[file:figs/spindle_errors_1rpm_op_cl_rot.png]]
|
|
|
|
* Nano-Hexapod fixed on the Spindle :noexport:
|
|
<<sec:nano_hexapod_spindle>>
|
|
** Introduction :ignore:
|
|
|
|
The Spindle is now fixed on top of a Spindle: the RS250S from LAB Motion Systems ([[file:doc/RS250S.pdf][doc]]).
|
|
All electrical connections between the nano-hexapod and the control electronics are passing trough a Slip-Ring.
|
|
A picture of the nano-hexapod on top of the Spindle is shown in Figure ref:fig:hexapod_spindle_picture.
|
|
|
|
#+name: fig:hexapod_spindle_picture
|
|
#+caption: Nano-Hexapod fixed on top of the Spindle
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/hexapod_spindle_picture.jpg]]
|
|
|
|
** Change of dynamics when fixed on the Spindle
|
|
*** 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 :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
*** Measured Frequency Response Functions
|
|
The identification only performed without any payload.
|
|
|
|
The following data are loaded:
|
|
- =Va=: the excitation voltage (corresponding to $u_i$)
|
|
- =Vs=: the generated voltage by the 6 force sensors (corresponding to $\bm{\tau}_m$)
|
|
- =de=: the measured motion by the 6 encoders (corresponding to $d\bm{\mathcal{L}}_m$)
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_added_mass = {};
|
|
|
|
for i_strut = 1:6
|
|
meas_added_mass(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_0m.mat', i_strut), 't', 'Va', 'Vs', 'de')};
|
|
end
|
|
#+end_src
|
|
|
|
The window =win= and the frequency vector =f= are defined.
|
|
#+begin_src matlab
|
|
% Sampling Time [s]
|
|
Ts = (meas_added_mass{1}.t(end) - (meas_added_mass{1}.t(1)))/(length(meas_added_mass{1}.t)-1);
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1/Ts));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_added_mass{1}.Va, meas_added_mass{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
Finally the $6 \times 6$ transfer function matrices from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ and from $\bm{u}$ to $\bm{\tau}_m$ are identified:
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_dL = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
G_dL(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.de, win, [], [], 1/Ts);
|
|
end
|
|
|
|
%% IFF Plant (transfer function from u to taum)
|
|
G_tau = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
G_tau(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.Vs, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
The identified dynamics are then saved for further use.
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/data_frf/frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
save('data_frf/frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
frf_ol = load('frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL');
|
|
frf_vib_tab = load('frf_vib_table_m.mat', 'f', 'Ts', 'G_tau', 'G_dL');
|
|
#+end_src
|
|
|
|
*** Transfer function from Actuator to Encoder
|
|
The transfer functions from $u_i$ to $d\mathcal{L}_{m,i}$ are shown in Figure ref:fig:frf_GdL_spindle_0m.
|
|
|
|
#+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(frf_ol.f, abs(frf_ol.G_dL(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:,i, i)), ...
|
|
'DisplayName', sprintf('$d\\mathcal{L}_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(frf_ol.f, 180/pi*angle(frf_ol.G_dL(:,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);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_GdL_spindle_0m.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_GdL_spindle_0m
|
|
#+caption: Measured Frequency Response Functions from $u_i$ to $d\mathcal{L}_{m,i}$ when the nano-hexapod is fixed to the Spindle
|
|
#+RESULTS:
|
|
[[file:figs/frf_GdL_spindle_0m.png]]
|
|
|
|
The dynamics of the nano-hexapod when fixed on the Spindle is compared with the dynamics when the nano-hexapod is fixed on the "vibration table" in Figure ref:fig:frf_GdL_comp_spindle_vib_table_0m.
|
|
|
|
#+begin_question
|
|
Why are the frequency of the suspension modes are *decreased* when the nano-hexapod is fixed to the Spindle?
|
|
#+end_question
|
|
|
|
#+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(frf_ol.f, abs(frf_ol.G_dL(:,i,j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,i,j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:,1,1)), 'color', colors(1,:), ...
|
|
'DisplayName', 'Spindle');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,1,1)), 'color', colors(2,:), ...
|
|
'DisplayName', 'Vib. Table');
|
|
for i = 2:6
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:,i, i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,i, i)), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:,1,2)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', 'Spindle - Coupling');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,1,2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', 'Vib. Table - Coupling');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(frf_ol.f, 180/pi*angle(frf_ol.G_dL(:,i, i)));
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(frf_vib_tab.f, 180/pi*angle(frf_vib_tab.G_dL{1}(:,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);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_GdL_comp_spindle_vib_table_0m.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_GdL_comp_spindle_vib_table_0m
|
|
#+caption: Comparison of the dynamics from $u$ to $d\mathcal{L}$ when the nano-hexapod is fixed on top of the Spindle and when it is fixed on top of the "Vibration Table".
|
|
#+RESULTS:
|
|
[[file:figs/frf_GdL_comp_spindle_vib_table_0m.png]]
|
|
|
|
*** Transfer function from Actuator to Force Sensor
|
|
The transfer functions from $u_i$ to $\tau_m$ are shown in Figure ref:fig:frf_Gtau_spindle_0m.
|
|
|
|
#+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(frf_ol.f, abs(frf_ol.G_tau(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',i)
|
|
plot(frf_ol.f, abs(frf_ol.G_tau(:,i, i)), ...
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_tau(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\\tau_{m,i}/u_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-2, 1e2]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(frf_ol.f, 180/pi*angle(frf_ol.G_tau(:,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);
|
|
ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([20, 2e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_Gtau_spindle_0m.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_Gtau_spindle_0m
|
|
#+caption: Measured Frequency Response Functions from $u_i$ to $\tau_{m,i}$ when the nano-hexapod is fixed to the Spindle
|
|
#+RESULTS:
|
|
[[file:figs/frf_Gtau_spindle_0m.png]]
|
|
|
|
The dynamics of the nano-hexapod when fixed on the Spindle is compared with the dynamics when the nano-hexapod is fixed on the "vibration table" in Figure ref:fig:frf_Gtau_comp_spindle_vib_table_0m.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the 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(frf_ol.f, abs(frf_ol.G_tau(:,i,j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,i,j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_tau(:,1,1)), 'color', colors(1,:), ...
|
|
'DisplayName', 'Spindle');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,1,1)), 'color', colors(2,:), ...
|
|
'DisplayName', 'Vib. Table');
|
|
for i = 2:6
|
|
plot(frf_ol.f, abs(frf_ol.G_tau(:,i, i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,i, i)), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_tau(:,1,2)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', 'Spindle - Coupling');
|
|
plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,1,2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', 'Vib. Table - Coupling');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [-]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e2]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(frf_ol.f, 180/pi*angle(frf_ol.G_tau(:,i, i)));
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(frf_vib_tab.f, 180/pi*angle(frf_vib_tab.G_tau{1}(:,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);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_Gtau_comp_spindle_vib_table_0m.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_Gtau_comp_spindle_vib_table_0m
|
|
#+caption: Comparison of the dynamics from $u$ to $d\mathcal{L}$ when the nano-hexapod is fixed on top of the Spindle and when it is fixed on top of the "Vibration Table".
|
|
#+RESULTS:
|
|
[[file:figs/frf_Gtau_comp_spindle_vib_table_0m.png]]
|
|
|
|
*** Conclusion
|
|
#+begin_important
|
|
The dynamics of the nano-hexapod does not change a lot when it is fixed to the Spindle.
|
|
The "suspension" modes are just increased a little bit due to the added stiffness of the spindle as compared to the vibration table.
|
|
#+end_important
|
|
|
|
** Dynamics of the Damped plant
|
|
*** Introduction :ignore:
|
|
As the dynamics is not much changed when the nano-hexapod is fixed on top of the Spindle, the same IFF controller is used to damp the plant.
|
|
|
|
*** 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 :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
*** Measured Frequency Response Functions
|
|
The identification is performed without added mass, and with one, two and three layers of added cylinders.
|
|
#+begin_src matlab
|
|
i_masses = 0:3;
|
|
#+end_src
|
|
|
|
The following data are loaded:
|
|
- =Va=: the excitation voltage (corresponding to $u_i$)
|
|
- =Vs=: the generated voltage by the 6 force sensors (corresponding to $\bm{\tau}_m$)
|
|
- =de=: the measured motion by the 6 encoders (corresponding to $d\bm{\mathcal{L}}_m$)
|
|
#+begin_src matlab
|
|
%% Load Identification Data
|
|
meas_added_mass = {};
|
|
|
|
for i_mass = i_masses
|
|
for i_strut = 1:6
|
|
meas_added_mass(i_strut, i_mass+1) = {load(sprintf('frf_data_exc_strut_%i_spindle_%im_iff.mat', i_strut, i_mass), 't', 'Va', 'Vs', 'de')};
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
The window =win= and the frequency vector =f= are defined.
|
|
#+begin_src matlab
|
|
% Sampling Time [s]
|
|
Ts = (meas_added_mass{1,1}.t(end) - (meas_added_mass{1,1}.t(1)))/(length(meas_added_mass{1,1}.t)-1);
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1/Ts));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_added_mass{1,1}.Va, meas_added_mass{1,1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
Finally the $6 \times 6$ transfer function matrices from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ and from $\bm{u}$ to $\bm{\tau}_m$ are identified:
|
|
#+begin_src matlab
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_dL = {};
|
|
|
|
for i_mass = i_masses
|
|
G_dL(i_mass+1) = {zeros(length(f), 6, 6)};
|
|
for i_strut = 1:6
|
|
G_dL{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_strut, i_mass+1}.Va, meas_added_mass{i_strut, i_mass+1}.de, win, [], [], 1/Ts);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
The identified dynamics are then saved for further use.
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/data_frf/frf_spindle_iff_m.mat', 'f', 'Ts', 'G_dL')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
save('data_frf/frf_spindle_iff_m.mat', 'f', 'Ts', 'G_dL')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
frf_ol = load('frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL');
|
|
frf_iff = load('frf_spindle_iff_m.mat', 'f', 'Ts', 'G_dL');
|
|
#+end_src
|
|
|
|
*** Effect of Integral Force Feedback
|
|
#+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(frf_ol.f, abs(frf_ol.G_dL(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(frf_ol.f, abs(frf_iff.G_dL{1}(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:,1,1)), 'color', colors(1,:), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
|
|
plot(frf_iff.f, abs(frf_iff.G_dL{1}(:,1,1)), 'color', colors(2,:), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$');
|
|
for i = 2:6
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:,i, i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(frf_iff.f, abs(frf_iff.G_dL{1}(:,i, i)), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(frf_ol.f, abs(frf_ol.G_dL(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
|
|
plot(frf_iff.f, abs(frf_iff.G_dL{1}(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-3]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(frf_ol.f, 180/pi*angle(frf_ol.G_dL(:,i, i)));
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(frf_iff.f, 180/pi*angle(frf_iff.G_dL{1}(:,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);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_spindle_comp_ol_iff.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_spindle_comp_ol_iff
|
|
#+caption: Effect of Integral Force Feedback on the transfer function from $u_i$ to $d\mathcal{L}_i$
|
|
#+RESULTS:
|
|
[[file:figs/frf_spindle_comp_ol_iff.png]]
|
|
|
|
*** Effect of the payload
|
|
#+begin_important
|
|
From Figure ref:fig:frf_spindle_iff_effect_payload we can see that the coupling is quite large when payloads are added to the nano-hexapod.
|
|
This was not the case when the nano-hexapod was fixed to the vibration table.
|
|
#+end_important
|
|
|
|
#+begin_question
|
|
What is causing the resonances at 20Hz, 25Hz and 30Hz when there is some added payload?
|
|
Why the coupling is much larger than when the nano-hexapod was on top of the isolation table?
|
|
#+end_question
|
|
|
|
#+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_mass = i_masses
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(frf_iff.f, abs(frf_iff.G_dL{i_mass+1}(:, i, j)), 'color', [colors(i_mass+1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(frf_iff.f, abs(frf_iff.G_dL{i_mass+1}(:,1, 1)), 'color', colors(i_mass+1,:), ...
|
|
'DisplayName', sprintf('$d\\mathcal{L}_{m,i}/u_i$ - %i', i_mass));
|
|
for i = 2:6
|
|
plot(frf_iff.f, abs(frf_iff.G_dL{i_mass+1}(:,i, i)), 'color', colors(i_mass+1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
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', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i_mass = i_masses
|
|
for i =1:6
|
|
plot(frf_iff.f, 180/pi*angle(frf_iff.G_dL{i_mass+1}(:,i, i)), 'color', colors(i_mass+1,:));
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_spindle_iff_effect_payload.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_spindle_iff_effect_payload
|
|
#+caption: Effect of the payload on the transfer functions from $u^\prime_i$ to $d\mathcal{L}_i$
|
|
#+RESULTS:
|
|
[[file:figs/frf_spindle_iff_effect_payload.png]]
|
|
|
|
*** Effect of rotation
|
|
#+begin_src matlab :exports none
|
|
%% Load Identification Data
|
|
meas_0rpm = {};
|
|
meas_60rpm = {};
|
|
|
|
for i_strut = 1:6
|
|
meas_0rpm(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_3m_iff.mat', i_strut), 't', 'Va', 'Vs', 'de')};
|
|
meas_60rpm(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_3m_iff_60rpm.mat', i_strut), 't', 'Va', 'Vs', 'de')};
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
% Sampling Time [s]
|
|
Ts = (meas_0rpm{1}.t(end) - (meas_0rpm{1}.t(1)))/(length(meas_0rpm{1}.t)-1);
|
|
|
|
% Hannning Windows
|
|
win = hanning(ceil(1/Ts));
|
|
|
|
% And we get the frequency vector
|
|
[~, f] = tfestimate(meas_0rpm{1}.Va, meas_0rpm{1}.de, win, [], [], 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% DVF Plant (transfer function from u to dLm)
|
|
G_dL_0rpm = zeros(length(f), 6, 6);
|
|
G_dL_60rpm = zeros(length(f), 6, 6);
|
|
|
|
for i_strut = 1:6
|
|
G_dL_0rpm(:,:,i_strut) = tfestimate(meas_0rpm{i_strut}.Va, meas_0rpm{i_strut}.de, win, [], [], 1/Ts);
|
|
G_dL_60rpm(:,:,i_strut) = tfestimate(meas_60rpm{i_strut}.Va, meas_60rpm{i_strut}.de, win, [], [], 1/Ts);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_important
|
|
The identified plants with and without spindle's rotation are compared in Figure ref:fig:frf_comp_spindle_0rpm_60rpm_3m.
|
|
It is shown that the rotational speed as little effect on the plant dynamics.
|
|
#+end_important
|
|
|
|
#+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, abs(G_dL_0rpm(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f, abs(G_dL_60rpm(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(G_dL_0rpm(:,1,1)), 'color', colors(1,:), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - 0rpm');
|
|
plot(f, abs(G_dL_60rpm(:,1,1)), 'color', colors(2,:), ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$ - 60rpm');
|
|
for i = 2:6
|
|
plot(f, abs(G_dL_0rpm(:,i, i)), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(f, abs(G_dL_60rpm(:,i, i)), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(G_dL_0rpm(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - 0rpm');
|
|
plot(f, abs(G_dL_60rpm(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$ - 60rpm');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-8, 1e-4]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f, 180/pi*angle(G_dL_0rpm(:,i, i)));
|
|
set(gca,'ColorOrderIndex',2)
|
|
plot(f, 180/pi*angle(G_dL_60rpm(:,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);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/frf_comp_spindle_0rpm_60rpm_3m.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:frf_comp_spindle_0rpm_60rpm_3m
|
|
#+caption: Comparison of the damped plant when the spindle is not rotating and when it is rotating at 60RPM
|
|
#+RESULTS:
|
|
[[file:figs/frf_comp_spindle_0rpm_60rpm_3m.png]]
|
|
|
|
* Helping Functions :noexport:
|
|
** Initialize Path
|
|
#+NAME: m-init-path
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./matlab/STEPS/'); % Path for STEPS files
|
|
addpath('./matlab/subsystems/'); % Path for Simulink subsystems
|
|
addpath('./matlab/mat/'); % Path for data
|
|
addpath('./matlab/src/'); % Path for functions
|
|
addpath('./matlab/'); % Path for scripts
|
|
#+END_SRC
|
|
|
|
#+NAME: m-init-path-tangle
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./STEPS/'); % Path for STEPS files
|
|
addpath('./subsystems/'); % Path for Simulink subsystems
|
|
addpath('./mat/'); % Path for data
|
|
addpath('./src/'); % Path for functions
|
|
#+END_SRC
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
Ts = 1e-4;
|
|
data_dir = "/home/thomas/mnt/data_easy/id00/inhouse/MEL/NASS"
|
|
#+END_SRC
|
|
|
|
** =circlefit=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/circlefit.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:circlefit>>
|
|
|
|
#+begin_src matlab
|
|
function [xc,yc,R,a] = circfit(x,y)
|
|
%
|
|
% [xc yx R] = circfit(x,y)
|
|
%
|
|
% fits a circle in x,y plane in a more accurate
|
|
% (less prone to ill condition )
|
|
% procedure than circfit2 but using more memory
|
|
% x,y are column vector where (x(i),y(i)) is a measured point
|
|
%
|
|
% result is center point (yc,xc) and radius R
|
|
% an optional output is the vector of coeficient a
|
|
% describing the circle's equation
|
|
%
|
|
% x^2+y^2+a(1)*x+a(2)*y+a(3)=0
|
|
%
|
|
% By: Izhak bucher 25/oct /1991,
|
|
x=x(:); y=y(:);
|
|
a=[x y ones(size(x))]\[-(x.^2+y.^2)];
|
|
xc = -.5*a(1);
|
|
yc = -.5*a(2);
|
|
R = sqrt((a(1)^2+a(2)^2)/4-a(3));
|
|
#+end_src
|