% Created 2024-03-19 Tue 11:19
% Intended LaTeX compiler: pdflatex
\documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt}

\input{preamble.tex}
\bibliography{test-bench-nass-spindle.bib}
\author{Dehaeze Thomas}
\date{\today}
\title{Nano-Hexapod on top of a Spindle - Test Bench}
\hypersetup{
 pdfauthor={Dehaeze Thomas},
 pdftitle={Nano-Hexapod on top of a Spindle - Test Bench},
 pdfkeywords={},
 pdfsubject={},
 pdfcreator={Emacs 29.2 (Org mode 9.7)}, 
 pdflang={English}}
\usepackage{biblatex}

\begin{document}

\maketitle
\tableofcontents

\clearpage
\begin{figure}[htbp]
\centering
\includegraphics[scale=1,width=\linewidth]{figs/IMG_20221220_152429.jpg}
\caption{\label{fig:picture_setup}Setup with the Spindle, nano-hexapod and metrology}
\end{figure}
\chapter{Test-Bench Description}
\begin{note}
Here are the documentation of the equipment used for this test bench:
\begin{itemize}
\item Voltage Amplifier: PiezoDrive \href{doc/PD200-V7-R1.pdf}{PD200}
\item Amplified Piezoelectric Actuator: Cedrat \href{doc/APA300ML.pdf}{APA300ML}
\item DAC/ADC: Speedgoat \href{doc/IO131-OEM-Datasheet.pdf}{IO131}
\item Encoder: Renishaw \href{doc/L-9517-9678-05-A\_Data\_sheet\_VIONiC\_series\_en.pdf}{Vionic} and used \href{doc/L-9517-9862-01-C\_Data\_sheet\_RKLC\_EN.pdf}{Ruler}
\item LION Precision \href{doc/Catalog-CPL190290.pdf}{CPL290}
\item Spindle: Lab Motion \href{doc/RS250S.pdf}{RT250S} with \href{doc/DB36\_connections\_english\_V2.pdf}{Drivebox 3.6} controller
\end{itemize}
\end{note}
\section{Alignment}

Procedure:
\begin{enumerate}
\item Align bottom sphere with the spindle rotation axis (\textasciitilde{} 10um)
\item Align top sphere with the spindle rotation axis (\textasciitilde{} 10um)
\end{enumerate}
\section{Short Range metrology system}

There are 5 interferometers pointing at 2 spheres as shown in Figure \ref{fig:LION_metrology_interferometers}.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1,width=0.5\linewidth]{figs/IMG_20221216_181305.jpg}
\caption{\label{fig:picture_metrology}Metrology system with LION sphere (1 inch diameter) and 5 interferometers fixed to their individual tip-tilts}
\end{figure}

\begin{center}
\begin{tabular}{ll}
 & Value\\
\hline
Sphere Diameter & 25.4mm\\
Distance between the spheres & 76.2mm\\
\end{tabular}
\end{center}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1,width=0.5\linewidth]{figs/LION_metrology_interferometers.png}
\caption{\label{fig:LION_metrology_interferometers}Schematic of the measurement system}
\end{figure}

\textbf{Assumptions}:
\begin{itemize}
\item Interferometers are perfectly positioned / oriented
\item Sphere is perfect
\end{itemize}

\textbf{Compute the Jacobian matrix}:
\begin{itemize}
\item From pure X-Y-Z-Rx-Ry small motions, compute the effect on the 5 measured distances
\item Compute the matrix
\item Inverse the matrix
\item Verify that it is working with simple example (for example using Solidworkds)
\end{itemize}

We have the following set of equations:
\begin{align}
d_1 &= -D_y + l_2 R_x \\
d_2 &= -D_y - l_1 R_x \\
d_3 &= -D_x - l_2 R_y \\
d_4 &= -D_x + l_1 R_y \\
d_5 &= -D_z
\end{align}

That can be written as a linear transformation:
\begin{equation}
\begin{bmatrix}
  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
\end{bmatrix} = \begin{bmatrix}
   0 & -1 &  0 &  l_2 &  0   \\
   0 & -1 &  0 & -l_1 &  0   \\
  -1 &  0 &  0 &  0   & -l_2 \\
  -1 &  0 &  0 &  0   &  l_1 \\
   0 &  0 & -1 &  0   &  0
\end{bmatrix} \cdot \begin{bmatrix}
  D_x \\ D_y \\ D_z \\ R_x \\ R_y
