2874 lines
100 KiB
Org Mode
2874 lines
100 KiB
Org Mode
#+TITLE: ESRF Double Crystal Monochromator - Dynamical Multi-Body Model
|
|
:DRAWER:
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ../index.html
|
|
#+HTML_LINK_UP: ../index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
|
|
#+BIND: org-latex-image-default-option "scale=1"
|
|
#+BIND: org-latex-image-default-width ""
|
|
|
|
#+LaTeX_CLASS: scrreprt
|
|
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full]
|
|
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
|
|
#+LATEX_HEADER_EXTRA: \bibliography{ref}
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :noweb yes
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results file raw replace
|
|
#+PROPERTY: header-args:latex+ :buffer no
|
|
#+PROPERTY: header-args:latex+ :tangle no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports results
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
|
:END:
|
|
|
|
#+begin_export html
|
|
<hr>
|
|
<p>This report is also available as a <a href="./dcm-simscape-model.pdf">pdf</a>.</p>
|
|
<hr>
|
|
#+end_export
|
|
|
|
#+latex: \clearpage
|
|
|
|
* Introduction :ignore:
|
|
In this document, a Simscape (.e.g. multi-body) model of the ESRF Double Crystal Monochromator (DCM) is presented and used to develop and optimize the control strategy.
|
|
|
|
It is structured as follow:
|
|
- Section [[sec:dcm_kinematics]]: the kinematics of the DCM is presented, and Jacobian matrices which are used to solve the inverse and forward kinematics are computed.
|
|
- Section [[sec:open_loop_identification]]: the system dynamics is identified in the absence of control.
|
|
- Section [[sec:dcm_noise_budget]]: an open-loop noise budget is performed.
|
|
- Section [[sec:active_damping_strain_gauges]]: it is studied whether if the strain gauges fixed to the piezoelectric actuators can be used to actively damp the plant.
|
|
- Section [[sec:active_damping_iff]]: piezoelectric force sensors are added in series with the piezoelectric actuators and are used to actively damp the plant using the Integral Force Feedback (IFF) control strategy.
|
|
- Section [[sec:hac_iff]]: the High Authority Control - Low Authority Control (HAC-LAC) strategy is tested on the Simscape model.
|
|
|
|
* System Kinematics
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_kinematics.m
|
|
:END:
|
|
<<sec:dcm_kinematics>>
|
|
|
|
** Introduction :ignore:
|
|
In this section, the kinematics of the DCM is described.
|
|
The elements (crystals, fast jacks, metrology frame, ...) are all defined.
|
|
The associated frames in which the motion of those elements are expressed will also be defined.
|
|
Finally, the transformation necessary to compute the relative pose of the crystals from the metrology is computed.
|
|
|
|
** General description of the system architecture - Terminology
|
|
In order to describe the motion of the different elements, reference frames are used.
|
|
|
|
One global frame is first defined with:
|
|
- the $x$ axis is aligned with the x-ray
|
|
- the $y$ axis aligned with the Bragg axis
|
|
- the $z$ axis is pointing up aligned with the gravity
|
|
The axis are shown in Figure [[fig:dcm_schematic_overview]].
|
|
The origin of this frame is usually taken at the intersection between the x-ray and the bragg axis.
|
|
|
|
The Bragg axis is therefore rotating around the $y$ axis.
|
|
Two sets of two crystals are also rotating with the bragg axis.
|
|
One set is positioned on the "hall" side (i.e toward $+y$) while the other set is positioned on the "ring" side (i.e. toward $-y$).
|
|
|
|
For each of these two sets, there is a "primary" crystal directly fixed to the bragg axis, and a "secondary" crystal fixed on top of three fastjacks.
|
|
There are therefore 4 crystals in total for each Double Crystal Monochromator.
|
|
|
|
There is then a metrology frame that hold all the interferometers.
|
|
|
|
Several reference frame are used (displayed in Figure [[fig:dcm_schematic_overview]] for the "ring" crystals):
|
|
- $\{\mathcal{F}_{1r}\}$ and $\{\mathcal{F}_{1h}\}$: frames fixed to the bottom surface of first crystals with their origin at the intersection between the X-ray and the Bragg axis
|
|
- $\{\mathcal{F}_{2r}\}$ and $\{\mathcal{F}_{2h)}\}$: frames fixed to the top surface of second crystals. Their origin is at the crystal's center
|
|
- $\{\mathcal{F}_{mf}\}$: frame fixed to the center of the top part of the metrology frame
|
|
|
|
#+name: fig:dcm_schematic_overview
|
|
#+caption: Schematic of the DCM with one set of first and second crystals with the associated frames
|
|
[[file:figs/dcm_schematic_overview.png]]
|
|
|
|
#+begin_note
|
|
Here are the different letters used in the notations and there meaning:
|
|
- =r=: "ring" (i.e. toward $-y$)
|
|
- =h=: "hall" (i.e. toward $+y$)
|
|
- =u=: "upstream" (i.e. towards $-x$)
|
|
- =d=: "downstream" (i.e. towards $+x$)
|
|
- =c=: "center" (i.e. centered at $x=0$)
|
|
- =1=: primary crystal
|
|
- =2=: secondary crystal
|
|
- =mf=: metrology frame
|
|
#+end_note
|
|
|
|
** Forward and Inverse Kinematics problems
|
|
|
|
Let's consider one plane initially orthogonal to the $z$ direction (Figure [[fig:schematic_measurement_plane]]).
|
|
The pose of the plane, expressed in a frame $\{\mathcal{F}\}$ with origin $\mathcal{O}$ is defined by:
|
|
- its vertical motion: $d_z$
|
|
- its rotation around the $y$ axis: $r_y$
|
|
- its rotation around the $x$ axis: $r_x$
|
|
|
|
#+name: fig:schematic_measurement_plane
|
|
#+caption: SChematic of a plane with three vertical interferometers
|
|
[[file:figs/schematic_measurement_plane.png]]
|
|
|
|
In order to measure the pose of the crystal, three interferometers are measuring the motion of the plane at three distinct positions $\mathcal{O}_1 = [x_1, y_1, 0]$, $\mathcal{O}_2 = [x_2, y_2, 0]$ and $\mathcal{O}_3 = [x_3, y_3, 0]$ (See Figure [[fig:schematic_measurement_plane]]).
|
|
The interferometers can be pointed in the $z$ direction or in the opposite $z$ direction.
|
|
The measured motion are noted $z_1$, $z_2$ and $z_3$.
|
|
|
|
The *inverse kinematic* problem consists of deriving the measured $[z_1, z_2, z_3]$ motions from the pose $[d_z, r_y, r_x]$ of the crystal.
|
|
For small motion, this problem can be easily solved as follows:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
z_{1} \\ z_{2} \\ z_{3}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}
|
|
\begin{bmatrix}
|
|
d_{z} \\ r_{y} \\ r_{x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
with $\bm{J}$ called the *Jacobian matrix*.
|
|
|
|
The Jacobian matrix can be computed as follows in the case where the interferometers are pointing toward positive $z$:
|
|
\begin{equation} \label{eq:jacobian_3dof_positive_z}
|
|
\boxed{
|
|
\bm{J} = \begin{bmatrix}
|
|
1 & -x_1 & y_1 \\
|
|
1 & -x_2 & y_2 \\
|
|
1 & -x_3 & y_3
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
and computed as follows if there are pointing torward negative $z$:
|
|
\begin{equation} \label{eq:jacobian_3dof_negative_z}
|
|
\boxed{
|
|
\bm{J} = \begin{bmatrix}
|
|
-1 & x_1 & -y_1 \\
|
|
-1 & x_2 & -y_2 \\
|
|
-1 & x_3 & -y_3
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
The *forward kinematic* problem is then solved by inverting the Jacobian matrix:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{z} \\ r_{y} \\ r_{x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}^{-1}
|
|
\begin{bmatrix}
|
|
z_{1} \\ z_{2} \\ z_{3}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
The same reasoning can be performed to convert the wanted motion of one plane to the motion of three vertical actuators fixed to the plane (tripod architecture).
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_kinematics.m
|
|
% Computation of the DCM kinematics
|
|
#+end_src
|
|
|
|
#+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
|
|
|
|
** Bragg Angle
|
|
<<sec:kinematics_bragg>>
|
|
In order to have a *fix exit*, the following relation must be verified:
|
|
\begin{equation} \label{eq:bragg_angle_formula}
|
|
\boxed{d_z = \frac{d_{\text{off}}}{2 \cos \theta_b}}
|
|
\end{equation}
|
|
with:
|
|
- $d_{\text{off}}$ the wanted offset between the incident x-ray and the output x-ray
|
|
- $\theta_b$ the bragg angle
|
|
- $d_z$ the corresponding distance between the first and second crystal
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Tested bragg angles
|
|
bragg = linspace(0, 80, 5000); % Bragg angle [deg]
|
|
d_off = 10e-3; % Wanted offset between x-rays [m]
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Vertical Jack motion as a function of Bragg angle
|
|
dz = d_off./(2*cos(bragg*pi/180));
|
|
#+end_src
|
|
|
|
This relation is shown in Figure [[fig:jack_motion_bragg_angle]] for a wanted fix exit offset of 10mm.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Jack motion as a function of Bragg angle
|
|
figure;
|
|
plot(bragg, 1e3*dz)
|
|
xlabel('Bragg angle [deg]'); ylabel('Crystal Distance [mm]');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/jack_motion_bragg_angle.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:jack_motion_bragg_angle
|
|
#+caption: Jack motion as a function of Bragg angle ($d_z = 10\,mm$)
|
|
#+RESULTS:
|
|
[[file:figs/jack_motion_bragg_angle.png]]
|
|
|
|
** Kinematics of "hall" Crystals
|
|
<<sec:kinematics_hall_crystal>>
|
|
*** Introduction :ignore:
|
|
There are two "hall" crystals, the primary one and the secondary one.
|
|
|
|
*** Interferometers - Primary "Hall" Crystal
|
|
|
|
Three interferometers fixed to the top metrology frame are pointed to the top surface (i.e. toward negative $z$) of the primary "hall" crystal.
|
|
The measurements are performed along the $z$ direction.
|
|
|
|
The notations are:
|
|
- $\{\mathcal{F}_{1h}\}$ is the frame in which motion of the crystal are expressed
|
|
- $\mathcal{O}_{1h,ur}$ measurement point for the "upstream-ring" interferometer
|
|
- $\mathcal{O}_{1h,ch}$ measurement point for the "center-hall" interferometer
|
|
- $\mathcal{O}_{1h,dr}$ measurement point for the "downstream-ring" interferometer
|
|
|
|
The measured displacements by the interferometers are noted $[z_{1h,ur},\ z_{1h,ch},\ z_{1h,dr}]$.
|
|
|
|
The corresponding motion of the crystal expressed in the frame as shown in Figure [[fig:sensor_hall_crystal_points_primary]] are:
|
|
- $d_{1h,z}$ motion in the $z$ direction
|
|
- $r_{1h,y}$ rotation around the $y$ axis
|
|
- $r_{1h,x}$ rotation around the $x$ axis
|
|
|
|
#+begin_src latex :file sensor_hall_crystal_points_primary.pdf
|
|
\begin{tikzpicture}
|
|
% Crystal
|
|
\draw (-7.5/2, -3.5/2) rectangle (7.5/2, 3.5/2);
|
|
|
|
% Measurement Points
|
|
\node[branch] (a1) at (-3.6, -1.5){};
|
|
\node[branch] (a2) at ( 0, 1.5){};
|
|
\node[branch] (a3) at ( 3.6, -1.5){};
|
|
|
|
% Labels
|
|
\node[right] at (a1) {$\mathcal{O}_{1h,ur} = (-36, -15)$};
|
|
\node[right] at (a2) {$\mathcal{O}_{1h,ch} = ( 0, 15)$};
|
|
\node[left] at (a3) {$\mathcal{O}_{1h,dr} = ( 36, -15)$};
|
|
|
|
% Origin
|
|
\draw[->] (0, 0) node[] -- ++(1, 0) node[right]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, 1) node[right]{$y$};
|
|
\draw[fill, color=black] (0, 0) circle (0.05);
|
|
\draw[color=black] (0, 0) circle (0.15);
|
|
\node[above right] at (0, 0){$z$};
|
|
|
|
\node[left] at (0,0) {$\{\mathcal{F}_{1h}\}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:sensor_hall_crystal_points_primary
|
|
#+caption: Top view of the primary crystal hall. Position of the measurement points. Units in [mm]
|
|
#+RESULTS:
|
|
[[file:figs/sensor_hall_crystal_points_primary.png]]
|
|
|
|
|
|
The inverse kinematics problem consists of deriving the measured displacement by the interferometer from the motion of the crystal (see Figure [[fig:schematic_sensor_jacobian_inverse_kinematics_hall_primary]]):
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
z_{1h,ur} \\ z_{1h,ch} \\ z_{1h,dr}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{1h,s}
|
|
\begin{bmatrix}
|
|
d_{1h,z} \\ r_{1h,y} \\ r_{1h,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_inverse_kinematics_hall_primary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js) {$\bm{J}_{1h,s}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{1h,z} \\ r_{1h,y} \\ r_{1h,x} \end{bmatrix}$} -- (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} z_{1h,ur} \\ z_{1h,ch} \\ z_{1h,dr} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics_hall_primary
|
|
#+caption: Inverse Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_inverse_kinematics_hall_primary.png]]
|
|
|
|
From the measurement coordinates in Figure [[fig:sensor_hall_crystal_points_primary]] and equation eqref:eq:jacobian_3dof_negative_z, the inverse kinematics the folowing Jacobian matrix is obtained:
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{1h,s}
|
|
=
|
|
\begin{bmatrix}
|
|
-1 & -0.036 & 0.015 \\
|
|
-1 & 0 & -0.015 \\
|
|
-1 & 0.036 & 0.015
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Sensor Jacobian matrix for hall crystal
|
|
J_1h_s = [-1, -0.036, 0.015
|
|
-1, 0, -0.015
|
|
-1, 0.036, 0.015];
|
|
#+end_src
|
|
|
|
The forward kinematics problem (computing the crystal motion from the 3 interferometers) is solved by inverting the Jacobian matrix (see Figure [[fig:schematic_sensor_jacobian_forward_kinematics_hall_primary]]).
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{1h,z} \\ r_{1h,y} \\ r_{1h,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{1h,s}^{-1}
|
|
\begin{bmatrix}
|
|
z_{1h,ur} \\ z_{1h,ch} \\ z_{1h,dr}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_forward_kinematics_hall_primary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js_inv) {$\bm{J}_{1h,s}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} z_{1h,ur} \\ z_{1h,ch} \\ z_{1h,dr} \end{bmatrix}$} -- (Js_inv.west);
|
|
\draw[->] (Js_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{1h,z} \\ r_{1h,y} \\ r_{1h,x} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_forward_kinematics_hall_primary
|
|
#+caption: Forward Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_forward_kinematics_hall_primary.png]]
|
|
|
|
The obtained matrix is displayed in Table [[tab:inverse_jacobian_sensor_hall_primary]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_1h_s), {}, {}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_sensor_hall_primary
|
|
#+caption: Inverse of the sensor Jacobian $\bm{J}_{1h,s}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| -0.25 | -0.5 | -0.25 |
|
|
| -13.89 | 0.0 | 13.89 |
|
|
| 16.67 | -33.33 | 16.67 |
|
|
|
|
*** Interferometers - Secondary "Hall" Crystal
|
|
|
|
Three interferometers are pointed to the bottom surface (i.e. toward positive $z$) of the secondary "hall" crystal.
|
|
|
|
The position of the measurement points are shown in Figure [[fig:sensor_hall_secondary_crystal_points]] as well as the origin where the motion of the crystal is computed.
|
|
|
|
The notations are:
|
|
- $\{\mathcal{F}_{2h}\}$ is the frame in which motion of the crystal are expressed
|
|
- $\mathcal{O}_{2h,ur}$ measurement point for the "upstream-ring" interferometer
|
|
- $\mathcal{O}_{2h,ch}$ measurement point for the "center-hall" interferometer
|
|
- $\mathcal{O}_{2h,dr}$ measurement point for the "downstream-ring" interferometer
|
|
|
|
The measured displacements by the interferometers are noted $[z_{2h,ur},\ z_{2h,ch},\ z_{2h,dr}]$.
|
|
|
|
The corresponding motion of the crystal expressed in the frame as shown in Figure [[fig:sensor_hall_secondary_crystal_points]] are:
|
|
- $d_{2h,z}$ motion in the $z$ direction
|
|
- $r_{2h,y}$ rotation around the $y$ axis
|
|
- $r_{2h,x}$ rotation around the $x$ axis
|
|
|
|
#+begin_src latex :file sensor_hall_secondary_crystal_points.pdf
|
|
\begin{tikzpicture}
|
|
% Crystal
|
|
\draw (-15/2, -3.5/2) rectangle (15/2, 3.5/2);
|
|
|
|
% Measurement Points
|
|
\node[branch] (a1) at (-7, 1.5){};
|
|
\node[branch] (a2) at ( 0, -1.5){};
|
|
\node[branch] (a3) at ( 7, 1.5){};
|
|
|
|
% Labels
|
|
\node[right] at (a1) {$\mathcal{O}_{2h,ur} = (-70, -15)$};
|
|
\node[right] at (a2) {$\mathcal{O}_{2h,ch} = ( 0, 15)$};
|
|
\node[left] at (a3) {$\mathcal{O}_{2h,dr} = ( 70, -15)$};
|
|
|
|
% Origin
|
|
\draw[->] (0, 0) node[] -- ++(1, 0) node[below]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, -1) node[right]{$y$};
|
|
\draw[fill, color=black] (0, 0) circle (0.05);
|
|
\draw[color=black] (0, 0) circle (0.15);
|
|
\draw[] (-0.1, 0.1) -- ( 0.1, -0.1);
|
|
\draw[] (-0.1,-0.1) -- ( 0.1, 0.1);
|
|
\node[below right] at (0,0) {$z$};
|
|
|
|
\node[left] at (0,0) {$\{\mathcal{F}_{2h}\}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:sensor_hall_secondary_crystal_points
|
|
#+caption: Bottom view of the secondary "hall" crystal. Position of the measurement points. Coordinate values are in [mm]
|
|
#+RESULTS:
|
|
[[file:figs/sensor_hall_secondary_crystal_points.png]]
|
|
|
|
The inverse kinematics consisting of deriving the interferometer measurements from the motion of the crystal (see Figure [[fig:schematic_sensor_jacobian_inverse_kinematics_hall_secondary]]):
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
z_{2h,ur} \\ z_{2h,ch} \\ z_{2h,dr}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{2h,s}
|
|
\begin{bmatrix}
|
|
d_{2h,z} \\ r_{2h,y} \\ r_{2h,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_inverse_kinematics_hall_secondary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js) {$\bm{J}_{2h,s}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{2h,z} \\ r_{2h,y} \\ r_{2h,x} \end{bmatrix}$} -- (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} z_{2h,ur} \\ z_{2h,ch} \\ z_{2h,dr} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics_hall_secondary
|
|
#+caption: Inverse Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_inverse_kinematics_hall_secondary.png]]
|
|
|
|
From the measurement coordinates in Figure [[fig:sensor_hall_secondary_crystal_points]] and equation eqref:eq:jacobian_3dof_positive_z, the inverse kinematics the folowing Jacobian matrix is obtained:
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{2h,s}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.07 & -0.015 \\
|
|
1 & 0 & 0.015 \\
|
|
1 & -0.07 & -0.015
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Sensor Jacobian matrix for hall crystal
|
|
J_2h_s = [1, 0.07, -0.015
|
|
1, 0, 0.015
|
|
1, -0.07, -0.015];
|
|
#+end_src
|
|
|
|
The forward kinematics is solved by inverting the Jacobian matrix (see Figure [[fig:schematic_sensor_jacobian_forward_kinematics_hall_secondary]]).
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{2h,z} \\ r_{2h,y} \\ r_{2h,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{2h,s}^{-1}
|
|
\begin{bmatrix}
|
|
z_{2h,ur} \\ z_{2h,ch} \\ z_{2h,dr}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_forward_kinematics_hall_secondary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js_inv) {$\bm{J}_{2h,s}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} z_{2h,ur} \\ z_{2h,ch} \\ z_{2h,dr} \end{bmatrix}$} -- (Js_inv.west);
|
|
\draw[->] (Js_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{2h,z} \\ r_{2h,y} \\ r_{2h,x} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_forward_kinematics_hall_secondary
|
|
#+caption: Forward Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_forward_kinematics_hall_secondary.png]]
|
|
|
|
The results is shown in Table [[tab:inverse_jacobian_sensor_hall_secondary]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_2h_s), {}, {}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_sensor_hall_secondary
|
|
#+caption: Inverse of the sensor Jacobian $\bm{J}_{s,h}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.25 | 0.5 | 0.25 |
|
|
| 7.14 | 0.0 | -7.14 |
|
|
| -16.67 | 33.33 | -16.67 |
|
|
|
|
*** Piezo - hall Crystal
|
|
|
|
The locations of the actuators with respect with the center of the secondary "hall" crystal are shown in Figure [[fig:actuator_jacobian_hall_points]].
|
|
|
|
#+name: fig:actuator_jacobian_hall_points
|
|
#+caption: Location of actuators with respect to the center of the hall second crystal (bottom view). Units are in [mm]
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/actuator_jacobian_hall_points.png]]
|
|
|
|
Inverse Kinematics consist of deriving the corresponding z motion of the 3 actuators from the motion of the crystal expressed in $\{\mathcal{F}_{2h}\}$.
|
|
As in the previous section, the motion of the secondary "hall" crystal is expressed by $[d_{2h,z},\ r_{2h,y},\ r_{2h,x}]$.
|
|
The motion of the three fast jacks are expressed by $[d_{u_r},\ d_{u_h},\ d_{d}]$ are are corresponding to a motion along the $z$ axis toward positive values.
|
|
|
|
The inverse kinematics can therefore be solved as follows:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{u_r} \\ d_{u_h} \\ d_{d}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{h,a}
|
|
\begin{bmatrix}
|
|
d_{2h,z} \\ r_{2h,y} \\ r_{2h,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_actuator_jacobian_inverse_kinematics_hall.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Ja) {$\bm{J}_{h,a}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{2h,z} \\ r_{2h,y} \\ r_{2h,x} \end{bmatrix}$} -- (Ja.west);
|
|
\draw[->] (Ja.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_d \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics_hall
|
|
#+caption: Inverse Kinematics - Actuators
|
|
#+RESULTS:
|
|
[[file:figs/schematic_actuator_jacobian_inverse_kinematics_hall.png]]
|
|
|
|
Based on the geometry in Figure [[fig:actuator_jacobian_hall_points]], the following Jacobian matrix is obtained (see Eq. eqref:eq:jacobian_3dof_positive_z):
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{h,a}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.14 & -0.1525 \\
|
|
1 & 0.14 & 0.0675 \\
|
|
1 & -0.14 & -0.0425
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Actuator Jacobian - hall crystal
|
|
J_h_a = [1, 0.14, -0.1525
|
|
1, 0.14, 0.0675
|
|
1, -0.14, -0.0425];
|
|
#+end_src
|
|
|
|
The forward Kinematics is solved by inverting the Jacobian matrix:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{2h,z} \\ r_{2h,y} \\ r_{2h,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{h,a}^{-1}
|
|
\begin{bmatrix}
|
|
d_{u_r} \\ d_{u_h} \\ d_{d}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_actuator_jacobian_forward_kinematics_hall.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Ja_inv) {$\bm{J}_{h,a}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_d \end{bmatrix}$} -- (Ja_inv.west);
|
|
\draw[->] (Ja_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{2h,z} \\ r_{2h,y} \\ r_{2h,x} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_actuator_jacobian_forward_kinematics_hall
|
|
#+caption: Forward Kinematics - Actuators for hall crystal
|
|
#+RESULTS:
|
|
[[file:figs/schematic_actuator_jacobian_forward_kinematics_hall.png]]
|
|
The obtained inverse Jacobian matrix is shown in Table [[tab:inverse_jacobian_actuator_hall]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_h_a), {}, {}, ' %.4f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_actuator_hall
|
|
#+caption: Inverse of the actuator Jacobian $\bm{J}_{a,h}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.0568 | 0.4432 | 0.5 |
|
|
| 1.7857 | 1.7857 | -3.5714 |
|
|
| -4.5455 | 4.5455 | 0.0 |
|
|
|
|
** Kinematics of "ring" Crystals
|
|
<<sec:kinematics_ring_crystal>>
|
|
*** Introduction :ignore:
|
|
The same reasoning is now done for the "ring" crystals.
|
|
|
|
*** Interferometers - ring primary Crystal
|
|
|
|
Three interferometers fixed to the top metrology frame are pointed to the top surface (i.e. toward negative $z$) of the primary "hall" crystal.
|
|
The measurements are perform along the $z$ direction.
|
|
|
|
The notations are:
|
|
- $\{\mathcal{F}_{1r}\}$ is the frame in which motion of the crystal are expressed
|
|
- $\mathcal{O}_{1r,uh}$ measurement point for the "upstream-hall" interferometer
|
|
- $\mathcal{O}_{1r,cr}$ measurement point for the "center-ring" interferometer
|
|
- $\mathcal{O}_{1r,dh}$ measurement point for the "downstream-hall" interferometer
|
|
|
|
The measured displacements by the interferometers are noted $[z_{1r,ur},\ z_{1r,ch},\ z_{1r,dr}]$.
|
|
|
|
The corresponding motion of the crystal expressed in the frame as shown in Figure [[fig:sensor_hall_crystal_points_primary]] are:
|
|
- $d_{1r,z}$ motion in the $z$ direction
|
|
- $r_{1r,y}$ rotation around the $y$ axis
|
|
- $r_{1r,x}$ rotation around the $x$ axis
|
|
|
|
#+begin_src latex :file sensor_ring_crystal_points_primary.pdf
|
|
\begin{tikzpicture}
|
|
% Crystal
|
|
\draw (-7.5/2, -3.5/2) rectangle (7.5/2, 3.5/2);
|
|
|
|
% Measurement Points
|
|
\node[branch] (a1) at (-3.6, 1.5){};
|
|
\node[branch] (a2) at ( 0, -1.5){};
|
|
\node[branch] (a3) at ( 3.6, 1.5){};
|
|
|
|
% Labels
|
|
\node[right] at (a1) {$\mathcal{O}_{1r,uh} = (-36, 15)$};
|
|
\node[right] at (a2) {$\mathcal{O}_{1r,cr} = ( 0, -15)$};
|
|
\node[left] at (a3) {$\mathcal{O}_{1r,dh} = ( 36, 15)$};
|
|
|
|
% Origin
|
|
\draw[->] (0, 0) node[] -- ++(1, 0) node[right]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, 1) node[right]{$y$};
|
|
\draw[fill, color=black] (0, 0) circle (0.05);
|
|
\draw[color=black] (0, 0) circle (0.15);
|
|
\node[above right] at (0, 0){$z$};
|
|
|
|
\node[left] at (0,0) {$\{\mathcal{F}_{1r}\}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:sensor_ring_crystal_points_primary
|
|
#+caption: Top view of the primary crystal ring. Position of the measurement points in [mm].
|
|
#+RESULTS:
|
|
[[file:figs/sensor_ring_crystal_points_primary.png]]
|
|
|
|
The inverse kinematics consisting of deriving the interferometer measurements from the motion of the crystal (see Figure [[fig:schematic_sensor_jacobian_inverse_kinematics_ring_primary]]):
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
z_{1r,ur} \\ z_{1r,ch} \\ z_{1r,dr}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{1r,s}
|
|
\begin{bmatrix}
|
|
d_{1r,z} \\ r_{1r,y} \\ r_{1r,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_inverse_kinematics_ring_primary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js) {$\bm{J}_{1r,s}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{1r,z} \\ r_{1r,y} \\ r_{1r,x} \end{bmatrix}$} -- (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} z_{1r,ur} \\ z_{1r,ch} \\ z_{1r,dr} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics_ring_primary
|
|
#+caption: Inverse Kinematics - Interferometers (primary "ring" crystal)
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_inverse_kinematics_ring_primary.png]]
|
|
|
|
From the measurement coordinates in Figure [[fig:sensor_ring_crystal_points_primary]] and equation eqref:eq:jacobian_3dof_negative_z, the inverse kinematics the folowing Jacobian matrix is obtained:
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{1r,s}
|
|
=
|
|
\begin{bmatrix}
|
|
-1 & -0.036 & -0.015 \\
|
|
-1 & 0 & 0.015 \\
|
|
-1 & 0.036 & -0.015
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Sensor Jacobian matrix for ring crystal
|
|
J_1r_s = [-1, -0.036, -0.015
|
|
-1, 0, 0.015
|
|
-1, 0.036, -0.015];
|
|
#+end_src
|
|
|
|
The forward kinematics is solved by inverting the Jacobian matrix (see Figure [[fig:schematic_sensor_jacobian_forward_kinematics_ring_primary]]).
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{1r,z} \\ r_{1r,y} \\ r_{1r,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{1r,s}^{-1}
|
|
\begin{bmatrix}
|
|
z_{1r,ur} \\ z_{1r,ch} \\ z_{1r,dr}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_forward_kinematics_ring_primary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js_inv) {$\bm{J}_{1r,s}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} z_{1r,ur} \\ z_{1r,ch} \\ z_{1r,dr} \end{bmatrix}$} -- (Js_inv.west);
|
|
\draw[->] (Js_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{1r,z} \\ r_{1r,y} \\ r_{1r,x} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_forward_kinematics_ring_primary
|
|
#+caption: Forward Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_forward_kinematics_ring_primary.png]]
|
|
|
|
The obtained inverse Jacobian matrix is shown in Table [[tab:inverse_jacobian_sensor_ring_primary]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_1r_s), {}, {}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_sensor_ring_primary
|
|
#+caption: Inverse of the sensor Jacobian $\bm{J}_{1r,s}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| -0.25 | -0.5 | -0.25 |
|
|
| -13.89 | 0.0 | 13.89 |
|
|
| -16.67 | 33.33 | -16.67 |
|
|
|
|
*** Interferometers - ring secondary Crystal
|
|
|
|
Three interferometers are pointed to the bottom surface (i.e. toward positive $z$) of the secondary "ring" crystal.
|
|
|
|
The position of the measurement points are shown in Figure [[fig:sensor_ring_secondary_crystal_points]] as well as the origin where the motion of the crystal is computed.
|
|
|
|
The notations are:
|
|
- $\{\mathcal{F}_{2r}\}$ is the frame in which motion of the crystal are expressed
|
|
- $\mathcal{O}_{2r,uh}$ measurement point for the "upstream-hall" interferometer
|
|
- $\mathcal{O}_{2r,cr}$ measurement point for the "center-ring" interferometer
|
|
- $\mathcal{O}_{2r,dh}$ measurement point for the "downstream-hall" interferometer
|
|
|
|
The measured displacements by the interferometers are noted $[z_{2r,ur},\ z_{2r,ch},\ z_{2r,dr}]$.
|
|
|
|
The corresponding motion of the crystal expressed in the frame as shown in Figure [[fig:sensor_hall_secondary_crystal_points]] are:
|
|
- $d_{2r,z}$ motion in the $z$ direction
|
|
- $r_{2r,y}$ rotation around the $y$ axis
|
|
- $r_{2r,x}$ rotation around the $x$ axis
|
|
The position of the measurement points are shown in Figure [[fig:sensor_ring_secondary_crystal_points]] as well as the origin where the motion of the crystal is computed.
|
|
|
|
#+begin_src latex :file sensor_ring_secondary_crystal_points.pdf
|
|
\begin{tikzpicture}
|
|
% Crystal
|
|
\draw (-15/2, -3.5/2) rectangle (15/2, 3.5/2);
|
|
|
|
% Measurement Points
|
|
\node[branch] (a1) at (-7, -1.5){};
|
|
\node[branch] (a2) at ( 0, 1.5){};
|
|
\node[branch] (a3) at ( 7, -1.5){};
|
|
|
|
% Labels
|
|
\node[right] at (a1) {$\mathcal{O}_{2r,uh} = (-70, 15)$};
|
|
\node[right] at (a2) {$\mathcal{O}_{2r,cr} = ( 0, -15)$};
|
|
\node[left] at (a3) {$\mathcal{O}_{2r,dh} = ( 70, 15)$};
|
|
|
|
% Origin
|
|
\draw[->] (0, 0) node[] -- ++(1, 0) node[below]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, -1) node[right]{$y$};
|
|
\draw[fill, color=black] (0, 0) circle (0.05);
|
|
\draw[color=black] (0, 0) circle (0.15);
|
|
\draw[] (-0.1, 0.1) -- ( 0.1, -0.1);
|
|
\draw[] (-0.1,-0.1) -- ( 0.1, 0.1);
|
|
\node[below right] at (0,0) {$z$};
|
|
|
|
\node[left] at (0,0) {$\{\mathcal{F}_{2r}\}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:sensor_ring_secondary_crystal_points
|
|
#+caption: Bottom view of the second crystal ring. Position of the measurement points.
|
|
#+RESULTS:
|
|
[[file:figs/sensor_ring_secondary_crystal_points.png]]
|
|
|
|
The inverse kinematics consisting of deriving the interferometer measurements from the motion of the crystal (see Figure [[fig:schematic_sensor_jacobian_inverse_kinematics_ring_secondary]]):
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
z_{2r,uh} \\ z_{2r,cr} \\ z_{2r,dh}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{2r,s}
|
|
\begin{bmatrix}
|
|
d_{2r,z} \\ r_{2r,y} \\ r_{2r,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_inverse_kinematics_ring_secondary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js) {$\bm{J}_{2r,s}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{2r,z} \\ r_{2r,y} \\ r_{2r,x} \end{bmatrix}$} -- (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} z_{2r,uh} \\ z_{2r,cr} \\ z_{2r,dh} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics_ring_secondary
|
|
#+caption: Inverse Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_inverse_kinematics_ring_secondary.png]]
|
|
|
|
|
|
From the measurement coordinates in Figure [[fig:sensor_ring_secondary_crystal_points]] and equation eqref:eq:jacobian_3dof_positive_z, the inverse kinematics the folowing Jacobian matrix is obtained:
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{2r,s}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.07 & 0.015 \\
|
|
1 & 0 & -0.015 \\
|
|
1 & -0.07 & 0.015
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Sensor Jacobian matrix for ring crystal
|
|
J_2r_s = [1, 0.07, 0.015
|
|
1, 0, -0.015
|
|
1, -0.07, 0.015];
|
|
#+end_src
|
|
|
|
The forward kinematics is solved by inverting the Jacobian matrix (see Figure [[fig:schematic_sensor_jacobian_forward_kinematics_ring_secondary]]).
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{2r,z} \\ r_{2r,y} \\ r_{2r,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{2r,s}^{-1}
|
|
\begin{bmatrix}
|
|
z_{2r,uh} \\ z_{2r,cr} \\ z_{2r,dh}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_forward_kinematics_ring_secondary.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js_inv) {$\bm{J}_{s,r}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} z_{2r,uh} \\ z_{2r,cr} \\ z_{2r,dh} \end{bmatrix}$} -- (Js_inv.west);
|
|
\draw[->] (Js_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{2r,z} \\ r_{2r,y} \\ r_{2r,x} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_forward_kinematics_ring_secondary
|
|
#+caption: Forward Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_forward_kinematics_ring_secondary.png]]
|
|
|
|
The obtained inverse Jacobian matrix is shown in Table [[tab:inverse_jacobian_sensor_ring_secondary]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_2r_s), {}, {}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_sensor_ring_secondary
|
|
#+caption: Inverse of the sensor Jacobian $\bm{J}_{2r,s}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.25 | 0.5 | 0.25 |
|
|
| 7.14 | 0.0 | -7.14 |
|
|
| 16.67 | -33.33 | 16.67 |
|
|
|
|
*** Piezo - ring Crystal
|
|
|
|
The location of the actuators with respect with the center of the secondary "ring" crystal are shown in Figure [[fig:actuator_jacobian_ring_points]].
|
|
|
|
#+name: fig:actuator_jacobian_ring_points
|
|
#+caption: Location of actuators with respect to the center of the ring second crystal (bottom view)
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/actuator_jacobian_ring_points.png]]
|
|
|
|
|
|
Inverse Kinematics consist of deriving the corresponding z motion of the 3 actuators from the motion of the crystal expressed in $\{\mathcal{F}_{2r}\}$.
|
|
As in the previous section, the motion of the secondary "ring" crystal is expressed by $[d_{2r,z},\ r_{2r,y},\ r_{2r,x}]$.
|
|
The motion of the three fast jacks are expressed by $[d_{u_r},\ d_{u_h},\ d_{d}]$ are are corresponding to a motion along the $z$ axis toward positive values.
|
|
|
|
The inverse kinematics can therefore be solved as follows:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{u_r} \\ d_{u_h} \\ d_{d}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{h,a}
|
|
\begin{bmatrix}
|
|
d_{2r,z} \\ r_{2r,y} \\ r_{2r,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_actuator_jacobian_inverse_kinematics_ring.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Ja) {$\bm{J}_{r,a}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{2r,z} \\ r_{2r,y} \\ r_{2r,x} \end{bmatrix}$} -- (Ja.west);
|
|
\draw[->] (Ja.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_d \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics_ring
|
|
#+caption: Inverse Kinematics - Actuators
|
|
#+RESULTS:
|
|
[[file:figs/schematic_actuator_jacobian_inverse_kinematics_ring.png]]
|
|
|
|
Based on the geometry in Figure [[fig:actuator_jacobian_ring_points]], we obtain (see Eq. eqref:eq:jacobian_3dof_positive_z):
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{r,a}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.14 & -0.0675 \\
|
|
1 & 0.14 & 0.1525 \\
|
|
1 & -0.14 & 0.0425
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Actuator Jacobian - ring crystal
|
|
J_r_a = [1, 0.14, -0.0675
|
|
1, 0.14, 0.1525
|
|
1, -0.14, 0.0425];
|
|
#+end_src
|
|
|
|
The forward Kinematics is solved by inverting the Jacobian matrix:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{2r,z} \\ r_{2r,y} \\ r_{2r,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{r,a}^{-1}
|
|
\begin{bmatrix}
|
|
d_{u_r} \\ d_{u_h} \\ d_{d}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_actuator_jacobian_forward_kinematics_ring.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Ja_inv) {$\bm{J}_{r,a}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_d \end{bmatrix}$} -- (Ja_inv.west);
|
|
\draw[->] (Ja_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{2r,z} \\ r_{2r,y} \\ r_{2r,x} \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_actuator_jacobian_forward_kinematics_ring
|
|
#+caption: Forward Kinematics - Actuators for ring crystal
|
|
#+RESULTS:
|
|
[[file:figs/schematic_actuator_jacobian_forward_kinematics_ring.png]]
|
|
|
|
The obtained inverse Jacobian matrix is shown in Table [[tab:inverse_jacobian_actuator_ring_secondary]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_r_a), {}, {}, ' %.4f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_actuator_ring_secondary
|
|
#+caption: Inverse of the actuator Jacobian $\bm{J}_{r,a}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.4432 | 0.0568 | 0.5 |
|
|
| 1.7857 | 1.7857 | -3.5714 |
|
|
| -4.5455 | 4.5455 | 0.0 |
|
|
|
|
** Kinematics of the Metrology Frame
|
|
<<sec:kinematics_metrology_frame>>
|
|
|
|
Three interferometers (aligned with the $z$ axis) are used to measure the relative motion between the bottom part and the top part of the metrology frame.
|
|
|
|
The location of the three interferometers are shown in Figure [[fig:jacobian_metrology_frame]].
|
|
|
|
The measured displacements by the interferometers are are noted $[z_{mf,u},\ z_{mf,dh},\ z_{mf,dr}]$.
|
|
|
|
We here suppose that the bottom part of the metrology frame is fixed and only the top part is moving.
|
|
The motion of the metrology top part expressed in the frame as shown in Figure [[fig:jacobian_metrology_frame]] are:
|
|
- $d_{mf,z}$ motion in the $z$ direction
|
|
- $r_{mf,y}$ rotation around the $y$ axis
|
|
- $r_{mf,x}$ rotation around the $x$ axis
|
|
|
|
#+name: fig:jacobian_metrology_frame
|
|
#+caption: Top View of the top part of the metrology frame
|
|
[[file:figs/jacobian_metrology_frame.png]]
|
|
|
|
The inverse kinematics consisting of deriving the interferometer measurements from the motion of the metrology top part:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
z_{mf,u} \\ z_{mf,dh} \\ z_{mf,dr}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{mf,s}
|
|
\begin{bmatrix}
|
|
d_{mf,z} \\ r_{mf,y} \\ r_{mf,x}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
From the coordinates in Figure [[fig:jacobian_metrology_frame]] and the equation eqref:eq:jacobian_3dof_positive_z, the following Jacobian matrix is obtained:
|
|
\begin{equation}
|
|
\boxed{\bm{J}_{mf,r}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.102 & 0 \\
|
|
1 & -0.088 & 0.1275 \\
|
|
1 & -0.088 & -0.1275
|
|
\end{bmatrix}}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Sensor Jacobian matrix for Metrology Frame
|
|
J_mf_s = [1, 0.102, 0
|
|
1, -0.088, 0.1275
|
|
1, -0.088, -0.1275];
|
|
#+end_src
|
|
|
|
The forward kinematics is solved by inverting the Jacobian matrix:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix}
|
|
d_{mf,z} \\ r_{mf,y} \\ r_{mf,x}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{mf,s}^{-1}
|
|
\begin{bmatrix}
|
|
z_{mf,u} \\ z_{mf,dh} \\ z_{mf,dr}
|
|
\end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
The obtained inverse Jacobian matrix is shown in Table [[tab:inverse_jacobian_sensor_metrology_frame]].
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_mf_s), {}, {}, ' %.3f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_sensor_metrology_frame
|
|
#+caption: Inverse of the sensor Jacobian $\bm{J}_{mf,f}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.463 | 0.268 | 0.268 |
|
|
| 5.263 | -2.632 | -2.632 |
|
|
| 0.0 | 3.922 | -3.922 |
|
|
|
|
** Computing relative position between Crystals
|
|
|
|
In the previous sections, the motion of the individual crystals have been computed.
|
|
|
|
We wish to express the *relative pose* of the primary and secondary crystals $[d_{h,z},\ r_{h,y},\ r_{h,x}]$ or $[d_{r,z},\ r_{r,y},\ r_{r,x}]$.
|
|
|
|
#+begin_note
|
|
The sign conventions for the relative crystal pose are:
|
|
- An increase of $d_{h,z}$ means the two crystals are further apart
|
|
- An increase of $r_{h,}$ means that the second crystals experiences a rotation around $y$ with respect to the primary crystal
|
|
- An increase of $r_{h,x}$ means that the second crystals experiences a rotation around $x$ with respect to the primary crystal
|
|
#+end_note
|
|
|
|
We therefore have the following relation (same for "hall" and "ring" crystals):
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix} d_{h,z} \\ r_{h,y} \\ r_{h,x} \end{bmatrix}
|
|
=
|
|
\begin{bmatrix} d_{2h,z} \\ r_{2h,y} \\ r_{2h,x} \end{bmatrix}
|
|
-
|
|
\begin{bmatrix} d_{1h,z} \\ r_{1h,y} \\ r_{1h,x} \end{bmatrix}
|
|
-
|
|
\begin{bmatrix} d_{mf,z} \\ r_{mf,y} \\ r_{mf,x} \end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
The relative pose can be expressed as a function of the interferometers using the Jacobian matrices for the "hall" crystals:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix} d_{h,z} \\ r_{h,y} \\ r_{h,x} \end{bmatrix}
|
|
=
|
|
\bm{J}_{2h,s}^{-1}
|
|
\begin{bmatrix} z_{2h,ur} \\ z_{2h,ch} \\ z_{2h,dr} \end{bmatrix}
|
|
-
|
|
\bm{J}_{1h,s}^{-1}
|
|
\begin{bmatrix} z_{1h,ur} \\ z_{1h,ch} \\ z_{1h,dr} \end{bmatrix}
|
|
-
|
|
\bm{J}_{mf,s}^{-1}
|
|
\begin{bmatrix} z_{mf,u} \\ z_{mf,dh} \\ z_{mf,dr} \end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
As well as for the "ring" crystals:
|
|
\begin{equation}
|
|
\boxed{
|
|
\begin{bmatrix} d_{r,z} \\ r_{r,y} \\ r_{r,x} \end{bmatrix}
|
|
=
|
|
\bm{J}_{2r,s}^{-1}
|
|
\begin{bmatrix} z_{2r,ur} \\ z_{2r,cr} \\ z_{2r,dr} \end{bmatrix}
|
|
-
|
|
\bm{J}_{1r,s}^{-1}
|
|
\begin{bmatrix} z_{1r,ur} \\ z_{1r,cr} \\ z_{1r,dr} \end{bmatrix}
|
|
-
|
|
\bm{J}_{mf,s}^{-1}
|
|
\begin{bmatrix} z_{mf,u} \\ z_{mf,dr} \\ z_{mf,dr} \end{bmatrix}
|
|
}
|
|
\end{equation}
|
|
|
|
** Summary - List of notations
|
|
|
|
There are 5 Jacobian matrices that are used to convert the 15 interferometers to the relative pose between the primary and secondary crystals as well as 2 Jacobian matrices for the actuators.
|
|
All Jacobian matrices are summarized in Table [[tab:jacobian_matrices]].
|
|
|
|
#+name: tab:jacobian_matrices
|
|
#+caption: List of Jacobian matrices
|
|
#+attr_latex: :environment tabularx :width 0.9\linewidth :align ccl
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Notation* | *Variable* | *Description* |
|
|
|-----------------+------------+------------------------------------------------------------|
|
|
| $\bm{J}_{1h,s}$ | =J_1h_s= | Converts interferometers to the first "hall" crystal pose |
|
|
| $\bm{J}_{2h,s}$ | =J_2h_s= | Converts interferometers to the second "hall" crystal pose |
|
|
| $\bm{J}_{1r,s}$ | =J_1r_s= | Converts interferometers to the first "ring" crystal pose |
|
|
| $\bm{J}_{2r,s}$ | =J_2r_s= | Converts interferometers to the second "ring" crystal pose |
|
|
| $\bm{J}_{mf,s}$ | =J_mf_s= | Converts interferometers to the first "hall" crystal pose |
|
|
| $\bm{J}_{h,a}$ | =J_h_a= | Converts Fastjack motion to the second "hall" crystal pose |
|
|
| $\bm{J}_{r,a}$ | =J_r_a= | Converts Fastjack motion to the second "ring" crystal pose |
|
|
|
|
The 15 interferometer measurements are summarized in Table [[tab:interferometer_list]].
|
|
|
|
#+name: tab:interferometer_list
|
|
#+caption: List of Interferometer measurements
|
|
#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccl
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Notation* | *Variable* | *Interferometer* |
|
|
|-------------+------------+------------------------------------------|
|
|
| $z_{1h,ur}$ | =z_1h_ur= | First "Hall" Crystal, "upstream-ring" |
|
|
| $z_{1h,ch}$ | =z_1h_ch= | First "Hall" Crystal, "center-hall" |
|
|
| $z_{1h,dr}$ | =z_1h_dr= | First "Hall" Crystal, "downstream-ring" |
|
|
| $z_{2h,ur}$ | =z_2h_ur= | Second "Hall" Crystal, "upstream-ring" |
|
|
| $z_{2h,ch}$ | =z_2h_ch= | Second "Hall" Crystal, "center-hall" |
|
|
| $z_{2h,dr}$ | =z_2h_dr= | Second "Hall" Crystal, "downstream-ring" |
|
|
| $z_{1r,uh}$ | =z_1r_uh= | First "Hall" Crystal, "upstream-hall" |
|
|
| $z_{1r,cr}$ | =z_1r_cr= | First "Hall" Crystal, "center-ring" |
|
|
| $z_{1r,dh}$ | =z_1r_dh= | First "Hall" Crystal, "downstream-hall" |
|
|
| $z_{2r,uh}$ | =z_2r_uh= | Second "Hall" Crystal, "upstream-hall" |
|
|
| $z_{2r,cr}$ | =z_2r_cr= | Second "Hall" Crystal, "center-ring" |
|
|
| $z_{2r,dh}$ | =z_2r_dh= | Second "Hall" Crystal, "downstream-hall" |
|
|
| $z_{mf,u}$ | =z_mf_u= | Metrology Frame, "upstream" |
|
|
| $z_{mf,dh}$ | =z_mf_dh= | Metrology Frame, "downstream-hall" |
|
|
| $z_{mf,dr}$ | =z_mf_dr= | Metrology Frame, "downstream-ring" |
|
|
|
|
From the 15 interferometers, several crystal pose are computed.
|
|
They are summarized in Table [[tab:crystal_pose_list]].
|
|
|
|
#+name: tab:crystal_pose_list
|
|
#+caption: List of crystal's pose
|
|
#+attr_latex: :environment tabularx :width 0.9\linewidth :align ccl
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Notation* | *Variable* | *Description* |
|
|
|------------+------------+---------------------------------------------------------|
|
|
| $d_{1h,z}$ | =d_1h_z= | First "Hall" Crystal, z-translation |
|
|
| $r_{1h,y}$ | =r_1h_y= | First "Hall" Crystal, y-rotation |
|
|
| $r_{1h,x}$ | =r_1h_x= | First "Hall" Crystal, x-rotation |
|
|
| $d_{2h,z}$ | =d_2h_z= | Second "Hall" Crystal, z-translation |
|
|
| $r_{2h,y}$ | =r_2h_y= | Second "Hall" Crystal, y-rotation |
|
|
| $r_{2h,x}$ | =r_2h_x= | Second "Hall" Crystal, x-rotation |
|
|
| $d_{1r,z}$ | =d_1r_z= | First "Ring" Crystal, z-translation |
|
|
| $r_{1r,y}$ | =r_1r_y= | First "Ring" Crystal, y-rotation |
|
|
| $r_{1r,x}$ | =r_1r_x= | First "Ring" Crystal, x-rotation |
|
|
| $d_{2r,z}$ | =d_2r_z= | Second "Ring" Crystal, z-translation |
|
|
| $r_{2r,y}$ | =r_2r_y= | Second "Ring" Crystal, y-rotation |
|
|
| $r_{2r,x}$ | =r_2r_x= | Second "Ring" Crystal, x-rotation |
|
|
| $d_{mf,z}$ | =d_mf_z= | Metrology Frame, z-translation (top relative to bottom) |
|
|
| $r_{mf,y}$ | =r_mf_y= | Metrology Frame, y-rotation (top relative to bottom) |
|
|
| $r_{mf,x}$ | =r_mf_x= | Metrology Frame, x-rotation (top relative to bottom) |
|
|
|
|
And finally, the relative pose between the first and second crystals are defined in Table [[tab:crystal_relative_pose_list]].
|
|
|
|
#+name: tab:crystal_relative_pose_list
|
|
#+caption: Relative pose between the primary and secondary crystals
|
|
#+attr_latex: :environment tabularx :width 0.9\linewidth :align ccl
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Notation* | *Variable* | *Description* |
|
|
|------------+------------+------------------------------------------------------------------|
|
|
| $d_{h,z}$ | =d_h_z= | Relative distance between the "hall" crystals |
|
|
| $r_{h,y}$ | =r_h_y= | Relative y-rotation of the 2nd "hall" crystal w.r.t. the primary |
|
|
| $r_{h,x}$ | =r_h_x= | Relative x-rotation of the 2nd "hall" crystal w.r.t. the primary |
|
|
| $d_{r,z}$ | =d_r_z= | Relative distance between the "ring" crystals |
|
|
| $r_{r,y}$ | =r_r_y= | Relative y-rotation of the 2nd "ring" crystal w.r.t. the primary |
|
|
| $r_{r,x}$ | =r_r_x= | Relative x-rotation of the 2nd "ring" crystal w.r.t. the primary |
|
|
|
|
** Save Kinematics
|
|
|
|
All the Jacobian matrix are exported so that there can be easily imported for computation purposes.
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/mat/dcm_kinematics.mat', 'J_1h_s', 'J_1r_s', 'J_2h_s', 'J_2r_s', 'J_mf_s', 'J_h_a', 'J_r_a')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
save('mat/dcm_kinematics.mat', 'J_1h_s', 'J_1r_s', 'J_2h_s', 'J_2r_s', 'J_mf_s', 'J_h_a', 'J_r_a')
|
|
#+end_src
|
|
|
|
* Open Loop System Identification
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_identification.m
|
|
:END:
|
|
<<sec:open_loop_identification>>
|
|
** Introduction :ignore:
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_identification.m
|
|
% Extraction of system dynamics using Simscape model
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Identification
|
|
Let's considered the system $\bm{G}(s)$ with:
|
|
- 3 inputs: force applied to the 3 fast jacks
|
|
- 3 outputs: measured displacement by the 3 interferometers pointing at the ring second crystal
|
|
|
|
It is schematically shown in Figure [[fig:schematic_system_inputs_outputs]].
|
|
|
|
#+begin_src latex :file schematic_system_inputs_outputs.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$\bm{G}(s)$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(G.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} -- (G.west);
|
|
\draw[->] (G.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_system_inputs_outputs
|
|
#+caption: Dynamical system with inputs and outputs
|
|
#+RESULTS:
|
|
[[file:figs/schematic_system_inputs_outputs.png]]
|
|
|
|
The system is identified from the Simscape model.
|
|
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Interferometers {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Extraction of the dynamics
|
|
G = linearize(mdl, io);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Input and Output names
|
|
G.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G.OutputName = {'int_r_1', 'int_r_2', 'int_r_3'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results output replace :exports both :tangle no
|
|
size(G)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: size(G)
|
|
: State-space model with 3 outputs, 3 inputs, and 24 states.
|
|
|
|
** Plant in the frame of the fastjacks
|
|
#+begin_src matlab
|
|
load('dcm_kinematics.mat');
|
|
#+end_src
|
|
|
|
Using the forward and inverse kinematics, we can computed the dynamics from piezo forces to axial motion of the 3 fastjacks (see Figure [[fig:schematic_jacobian_frame_fastjack]]).
|
|
|
|
#+begin_src latex :file schematic_jacobian_frame_fastjack.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$\bm{G}(s)$};
|
|
\node[block, right=1.5 of G] (Js) {$\bm{J}_{s}^{-1}$};
|
|
\node[block, right=1.5 of Js] (Ja) {$\bm{J}_{a}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(G.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} -- (G.west);
|
|
\draw[->] (G.east) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- node[midway, above]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} (Ja.west);
|
|
\draw[->] (Ja.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_{d} \end{bmatrix}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(G.south west) ($(Ja.east)+(0, 1.4)$)}, fill=black!20!white, draw, inner sep=6pt] (system) {};
|
|
\node[above] at (system.north) {$\bm{G}_{\text{fj}}(s)$};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_frame_fastjack
|
|
#+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_fastjack.png]]
|
|
|
|
|
|
#+begin_src matlab
|
|
%% Compute the system in the frame of the fastjacks
|
|
G_pz = J_a_h*inv(J_2h_s)*G;
|
|
#+end_src
|
|
|
|
The DC gain of the new system shows that the system is well decoupled at low frequency.
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_pz)
|
|
#+end_src
|
|
|
|
#+name: tab:dc_gain_plan_fj
|
|
#+caption: DC gain of the plant in the frame of the fast jacks $\bm{G}_{\text{fj}}$
|
|
#+attr_latex: :environment tabularx :width 0.5\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 4.4407e-09 | 2.7656e-12 | 1.0132e-12 |
|
|
| 2.7656e-12 | 4.4407e-09 | 1.0132e-12 |
|
|
| 1.0109e-12 | 1.0109e-12 | 4.4424e-09 |
|
|
|
|
The bode plot of $\bm{G}_{\text{fj}}(s)$ is shown in Figure [[fig:bode_plot_plant_fj]].
|
|
|
|
#+begin_src matlab
|
|
G_pz = diag(1./diag(dcgain(G_pz)))*G_pz;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
freqs = logspace(0,3,1000);
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
ylim([1e-2, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))));
|
|
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/bode_plot_plant_fj_pres.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:bode_plot_plant_fj
|
|
#+caption: Bode plot of the diagonal and off-diagonal elements of the plant in the frame of the fast jacks
|
|
#+RESULTS:
|
|
[[file:figs/bode_plot_plant_fj.png]]
|
|
|
|
#+begin_important
|
|
Computing the system in the frame of the fastjack gives good decoupling at low frequency (until the first resonance of the system).
|
|
#+end_important
|
|
|
|
** Plant in the frame of the crystal
|
|
#+begin_src latex :file schematic_jacobian_frame_crystal.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$\bm{G}(s)$};
|
|
\node[block, left=1.5 of G] (Ja) {$\bm{J}_{a}^{-T}$};
|
|
\node[block, right=1.5 of G] (Js) {$\bm{J}_{s}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} \mathcal{F}_{z} \\ \mathcal{M}_{y} \\ \mathcal{M}_{x} \end{bmatrix}$} -- (Ja.west);
|
|
\draw[->] (Ja.east) -- node[midway, above]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} (G.west);
|
|
\draw[->] (G.east) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(Ja.south west) ($(Js.east)+(0, 1.4)$)}, fill=black!20!white, draw, inner sep=6pt] (system) {};
|
|
\node[above] at (system.north) {$\bm{G}_{\text{cr}}(s)$};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_frame_crystal
|
|
#+caption: Use of Jacobian matrices to obtain the system in the frame of the crystal
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_crystal.png]]
|
|
|
|
#+begin_src matlab
|
|
G_mr = inv(J_s_r)*G*inv(J_a_r');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_mr)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| 1.9978e-09 | 3.9657e-09 | 7.7944e-09 |
|
|
| 3.9656e-09 | 8.4979e-08 | -1.5135e-17 |
|
|
| 7.7944e-09 | -3.9252e-17 | 1.834e-07 |
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))));
|
|
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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
fig = figure;
|
|
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
for i_out = 1:3
|
|
for i_in = 1:3
|
|
ax = nexttile;
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(i_out, i_in), freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
end
|
|
end
|
|
|
|
linkaxes(findall(fig, 'type', 'axes'),'xy');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
This results in a coupled system.
|
|
The main reason is that, as we map forces to the center of the ring crystal and not at the center of mass/stiffness of the moving part, vertical forces will induce rotation and torques will induce vertical motion.
|
|
|
|
** Plant at the center of stiffness :noexport:
|
|
|
|
Here, we map the piezo forces at the center of stiffness.
|
|
|
|
Let's first compute the Jacobian:
|
|
|
|
* Open-Loop Noise Budgeting
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_noise_budget.m
|
|
:END:
|
|
<<sec:dcm_noise_budget>>
|
|
|
|
** Introduction :ignore:
|
|
|
|
#+begin_src latex :file noise_budget_dcm_schematic_fast_jack_frame.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$G(s)$};
|
|
\node[addb, left= of G] (adddu) {};
|
|
\node[block, left= of adddu] (K) {$K(s)$};
|
|
\node[addb={+}{}{}{}{-}, left= of K] (subL) {};
|
|
\node[addb, right= of G] (addd) {};
|
|
\node[addb, below right=0.8 and 0.6 of addd] (adddn) {};
|
|
|
|
% Connections and labels
|
|
\draw[->] (subL.east) -- (K.west);
|
|
\draw[->] (K.east) -- (adddu.west);
|
|
\draw[->] (adddu.east) -- (G.west);
|
|
\draw[->] (G.east) -- (addd.west);
|
|
\draw[->] (addd-|adddn)node[branch]{}node[above]{$y_{\text{fj}}$} -- (adddn.north);
|
|
\draw[->] (adddn.west) -| (subL.south) node[below right]{$y_{\text{fj},m}$};
|
|
\draw[<-] (adddu.north) -- ++(0, 1) node[below right]{$d_u$};
|
|
\draw[<-] (addd.north) -- ++(0, 1) node[below right]{$d_{\text{fj}}$};
|
|
\draw[<-] (adddn.east) -- ++(1, 0)coordinate(dn) node[above left]{$n_{\text{fj}}$};
|
|
\draw[->] (addd.east) -- (addd-|dn);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:noise_budget_dcm_schematic_fast_jack_frame
|
|
#+caption: Schematic representation of the control loop in the frame of one fast jack
|
|
#+RESULTS:
|
|
[[file:figs/noise_budget_dcm_schematic_fast_jack_frame.png]]
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_noise_budget.m
|
|
% Basic uniaxial noise budgeting
|
|
#+end_src
|
|
|
|
#+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>>
|
|
|
|
%% Frequency vector for noise budget [Hz]
|
|
f = logspace(-1, 3, 1000);
|
|
#+end_src
|
|
|
|
** Power Spectral Density of signals
|
|
|
|
Interferometer noise:
|
|
#+begin_src matlab
|
|
Wn = 6e-11*(1 + s/2/pi/200)/(1 + s/2/pi/60); % m/sqrt(Hz)
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('Measurement noise: %.2f [nm,rms]', 1e9*sqrt(trapz(f, abs(squeeze(freqresp(Wn, f, 'Hz'))).^2)));
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: Measurement noise: 0.79 [nm,rms]
|
|
|
|
DAC noise (amplified by the PI voltage amplifier, and converted to newtons):
|
|
#+begin_src matlab
|
|
Wdac = tf(3e-8); % V/sqrt(Hz)
|
|
Wu = Wdac*22.5*10; % N/sqrt(Hz)
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('DAC noise: %.2f [uV,rms]', 1e6*sqrt(trapz(f, abs(squeeze(freqresp(Wdac, f, 'Hz'))).^2)));
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: DAC noise: 0.95 [uV,rms]
|
|
|
|
Disturbances:
|
|
#+begin_src matlab
|
|
Wd = 5e-7/(1 + s/2/pi); % m/sqrt(Hz)
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('Disturbance motion: %.2f [um,rms]', 1e6*sqrt(trapz(f, abs(squeeze(freqresp(Wd, f, 'Hz'))).^2)));
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: Disturbance motion: 0.61 [um,rms]
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/mat/asd_noises_disturbances.mat', 'Wn', 'Wu', 'Wd');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Save ASD of noise and disturbances
|
|
save('mat/asd_noises_disturbances.mat', 'Wn', 'Wu', 'Wd');
|
|
#+end_src
|
|
|
|
** Open Loop disturbance and measurement noise
|
|
The comparison of the amplitude spectral density of the measurement noise and of the jack parasitic motion is performed in Figure [[fig:open_loop_noise_budget_fast_jack]].
|
|
It confirms that the sensor noise is low enough to measure the motion errors of the crystal.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant (strain gauge output)
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(freqresp(Wn, f, 'Hz'))), ...
|
|
'DisplayName', 'n');
|
|
plot(f, abs(squeeze(freqresp(Wd, f, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
xlim([f(1), f(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/open_loop_noise_budget_fast_jack.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:open_loop_noise_budget_fast_jack
|
|
#+caption: Open Loop noise budgeting
|
|
#+RESULTS:
|
|
[[file:figs/open_loop_noise_budget_fast_jack.png]]
|
|
|
|
* Active Damping Plant (Strain gauges)
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_active_damping_strain_gauges.m
|
|
:END:
|
|
<<sec:active_damping_strain_gauges>>
|
|
** Introduction :ignore:
|
|
In this section, we wish to see whether if strain gauges fixed to the piezoelectric actuator can be used for active damping.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_active_damping_strain_gauges.m
|
|
% Active Damping using relative motion sensors (strain gauges)
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Identification
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Strain Gauges {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 2, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Extraction of the dynamics
|
|
G_sg = linearize(mdl, io);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
G_sg.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_sg.OutputName = {'sg_ur', 'sg_uh', 'sg_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_sg)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| 4.4443e-09 | 1.0339e-13 | 3.774e-14 |
|
|
| 1.0339e-13 | 4.4443e-09 | 3.774e-14 |
|
|
| 3.7792e-14 | 3.7792e-14 | 4.4444e-09 |
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant (strain gauge output)
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-14, 1e-7]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/strain_gauge_plant_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:strain_gauge_plant_bode_plot
|
|
#+caption: Bode Plot of the transfer functions from piezoelectric forces to strain gauges measuremed displacements
|
|
#+RESULTS:
|
|
[[file:figs/strain_gauge_plant_bode_plot.png]]
|
|
|
|
#+begin_important
|
|
As the distance between the poles and zeros in Figure [[fig:iff_plant_bode_plot]] is very small, little damping can be actively added using the strain gauges.
|
|
This will be confirmed using a Root Locus plot.
|
|
#+end_important
|
|
|
|
** Relative Active Damping
|
|
#+begin_src matlab
|
|
Krad_g1 = eye(3)*s/(s^2/(2*pi*500)^2 + 2*s/(2*pi*500) + 1);
|
|
#+end_src
|
|
|
|
As can be seen in Figure [[fig:relative_damping_root_locus]], very little damping can be added using relative damping strategy using strain gauges.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Root Locus for IFF
|
|
gains = logspace(3, 8, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
plot(real(pole(G_sg)), imag(pole(G_sg)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_sg)), imag(tzero(G_sg)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_sg, g*Krad_g1, -1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
g = 2e5;
|
|
clpoles = pole(feedback(G_sg, g*Krad_g1, -1));
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
|
|
'DisplayName', sprintf('$g=%.0e$', g));
|
|
hold off;
|
|
xlim([-6, 0]); ylim([0, 2700]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/relative_damping_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:relative_damping_root_locus
|
|
#+caption: Root Locus for the relative damping control
|
|
#+RESULTS:
|
|
[[file:figs/relative_damping_root_locus.png]]
|
|
|
|
#+begin_src matlab
|
|
Krad = -g*Krad_g1;
|
|
#+end_src
|
|
|
|
** Damped Plant
|
|
The controller is implemented on Simscape, and the damped plant is identified.
|
|
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% DCM Kinematics
|
|
load('dcm_kinematics.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identification of the Open Loop plant
|
|
controller.type = 0; % Open Loop
|
|
G_ol = J_a_r*inv(J_s_r)*linearize(mdl, io);
|
|
G_ol.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_ol.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identification of the damped plant with Relative Active Damping
|
|
controller.type = 2; % RAD
|
|
G_dp = J_a_r*inv(J_s_r)*linearize(mdl, io);
|
|
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the damped and undamped plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur - OL');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'd - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'uh - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'ur - IFF');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-12, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_damp_undamped_plant_rad_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_damp_undamped_plant_rad_bode_plot
|
|
#+caption: Bode plot of both the open-loop plant and the damped plant using relative active damping
|
|
#+RESULTS:
|
|
[[file:figs/comp_damp_undamped_plant_rad_bode_plot.png]]
|
|
|
|
* Active Damping Plant (Force Sensors)
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_active_damping_iff.m
|
|
:END:
|
|
<<sec:active_damping_iff>>
|
|
** Introduction :ignore:
|
|
Force sensors are added above the piezoelectric actuators.
|
|
They can consists of a simple piezoelectric ceramic stack.
|
|
See for instance cite:fleming10_integ_strain_force_feedb_high.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_active_damping_iff.m
|
|
% Test of Integral Force Feedback Strategy
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Identification
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 3, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Extraction of the dynamics
|
|
G_fs = linearize(mdl, io);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
G_fs.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_fs.OutputName = {'fs_ur', 'fs_uh', 'fs_d'};
|
|
#+end_src
|
|
|
|
The Bode plot of the identified dynamics is shown in Figure [[fig:iff_plant_bode_plot]].
|
|
At high frequency, the diagonal terms are constants while the off-diagonal terms have some roll-off.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', 'off-diag');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-13, 1e-7]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))));
|
|
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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/iff_plant_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:iff_plant_bode_plot
|
|
#+caption: Bode plot of IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/iff_plant_bode_plot.png]]
|
|
|
|
** Controller - Root Locus
|
|
We want to have integral action around the resonances of the system, but we do not want to integrate at low frequency.
|
|
Therefore, we can use a low pass filter.
|
|
|
|
#+begin_src matlab
|
|
%% Integral Force Feedback Controller
|
|
Kiff_g1 = eye(3)*1/(1 + s/2/pi/20);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Root Locus for IFF
|
|
gains = logspace(9, 12, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
plot(real(pole(G_fs)), imag(pole(G_fs)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_fs)), imag(tzero(G_fs)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
g = 8e10;
|
|
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
|
|
'DisplayName', sprintf('$g=%.0e$', g));
|
|
hold off;
|
|
axis square;
|
|
xlim([-2700, 0]); ylim([0, 2700]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:iff_root_locus
|
|
#+caption: Root Locus plot for the IFF Control strategy
|
|
#+RESULTS:
|
|
[[file:figs/iff_root_locus.png]]
|
|
|
|
#+begin_src matlab
|
|
%% Integral Force Feedback Controller with optimal gain
|
|
Kiff = g*Kiff_g1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/mat/Kiff.mat', 'Kiff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Save the IFF controller
|
|
save('mat/Kiff.mat', 'Kiff');
|
|
#+end_src
|
|
|
|
** Damped Plant
|
|
Both the Open Loop dynamics (see Figure [[fig:schematic_jacobian_frame_fastjack]]) and the dynamics with IFF (see Figure [[fig:schematic_jacobian_frame_fastjack_iff]]) are identified.
|
|
|
|
We are here interested in the dynamics from $\bm{u}^\prime = [u_{u_r}^\prime,\ u_{u_h}^\prime,\ u_d^\prime]$ (input of the damped plant) to $\bm{d}_{\text{fj}} = [d_{u_r},\ d_{u_h},\ d_d]$ (motion of the crystal expressed in the frame of the fast-jacks).
|
|
This is schematically represented in Figure [[fig:schematic_jacobian_frame_fastjack_iff]].
|
|
|
|
#+begin_src latex :file schematic_jacobian_frame_fastjack_iff.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={4.0cm}{3.0cm}] (G) {$\bm{G}(s)$};
|
|
\coordinate[] (inputF) at ($(G.south west)!0.5!(G.north west)$);
|
|
\coordinate[] (outputF) at ($(G.south east)!0.8!(G.north east)$);
|
|
\coordinate[] (outputL) at ($(G.south east)!0.2!(G.north east)$);
|
|
|
|
\node[block, right=1.5 of outputL] (Js) {$\bm{J}_{s}^{-1}$};
|
|
\node[block, right=1.5 of Js] (Ja) {$\bm{J}_{a}$};
|
|
\node[addb, left= of G] (addF) {};
|
|
\node[block, above=0.5 of G] (Kiff) {$\bm{K}_{\text{IFF}}(s)$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(addF.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} u_{u_r}^\prime \\ u_{u_h}^\prime \\ u_d^\prime \end{bmatrix}$} -- (addF.west);
|
|
\draw[->] (addF.east) -- node[miday, above]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} (inputF);
|
|
\draw[->] (outputF) -- ++(2.0, 0) node[above left]{$\begin{bmatrix} \tau_{u_r} \\ \tau_{u_h} \\ \tau_d \end{bmatrix}$};
|
|
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
|
|
\draw[->] (Kiff.west) -| (addF.north);
|
|
\draw[->] (outputL) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- node[midway, above]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} (Ja.west);
|
|
\draw[->] (Ja.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_{d} \end{bmatrix}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(G.south -| addF.west) (Ja.east |- Kiff.north)}, fill=black!20!white, draw, inner sep=6pt] (system) {};
|
|
\node[above] at (system.north) {$\bm{G}_{\text{fj,IFF}}(s)$};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_frame_fastjack_iff
|
|
#+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_fastjack_iff.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Load DCM Kinematics
|
|
load('dcm_kinematics.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the Open Loop plant
|
|
controller.type = 0; % Open Loop
|
|
G_ol = J_a_r*inv(J_s_r)*linearize(mdl, io);
|
|
G_ol.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_ol.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the damped plant with IFF
|
|
controller.type = 1; % IFF
|
|
G_dp = J_a_r*inv(J_s_r)*linearize(mdl, io);
|
|
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
The dynamics from $\bm{u}$ to $\bm{d}_{\text{fj}}$ (open-loop dynamics) and from $\bm{u}^\prime$ to $\bm{d}_{\text{fs}}$ are compared in Figure [[fig:comp_damped_undamped_plant_iff_bode_plot]].
|
|
It is clear that the Integral Force Feedback control strategy is very effective in damping the resonances of the plant.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the damped and undamped plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur - OL');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'd - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'uh - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'ur - IFF');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-12, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_damped_undamped_plant_iff_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_damped_undamped_plant_iff_bode_plot
|
|
#+caption: Bode plot of both the open-loop plant and the damped plant using IFF
|
|
#+RESULTS:
|
|
[[file:figs/comp_damped_undamped_plant_iff_bode_plot.png]]
|
|
|
|
#+begin_important
|
|
The Integral Force Feedback control strategy is very effective in damping the modes present in the plant.
|
|
#+end_important
|
|
|
|
* Feedback Control
|
|
|
|
#+begin_src latex :file schematic_jacobian_frame_fastjack_feedback.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={1.5cm}{1.5cm}] (G) at (0,0) {$\bm{G}(s)$};
|
|
\node[block, right=1.2 of G] (Js) {$\bm{J}_{s}^{-1}$};
|
|
|
|
\node[block, left=1.2 of G] (Khac) {$\bm{K}(s)$};
|
|
\node[block, left=1.2 of Khac] (Ja) {$\bm{J}_{a}$};
|
|
\node[addb={+}{}{}{}{-}, left=1.0 of Ja] (subL) {};
|
|
|
|
% Connections and labels
|
|
\draw[->] (Khac.east) -- node[midway, above]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} (G.west);
|
|
\draw[->] (G.east) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.0, 0);
|
|
|
|
\draw[->] ($(subL.west) + (-1.0, 0)$) -- node[midway, above]{$\begin{bmatrix} r_{d_z} \\ r_{r_y} \\ r_{r_x} \end{bmatrix}$} (subL.west);
|
|
\draw[->] (subL.east) -- node[midway, above]{$\begin{bmatrix} \epsilon_{d_z} \\ \epsilon_{r_y} \\ \epsilon_{r_x} \end{bmatrix}$} (Ja.west);
|
|
\draw[->] (Ja.east) -- node[midway, above]{$\begin{bmatrix} \epsilon_{d_{u_r}} \\ \epsilon_{d_{u_h}} \\ \epsilon_{d_d} \end{bmatrix}$} (Khac.west);
|
|
|
|
\draw[->] ($(Js.east) + (0.6, 0)$)node[branch]{}node[above]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} -- ++(0, -1.0) -| (subL.south);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_fastjack_feedback.png]]
|
|
|
|
* HAC-LAC (IFF) architecture
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_hac_iff.m
|
|
:END:
|
|
<<sec:hac_iff>>
|
|
** Introduction :ignore:
|
|
|
|
The HAC-LAC architecture is shown in Figure [[fig:schematic_jacobian_frame_fastjack_hac_iff]].
|
|
|
|
#+begin_src latex :file schematic_jacobian_frame_fastjack_hac_iff.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={3.0cm}{3.0cm}] (G) {$\bm{G}(s)$};
|
|
\coordinate[] (inputF) at ($(G.south west)!0.5!(G.north west)$);
|
|
\coordinate[] (outputF) at ($(G.south east)!0.8!(G.north east)$);
|
|
\coordinate[] (outputL) at ($(G.south east)!0.2!(G.north east)$);
|
|
|
|
\node[block, right=1.2 of outputL] (Js) {$\bm{J}_{s}^{-1}$};
|
|
|
|
\node[addb, left= of G] (addF) {};
|
|
\node[block, above=0.5 of G] (Kiff) {$\bm{K}_{\text{IFF}}(s)$};
|
|
|
|
\node[block, left=1.2 of addF] (Khac) {$\bm{K}_{\text{HAC}}(s)$};
|
|
\node[block, left=1.2 of Khac] (Ja) {$\bm{J}_{a}$};
|
|
\node[addb={+}{}{}{}{-}, left=1.0 of Ja] (subL) {};
|
|
|
|
|
|
% Connections and labels
|
|
\draw[->] (Khac.east) -- node[midway, above]{$\begin{bmatrix} u_{u_r}^\prime \\ u_{u_h}^\prime \\ u_d^\prime \end{bmatrix}$} (addF.west);
|
|
\draw[->] (addF.east) -- node[midway, above]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} (inputF);
|
|
\draw[->] (outputF) -- ++(2.0, 0) node[above left]{$\begin{bmatrix} \tau_{u_r} \\ \tau_{u_h} \\ \tau_d \end{bmatrix}$};
|
|
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
|
|
\draw[->] (Kiff.west) -| (addF.north);
|
|
\draw[->] (outputL) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.0, 0);
|
|
|
|
|
|
\draw[->] ($(subL.west) + (-0.8, 0)$) -- node[midway, above]{$\begin{bmatrix} r_{d_z} \\ r_{r_y} \\ r_{r_x} \end{bmatrix}$} (subL.west);
|
|
\draw[->] (subL.east) -- node[midway, above]{$\begin{bmatrix} \epsilon_{d_z} \\ \epsilon_{r_y} \\ \epsilon_{r_x} \end{bmatrix}$} (Ja.west);
|
|
\draw[->] (Ja.east) -- node[midway, above]{$\begin{bmatrix} \epsilon_{d_{u_r}} \\ \epsilon_{d_{u_h}} \\ \epsilon_{d_d} \end{bmatrix}$} (Khac.west);
|
|
|
|
\draw[->] ($(Js.east) + (0.6, 0)$)node[branch]{}node[above]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} -- ++(0, -1.0) -| (subL.south);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_frame_fastjack_hac_iff
|
|
#+caption: HAC-LAC architecture
|
|
#+attr_latex: :width \linewidth
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_fastjack_hac_iff.png]]
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_hac_iff.m
|
|
% Development of the HAC-IFF control strategy
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** System Identification
|
|
Let's identify the damped plant.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Load DCM Kinematics and IFF controller
|
|
load('dcm_kinematics.mat');
|
|
load('Kiff.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the damped plant with IFF
|
|
controller.type = 1; % IFF
|
|
G_dp = J_a_h*inv(J_2h_s)*linearize(mdl, io);
|
|
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the damped and undamped plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'd - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'uh - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'ur - IFF');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-12, 1e-8]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/bode_plot_hac_iff_plant.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:bode_plot_hac_iff_plant
|
|
#+caption: Bode Plot of the plant for the High Authority Controller (transfer function from $\bm{u}^\prime$ to $\bm{\epsilon}_d$)
|
|
#+RESULTS:
|
|
[[file:figs/bode_plot_hac_iff_plant.png]]
|
|
|
|
** High Authority Controller
|
|
Let's design a controller with a bandwidth of 100Hz.
|
|
As the plant is well decoupled and well approximated by a constant at low frequency, the high authority controller can easily be designed with SISO loop shaping.
|
|
|
|
#+begin_src matlab
|
|
%% Controller design
|
|
wc = 2*pi*100; % Wanted crossover frequency [rad/s]
|
|
a = 2; % Lead parameter
|
|
|
|
Khac = diag(1./diag(abs(evalfr(G_dp, 1j*wc)))) * ... % Diagonal controller
|
|
wc/s * ... % Integrator
|
|
1/(sqrt(a))*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))) * ... % Lead
|
|
1/(s^2/(4*wc)^2 + 2*s/(4*wc) + 1); % Low pass filter
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/mat/Khac_iff.mat', 'Khac');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
%% Save the HAC controller
|
|
save('mat/Khac_iff.mat', 'Khac');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Loop Gain
|
|
L_hac_lac = G_dp * Khac;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode Plot of the Loop Gain
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(L_hac_lac(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-2, 1e1]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/hac_iff_loop_gain_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:hac_iff_loop_gain_bode_plot
|
|
#+caption: Bode Plot of the Loop gain for the High Authority Controller
|
|
#+RESULTS:
|
|
[[file:figs/hac_iff_loop_gain_bode_plot.png]]
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Compute the Eigenvalues of the loop gain
|
|
Ldet = zeros(3, length(freqs));
|
|
|
|
Lmimo = squeeze(freqresp(L_hac_lac, freqs, 'Hz'));
|
|
for i_f = 1:length(freqs)
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
#+end_src
|
|
|
|
As shown in the Root Locus plot in Figure [[fig:loci_hac_iff_fast_jack]], the closed loop system should be stable.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot of the eigenvalues of L in the complex plane
|
|
figure;
|
|
hold on;
|
|
% Angle used to draw the circles
|
|
theta = linspace(0, 2*pi, 100);
|
|
% Unit circle
|
|
plot(cos(theta), sin(theta), '--');
|
|
% Circle for module margin
|
|
plot(-1 + min(min(abs(Ldet + 1)))*cos(theta), min(min(abs(Ldet + 1)))*sin(theta), '--');
|
|
|
|
for i = 1:3
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
|
|
end
|
|
% Unstable Point
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
axis square;
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/loci_hac_iff_fast_jack.pdf', 'width', 'normal', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:loci_hac_iff_fast_jack
|
|
#+caption: Root Locus for the High Authority Controller
|
|
#+RESULTS:
|
|
[[file:figs/loci_hac_iff_fast_jack.png]]
|
|
|
|
** Performances
|
|
In order to estimate the performances of the HAC-IFF control strategy, the transfer function from motion errors of the stepper motors to the motion error of the crystal is identified both in open loop and with the HAC-IFF strategy.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Jack Motion Erros {3x1} [m]
|
|
io(io_i) = linio([mdl, '/stepper_errors'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Interferometer Output {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'output'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the transmissibility of errors in open-loop
|
|
controller.type = 0; % Open Loop
|
|
T_ol = inv(J_s_r)*linearize(mdl, io)*J_a_r;
|
|
T_ol.InputName = {'e_dz', 'e_ry', 'e_rx'};
|
|
T_ol.OutputName = {'dx', 'ry', 'rx'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Load DCM Kinematics and IFF controller
|
|
load('dcm_kinematics.mat');
|
|
load('Kiff.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the transmissibility of errors with HAC-IFF
|
|
controller.type = 3; % IFF
|
|
T_hl = inv(J_s_r)*linearize(mdl, io)*J_a_r;
|
|
T_hl.InputName = {'e_dz', 'e_ry', 'e_rx'};
|
|
T_hl.OutputName = {'dx', 'ry', 'rx'};
|
|
#+end_src
|
|
|
|
It is first verified that the closed-loop system is stable:
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
isstable(T_hl)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
And both transmissibilities are compared in Figure [[fig:stepper_transmissibility_comp_ol_hac_iff]].
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Transmissibility of stepper errors
|
|
f = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(freqresp(T_ol(1,1), f, 'Hz'))), '-', ...
|
|
'DisplayName', '$d_z$ - OL');
|
|
plot(f, abs(squeeze(freqresp(T_ol(2,2), f, 'Hz'))), '-', ...
|
|
'DisplayName', '$r_y$ - OL');
|
|
plot(f, abs(squeeze(freqresp(T_ol(3,3), f, 'Hz'))), '-', ...
|
|
'DisplayName', '$r_x$ - OL');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(f, abs(squeeze(freqresp(T_hl(1,1), f, 'Hz'))), '--', ...
|
|
'DisplayName', '$d_z$ - HAC-IFF');
|
|
plot(f, abs(squeeze(freqresp(T_hl(2,2), f, 'Hz'))), '--', ...
|
|
'DisplayName', '$r_y$ - HAC-IFF');
|
|
plot(f, abs(squeeze(freqresp(T_hl(3,3), f, 'Hz'))), '--', ...
|
|
'DisplayName', '$r_x$ - HAC-IFF');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Stepper transmissibility');
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-2, 1e2]);
|
|
xlim([f(1), f(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/stepper_transmissibility_comp_ol_hac_iff.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:stepper_transmissibility_comp_ol_hac_iff
|
|
#+caption: Comparison of the transmissibility of errors from vibrations of the stepper motor between the open-loop case and the hac-iff case.
|
|
#+RESULTS:
|
|
[[file:figs/stepper_transmissibility_comp_ol_hac_iff.png]]
|
|
|
|
#+begin_important
|
|
The HAC-IFF control strategy can effectively reduce the transmissibility of the motion errors of the stepper motors.
|
|
This reduction is effective inside the bandwidth of the controller.
|
|
#+end_important
|
|
|
|
** Close Loop noise budget
|
|
#+begin_src matlab :exports none
|
|
%% Load disturbances
|
|
load('asd_noises_disturbances.mat');
|
|
#+end_src
|
|
|
|
Let's compute the amplitude spectral density of the jack motion errors due to the sensor noise, the actuator noise and disturbances.
|
|
|
|
#+begin_src matlab
|
|
%% Computation of ASD of contribution of inputs to the closed-loop motion
|
|
% Error due to disturbances
|
|
asd_d = abs(squeeze(freqresp(Wd*(1/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
|
|
% Error due to actuator noise
|
|
asd_u = abs(squeeze(freqresp(Wu*(G_dp(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
|
|
% Error due to sensor noise
|
|
asd_n = abs(squeeze(freqresp(Wn*(G_dp(1,1)*Khac(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
|
|
#+end_src
|
|
|
|
The closed-loop ASD is then:
|
|
#+begin_src matlab
|
|
%% ASD of the closed-loop motion
|
|
asd_cl = sqrt(asd_d.^2 + asd_u.^2 + asd_n.^2);
|
|
#+end_src
|
|
|
|
The obtained ASD are shown in Figure [[fig:close_loop_asd_noise_budget_hac_iff]].
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Noise Budget (ASD)
|
|
f = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, asd_n, 'DisplayName', '$n$');
|
|
plot(f, asd_u, 'DisplayName', '$d_u$');
|
|
plot(f, asd_d, 'DisplayName', '$d$');
|
|
plot(f, asd_cl, 'k--', 'DisplayName', '$y$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
xlim([f(1), f(end)]);
|
|
ylim([1e-16, 1e-8]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/close_loop_asd_noise_budget_hac_iff.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:close_loop_asd_noise_budget_hac_iff
|
|
#+caption: Closed Loop noise budget
|
|
#+RESULTS:
|
|
[[file:figs/close_loop_asd_noise_budget_hac_iff.png]]
|
|
|
|
Let's compare the open-loop and close-loop cases (Figure [[fig:cps_comp_ol_cl_hac_iff]]).
|
|
#+begin_src matlab :exports none
|
|
% Amplitude spectral density of the open loop motion errors [m/sqrt(Hz)]
|
|
asd_ol = abs(squeeze(freqresp(Wd, f, 'Hz')));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
% CPS of open-loop motion [m^2]
|
|
cps_ol = flip(-cumtrapz(flip(f), flip(asd_ol.^2)));
|
|
% CPS of closed-loop motion [m^2]
|
|
cps_cl = flip(-cumtrapz(flip(f), flip(asd_cl.^2)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Cumulative Power Spectrum - Motion error of fast jack
|
|
figure;
|
|
hold on;
|
|
plot(f, cps_ol, 'DisplayName', sprintf('OL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_ol(1))));
|
|
plot(f, cps_cl, 'DisplayName', sprintf('CL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_cl(1))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('CPS [$m^2$]');
|
|
legend('location', 'southwest', 'FontSize', 8);
|
|
xlim([f(1), f(end)]);
|
|
% ylim([1e-16, 1e-8]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/cps_comp_ol_cl_hac_iff.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:cps_comp_ol_cl_hac_iff
|
|
#+caption: Cumulative Power Spectrum of the open-loop and closed-loop motion error along one fast-jack
|
|
#+RESULTS:
|
|
[[file:figs/cps_comp_ol_cl_hac_iff.png]]
|
|
|
|
* Helping Functions :noexport:
|
|
** Initialize Path
|
|
#+NAME: m-init-path
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./matlab/mat/'); % Path for data
|
|
addpath('./matlab/'); % Path for scripts
|
|
|
|
%% Simscape Model - Nano Hexapod
|
|
addpath('./matlab/STEPS/')
|
|
#+END_SRC
|
|
|
|
#+NAME: m-init-path-tangle
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for data
|
|
|
|
%% Simscape Model - Nano Hexapod
|
|
addpath('./STEPS/')
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
%% Initialize Parameters for Simscape model
|
|
controller.type = 0; % Open Loop Control
|
|
|
|
%% Options for Linearization
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Open Simulink Model
|
|
mdl = 'simscape_dcm';
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(1, 3, 1000);
|
|
#+END_SRC
|
|
|
|
* Bibliography :ignore:
|
|
#+latex: \printbibliography
|