\end{bmatrix}
\end{equation}

By inverting the matrix, we obtain the Jacobian relation:
\begin{equation}
\begin{bmatrix}
  D_x \\ D_y \\ D_z \\ R_x \\ R_y
\end{bmatrix} = \begin{bmatrix}
   0 & -1 &  0 &  l_2 &  0   \\
   0 & -1 &  0 & -l_1 &  0   \\
  -1 &  0 &  0 &  0   & -l_2 \\
  -1 &  0 &  0 &  0   &  l_1 \\
   0 &  0 & -1 &  0   &  0
\end{bmatrix}^{-1} \cdot \begin{bmatrix}
  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
\end{bmatrix}
\end{equation}

\begin{table}[htbp]
\caption{\label{tab:jacobian_metrology}Jacobian matrix for the metrology system}
\centering
\begin{tabularx}{\linewidth}{cXXXXX}
\toprule
 & \textbf{d1} & \textbf{d2} & \textbf{d3} & \textbf{d4} & \textbf{d5}\\
\midrule
\textbf{Dx} & 0.0 & 0.0 & -0.79 & -0.21 & 0.0\\
\textbf{Dy} & -0.79 & -0.21 & -0.0 & -0.0 & 0.0\\
\textbf{Dz} & 0.0 & 0.0 & 0.0 & 0.0 & -1.0\\
\textbf{Rx} & 13.12 & -13.12 & 0.0 & -0.0 & 0.0\\
\textbf{Ry} & 0.0 & 0.0 & -13.12 & 13.12 & 0.0\\
\bottomrule
\end{tabularx}
\end{table}
\section{Spindle errors}
The spindle is rotated at 60rpm during 10 turns.
The signal of all 5 interferometers are recorded.
\subsection{Errors in \(D_x\) and \(D_y\)}

Because of the eccentricity of the reference surfaces (the spheres), we expect the motion in the X-Y plane to be a circle as a first approximation.
We can first see that in Figure \ref{fig:dx_dy_motion_rotation} that shows the measured \(D_x\) and \(D_y\) motion as a function of the \(R_z\) angle.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dx_dy_motion_rotation.png}
\caption{\label{fig:dx_dy_motion_rotation}Dx and Dy motion during the rotation}
\end{figure}

A circle is fit, and the obtained radius of the circle (i.e. the excentricity) is estimated to be:

\begin{verbatim}
Error linked to excentricity = 19 um
\end{verbatim}


The motion in the X-Y plane as well as the circle fit and the residual motion (circle fit subtracted from the measured motion) are shown in Figure \ref{fig:dx_dy_spindle_rotation}.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dx_dy_spindle_rotation.png}
\caption{\label{fig:dx_dy_spindle_rotation}Dx and Dy motion during the spindle rotation}
\end{figure}

Let's now analyse the frequency content in the signal.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dx_dy_spindle_rotation_asd.png}
\caption{\label{fig:dx_dy_spindle_rotation_asd}Amplitude Spectral Density of the measured Dx and Dy motion}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dx_dy_spindle_rotation_cas.png}
\caption{\label{fig:dx_dy_spindle_rotation_cas}Cumulative Amplitude Spectrum of the measured Dx and Dy motion}
\end{figure}
\subsection{Errors in vertical motion \(D_z\)}

The top interferometer is measuring the vertical motion of the sphere.

However, if the top sphere is not perfectly aligned with the spindle axis, there will also measure some vertical motion due to this excentricity.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dz_motion_rotation.png}
\caption{\label{fig:dz_motion_rotation}Dz motion during the rotation}
\end{figure}

Let's fit a sinus with a period of one turn.

\begin{verbatim}
Errors linked to excentricity = 410 [nm]
\end{verbatim}


\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dz_motion_rotation_excentricity.png}
\caption{\label{fig:dz_motion_rotation_excentricity}Effect of the excentricity and remaining Dz motion}
\end{figure}

If we look at the remaining motion after removing the effect of the eccentricity (Figure \ref{fig:dz_motion_rotation_excentricity}, right), we can see a signal with 20 periods every turn.
Let's fit this.

\begin{verbatim}
Errors linked to spindle motor = 58 [nm]
\end{verbatim}


\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dz_motion_rotation_poles.png}
\caption{\label{fig:dz_motion_rotation_poles}Effect of the magnetic pole pairs and remaining Dz motion}
\end{figure}

Let's look at the signal in the frequency domain.

On top of the peak at 1Hz (excentricity) and at 20Hz (number of pole pairs), we can observe a frequency of 126Hz (i.e. 126 periods per turn, approx 2.85 deg).

\begin{quote}
Could this be related to the air bearing system?
\end{quote}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dz_spindle_rotation_asd.png}
\caption{\label{fig:dz_spindle_rotation_asd}Amplitude Spectral Density of the measured Dz motion}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/dz_spindle_rotation_cas.png}
\caption{\label{fig:dz_spindle_rotation_cas}Cumulative Amplitude Spectrum of the measured Dz motion}
\end{figure}
\subsection{Angle errors in \(R_x\) and \(R_y\)}

\begin{verbatim}
amplitude = 281 urad
\end{verbatim}


\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/rx_ry_spindle_rotation.png}
\caption{\label{fig:rx_ry_spindle_rotation}Rx and Ry motion during the spindle rotation}
\end{figure}

Let's now analyse the frequency content in the signal.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/rx_ry_spindle_rotation_asd.png}
\caption{\label{fig:rx_ry_spindle_rotation_asd}Amplitude Spectral Density of the measured Rx and Ry motion}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/rx_ry_spindle_rotation_cas.png}
\caption{\label{fig:rx_ry_spindle_rotation_cas}Cumulative Amplitude Spectrum of the measured Rx and Ry motion}
\end{figure}
\chapter{Simscape Model}
A 3D view of the Simscape model is shown in Figure \ref{fig:simscape_model_spindle_bench}.
The Spindle is represented by a \emph{Bushing joint}.
Axial, radial and tilt stiffnesses are taken from the Spindle datasheet (see Table).

\begin{table}[htbp]
\caption{\label{tab:spindle_stiffnesses}Spindle stiffnesses}
\centering
\begin{tabularx}{\linewidth}{lXX}
\toprule
\textbf{Stiffness} & \textbf{Value} & \textbf{Unit}\\
\midrule
Axial & 402 & \(N/\mu m\)\\
Radial & 226 & \(N/\mu m\)\\
Tilt & 2380 & \(Nm/mrad\)\\
\bottomrule
\end{tabularx}
\end{table}

The metrology system consists of 5 distance measurements (represented by the red lines in Figure \ref{fig:simscape_model_spindle_bench}).

\begin{figure}[htbp]
\centering
\includegraphics[scale=1,width=0.5\linewidth]{figs/simscape_model_spindle_bench.jpg}
\caption{\label{fig:simscape_model_spindle_bench}Screenshot of the 3D view of the Simscape model}
\end{figure}
\section{Simscape model parameters}
The nano-hexapod is initialized.

The Jacobian matrix that computes the \([x, y, z, R_x, R_y]\) motion of the sample from the 5 interferometers is defined below.
\section{Control Architecture}

Let's note:
\begin{itemize}
\item \(d\mathcal{L}_m = [d_{\mathcal{L}_1},\ d_{\mathcal{L}_2},\ d_{\mathcal{L}_3},\ d_{\mathcal{L}_4},\ d_{\mathcal{L}_5},\ d_{\mathcal{L}_6}]\) the measurement of the 6 encoders fixed to the nano-hexapod
\item \(\bm{\tau}_m = [\tau_{m_1},\ \tau_{m_2},\ \tau_{m_3},\ \tau_{m_4},\ \tau_{m_5},\ \tau_{m_6}]\) the voltages measured by the 6 force sensors
\item \(\bm{u} = [u_1,\ u_2,\ u_3,\ u_4,\ u_5,\ u_6]\) the voltages send to the voltage amplifiers for the 6 piezoelectric actuators
\item \(R_z\) the spindle measured angle (encoder)
\item \(\bm{d}_m = [d_1,\ d_2,\ d_3,\ d_4,\ d_5]\) the distances measured by the 5 interferometers (see Figure \ref{fig:LION_metrology_interferometers_bis})
\end{itemize}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1,width=0.5\linewidth]{figs/LION_metrology_interferometers.png}
\caption{\label{fig:LION_metrology_interferometers_bis}Schematic of the measurement system}
\end{figure}
\section{Computation of the strut errors from the external metrology}

The following frames are defined:
\begin{itemize}
\item \(\{ W \}\): the frame that represents the wanted pose of the sample
\item \(\{ M \}\): the frame that represents the measured pose of the sample (estimated from the 5 interferometers and the spindle encoder)
\item \(\{ G \}\): the frame fixed to the granite and positioned at the sample's center
\item \(\{ H \}\): the frame fixed to the the spindle rotor, and positioned at the sample's center
\end{itemize}

We can express several homogeneous transformation matrices.

Frame fixed to the spindle rotor (centered on the sample's position), expressed in the frame of the granite:
\begin{equation}
{}^{G}\bm{T}_H = \begin{bmatrix}
cos(R_z) & -sin(R_z) & 0 & 0 \\
sin(R_z) &  cos(R_z) & 0 & 0 \\
0        &  0        & 1 & 0 \\
0        &  0        & 0 & 1
\end{bmatrix}
\end{equation}
with \(R_z\) the spindle encoder.


Wanted position expressed in the frame of the granite:
\begin{equation}
{}^{G}\bm{T}_W = \begin{bmatrix}
 &                                 & & r_{D_x} \\
 & \bm{R}_x(r_{R_x}) \bm{R}_y(r_{R_y}) \bm{R}_z(r_{R_z}) & & r_{D_y} \\
 &                                 & & r_{D_z} \\
0        &  0                      & 0 & 1
\end{bmatrix}
\end{equation}
with \(\bm{R}(r_{R_x}, r_{R_y}, r_{R_z})\) representing the wanted orientation of the sample with respect to the granite.
Typically, \(r_{R_x} = 0\), \(r_{R_y} = 0\) and \(r_{R_z}\) corresponds to the spindle encoder \(R_z\).


Measured position of the sample with respect to the granite:
\begin{equation}
{}^{G}\bm{T}_M = \begin{bmatrix}
 &                                 & & y_{D_x} \\
 & \bm{R}_x(y_{R_x}) \bm{R}_y(y_{R_y}) \bm{R}_z(R_z) & & y_{D_y} \\
 &                                 & & y_{D_z} \\
0        &  0                      & 0 & 1
\end{bmatrix}
\end{equation}
with \(R_z\) the spindle encoder, and \([y_{D_x},\ y_{D_y},\ y_{D_z},\ y_{R_x},\ y_{R_y}]\) are obtained from the 5 interferometers:
\begin{equation}
\begin{bmatrix}
  y_{D_x} \\ y_{D_y} \\ y_{D_z} \\ y_{R_x} \\ y_{R_y}
\end{bmatrix} = \begin{bmatrix}
   0 & -1 &  0 &  l_2 &  0   \\
   0 & -1 &  0 & -l_1 &  0   \\
  -1 &  0 &  0 &  0   & -l_2 \\
  -1 &  0 &  0 &  0   &  l_1 \\
   0 &  0 & -1 &  0   &  0
\end{bmatrix}^{-1} \cdot \begin{bmatrix}
  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
\end{bmatrix}
\end{equation}


In order to have the \textbf{position error in the frame of the nano-hexapod}, we have to compute \({}^M\bm{T}_W\):
\begin{align}
{}^M\bm{T}_W &= {}^M\bm{T}_G \cdot {}^G\bm{T}_W \\
             &= {{}^G\bm{T}_M}^{-1} \cdot {}^G\bm{T}_W
\end{align}

The \textbf{inverse of the transformation matrix} can be obtained by
\begin{equation}
  {}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
  \left[ \begin{array}{ccc|c}
      &   &   & \\
      & {}^A\bm{R}_B^T  &   & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
      &   &   & \cr
    \hline
    0 & 0 & 0 & 1 \\
  \end{array} \right]
\end{equation}

The position errors \(\bm{\epsilon}_{\mathcal{X}} = [\epsilon_{D_x},\ \epsilon_{D_y},\ \epsilon_{D_z},\ \epsilon_{R_x},\ \epsilon_{R_y},\ \epsilon_{R_z}]\) expressed in a frame fixed to the nano-hexapod can be extracted from \({}^W\bm{T}_M\):
\begin{itemize}
\item \(\epsilon_{D_x} = {}^M\bm{T}_W(1,4)\)
\item \(\epsilon_{D_y} = {}^M\bm{T}_W(2,4)\)
\item \(\epsilon_{D_z} = {}^M\bm{T}_W(3,4)\)
\item \(\epsilon_{R_y} = \text{atan2}({}^M\bm{T}_W(1,3), \sqrt{{}^M\bm{T}_W(1,1)^2 + {}^M\bm{T}_W(1,2)^2})\)
\item \(\epsilon_{R_x} = \text{atan2}(\frac{-{}^M\bm{T}_W(2,3)}{\cos(\epsilon_{R_y})}, \frac{{}^M\bm{T}_W(3,3)}{\cos(\epsilon_{R_y})})\)
\item \(\epsilon_{R_z} = \text{atan2}(\frac{-{}^M\bm{T}_W(1,2)}{\cos(\epsilon_{R_y})}, \frac{{}^M\bm{T}_W(1,1)}{\cos(\epsilon_{R_y})})\)
\end{itemize}

Finally, the strut errors \(\bm{\epsilon}_{\mathcal{L}} = [\epsilon_{\matcal{L}_1},\ \epsilon_{\matcal{L}_2},\ \epsilon_{\matcal{L}_3},\ \epsilon_{\matcal{L}_4},\ \epsilon_{\matcal{L}_5},\ \epsilon_{\matcal{L}_6}]\) can be computed from:
\begin{equation}
\bm{\epsilon}_\mathcal{L} = \bm{J} \cdot \bm{\epsilon}_\mathcal{X}
\end{equation}
\section{IFF Plant}
\section{DVF Plant}
\section{HAC Plant}
The transfer functions from the 6 actuator inputs to the 6 estimated strut errors are extracted from the Simscape model.

The obtained transfer functions are shown in Figure \ref{fig:simscape_model_hac_plant}.

We can see that the system is well decoupled at low frequency (i.e. below the first resonance of the Nano-Hexapod).

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/simscape_model_hac_plant.png}
\caption{\label{fig:simscape_model_hac_plant}HAC plant obtained on the Simscape model}
\end{figure}
\chapter{Control Experiment}
\section{IFF Plant}
\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/G_iif_exp_no_rotation.png}
\caption{\label{fig:G_iif_exp_no_rotation}Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/G_iif_exp_comp_no_rotation.png}
\caption{\label{fig:G_iif_exp_comp_no_rotation}Comparison with the model}
\end{figure}
\section{IFF Controller}
\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/root_locus_iff_no_payload.png}
\caption{\label{fig:root_locus_iff_no_payload}Root Locus for IFF}
\end{figure}
\section{Open Loop Plant}
Here the \(R_z\) motion of the Hexapod is estimated from the encoders.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/G_damp_exp_no_rotation.png}
\caption{\label{fig:G_damp_exp_no_rotation}Obtained transfer function from generated voltages to estimated strut motion}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/comp_hac_plant_exp_simscape.png}
\caption{\label{fig:comp_hac_plant_exp_simscape}Comparison of the open-loop plant measured experimentally and extracted from Simscape}
\end{figure}
\section{Damped Plant}
\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/G_damp_damped_exp_no_rotation.png}
\caption{\label{fig:G_damp_damped_exp_no_rotation}Obtained transfer function from generated voltages to estimated strut motion}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/comp_damped_undamped_plant.png}
\caption{\label{fig:comp_damped_undamped_plant}Comparison of the undamped and damped plant with IFF}
\end{figure}
\section{HAC Controller}
\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/first_hac_K_exp_loop_gain.png}
\caption{\label{fig:first_hac_K_exp_loop_gain}Loop gain for the HAC}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/first_hac_K_exp_root_locus.png}
\caption{\label{fig:first_hac_K_exp_root_locus}Obtained Root Locus}
\end{figure}
\section{Compare dynamics seen by interferometers and by encoders}
\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/comp_dynamics_int_ext_metrology.png}
\caption{\label{fig:comp_dynamics_int_ext_metrology}Comparison of the identified dynamic by the internal metrology (encoders) and by the external metrology (interferometers)}
\end{figure}
\section{Compare dynamics obtained with different Rz estimations}
\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/comp_plant_encoders_Va.png}
\caption{\label{fig:comp_plant_encoders_Va}Comparison of the obtained plant using the Encoders or using the output Voltages to estimate Rz}
\end{figure}
\chapter{Closed-Loop Results}
\section{Open and Closed loop results}
\begin{center}
\includegraphics[scale=1]{figs/spindle_errors_1rpm_ol.png}
\end{center}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/spindle_errors_1rpm_op_cl.png}
\caption{\label{fig:spindle_errors_1rpm_op_cl}Comparison of the Open-Loop and Closed-Loop spindle errors}
\end{figure}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/spindle_errors_1rpm_op_cl_rot.png}
\caption{\label{fig:spindle_errors_1rpm_op_cl_rot}Comparison of the Open-Loop and Closed-Loop spindle errors - Rotation}
\end{figure}
\end{document}
