163 lines
6.3 KiB
Mathematica
163 lines
6.3 KiB
Mathematica
|
% Matlab Init :noexport:ignore:
|
||
|
|
|
||
|
|
%% ustation_1_kinematics.m
|
||
|
|
|
||
|
|
%% Clear Workspace and Close figures
|
||
|
|
clear; close all; clc;
|
||
|
|
|
||
|
|
%% Intialize Laplace variable
|
||
|
|
s = zpk('s');
|
||
|
|
|
||
|
|
%% Path for functions, data and scripts
|
||
|
|
addpath('./mat/'); % Path for Data
|
||
|
|
addpath('./src/'); % Path for functions
|
||
|
|
addpath('./STEPS/'); % Path for STEPS
|
||
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
||
|
|
|
||
|
|
% Simulink Model name
|
||
|
|
mdl = 'ustation_simscape';
|
||
|
|
|
||
|
|
load('nass_model_conf_simulink.mat');
|
||
|
|
|
||
|
|
%% Colors for the figures
|
||
|
|
colors = colororder;
|
||
|
|
|
||
|
|
%% Frequency Vector
|
||
|
|
freqs = logspace(log10(10), log10(2e3), 1000);
|
||
|
|
|
||
|
|
% Micro-Station Kinematics
|
||
|
|
% <<ssec:ustation_kinematics>>
|
||
|
|
|
||
|
|
% Each stage is described by two frames; one is attached to the fixed platform $\{A\}$ while the other is fixed to the mobile platform $\{B\}$.
|
||
|
|
% At "rest" position, the two have the same pose and coincide with the point of interest ($O_A = O_B$).
|
||
|
|
% An example of the tilt stage is shown in Figure ref:fig:ustation_stage_motion.
|
||
|
|
% The mobile frame of the translation stage is equal to the fixed frame of the tilt stage: $\{B_{D_y}\} = \{A_{R_y}\}$.
|
||
|
|
% Similarly, the mobile frame of the tilt stage is equal to the fixed frame of the spindle: $\{B_{R_y}\} = \{A_{R_z}\}$.
|
||
|
|
|
||
|
|
% #+name: fig:ustation_stage_motion
|
||
|
|
% #+caption: Example of the motion induced by the tilt-stage $R_y$. "Rest" position in shown in blue while a arbitrary position in shown in red. Parasitic motions are here magnified for clarity.
|
||
|
|
% [[file:figs/ustation_stage_motion.png]]
|
||
|
|
|
||
|
|
% The motion induced by a positioning stage can be described by a homogeneous transformation matrix from frame $\{A\}$ to frame $\{B\}$ as explain in Section ref:ssec:ustation_kinematics.
|
||
|
|
% As any motion stage induces parasitic motion in all 6 DoF, the transformation matrix representing its induced motion can be written as in eqref:eq:ustation_translation_stage_errors.
|
||
|
|
|
||
|
|
% \begin{equation}\label{eq:ustation_translation_stage_errors}
|
||
|
|
% {}^A\mathbf{T}_B(D_x, D_y, D_z, \theta_x, \theta_y, \theta_z) =
|
||
|
|
% \left[ \begin{array}{ccc|c}
|
||
|
|
% & & & D_x \\
|
||
|
|
% & \mathbf{R}_x(\theta_x) \mathbf{R}_y(\theta_y) \mathbf{R}_z(\theta_z) & & D_y \\
|
||
|
|
% & & & D_z \cr
|
||
|
|
% \hline
|
||
|
|
% 0 & 0 & 0 & 1
|
||
|
|
% \end{array} \right]
|
||
|
|
% \end{equation}
|
||
|
|
|
||
|
|
% The homogeneous transformation matrix corresponding to the micro-station $\mathbf{T}_{\mu\text{-station}}$ is simply equal to the matrix multiplication of the homogeneous transformation matrices of the individual stages as shown in Equation eqref:eq:ustation_transformation_station.
|
||
|
|
|
||
|
|
% \begin{equation}\label{eq:ustation_transformation_station}
|
||
|
|
% \mathbf{T}_{\mu\text{-station}} = \mathbf{T}_{D_y} \cdot \mathbf{T}_{R_y} \cdot \mathbf{T}_{R_z} \cdot \mathbf{T}_{\mu\text{-hexapod}}
|
||
|
|
% \end{equation}
|
||
|
|
|
||
|
|
% $\mathbf{T}_{\mu\text{-station}}$ represents the pose of the sample (supposed to be rigidly fixed on top of the positioning-hexapod) with respect to the granite.
|
||
|
|
|
||
|
|
% If the transformation matrices of the individual stages are each representing a perfect motion (i.e. the stages are supposed to have no parasitic motion), $\mathbf{T}_{\mu\text{-station}}$ then represent the pose setpoint of the sample with respect to the granite.
|
||
|
|
% The transformation matrices for the translation stage, tilt stage, spindle, and positioning hexapod can be written as shown in Equation eqref:eq:ustation_transformation_matrices_stages.
|
||
|
|
|
||
|
|
% \begin{equation}\label{eq:ustation_transformation_matrices_stages}
|
||
|
|
% \begin{align}
|
||
|
|
% \mathbf{T}_{D_y} &= \begin{bmatrix}
|
||
|
|
% 1 & 0 & 0 & 0 \\
|
||
|
|
% 0 & 1 & 0 & D_y \\
|
||
|
|
% 0 & 0 & 1 & 0 \\
|
||
|
|
% 0 & 0 & 0 & 1
|
||
|
|
% \end{bmatrix} \quad
|
||
|
|
% \mathbf{T}_{\mu\text{-hexapod}} =
|
||
|
|
% \left[ \begin{array}{ccc|c}
|
||
|
|
% & & & D_{\mu x} \\
|
||
|
|
% & \mathbf{R}_x(\theta_{\mu x}) \mathbf{R}_y(\theta_{\mu y}) \mathbf{R}_{z}(\theta_{\mu z}) & & D_{\mu y} \\
|
||
|
|
% & & & D_{\mu z} \cr
|
||
|
|
% \hline
|
||
|
|
% 0 & 0 & 0 & 1
|
||
|
|
% \end{array} \right] \\
|
||
|
|
% \mathbf{T}_{R_z} &= \begin{bmatrix}
|
||
|
|
% \cos(\theta_z) & -\sin(\theta_z) & 0 & 0 \\
|
||
|
|
% \sin(\theta_z) & \cos(\theta_z) & 0 & 0 \\
|
||
|
|
% 0 & 0 & 1 & 0 \\
|
||
|
|
% 0 & 0 & 0 & 1
|
||
|
|
% \end{bmatrix} \quad
|
||
|
|
% \mathbf{T}_{R_y} = \begin{bmatrix}
|
||
|
|
% \cos(\theta_y) & 0 & \sin(\theta_y) & 0 \\
|
||
|
|
% 0 & 1 & 0 & 0 \\
|
||
|
|
% -\sin(\theta_y) & 0 & \cos(\theta_y) & 0 \\
|
||
|
|
% 0 & 0 & 0 & 1
|
||
|
|
% \end{bmatrix}
|
||
|
|
% \end{align}
|
||
|
|
% \end{equation}
|
||
|
|
|
||
|
|
|
||
|
|
%% Stage setpoints
|
||
|
|
Dy = 1e-3; % Translation Stage [m]
|
||
|
|
Ry = 3*pi/180; % Tilt Stage [rad]
|
||
|
|
Rz = 180*pi/180; % Spindle [rad]
|
||
|
|
|
||
|
|
%% Stage individual Homogeneous transformations
|
||
|
|
% Translation Stage
|
||
|
|
Rty = [1 0 0 0;
|
||
|
|
0 1 0 Dy;
|
||
|
|
0 0 1 0;
|
||
|
|
0 0 0 1];
|
||
|
|
|
||
|
|
% Tilt Stage - Pure rotating aligned with Ob
|
||
|
|
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
||
|
|
0 1 0 0;
|
||
|
|
-sin(Ry) 0 cos(Ry) 0;
|
||
|
|
0 0 0 1];
|
||
|
|
|
||
|
|
% Spindle - Rotation along the Z axis
|
||
|
|
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
||
|
|
sin(Rz) cos(Rz) 0 0 ;
|
||
|
|
0 0 1 0 ;
|
||
|
|
0 0 0 1 ];
|
||
|
|
|
||
|
|
% Micro-Station homogeneous transformation
|
||
|
|
Ttot = Rty*Rry*Rrz;
|
||
|
|
|
||
|
|
%% Compute translations and rotations (Euler angles) induced by the micro-station
|
||
|
|
ustation_dx = Ttot(1,4);
|
||
|
|
ustation_dy = Ttot(2,4);
|
||
|
|
ustation_dz = Ttot(3,4);
|
||
|
|
ustation_ry = atan2( Ttot(1, 3), sqrt(Ttot(1, 1)^2 + Ttot(1, 2)^2));
|
||
|
|
ustation_rx = atan2(-Ttot(2, 3)/cos(Ery), Ttot(3, 3)/cos(Ery));
|
||
|
|
ustation_rz = atan2(-Ttot(1, 2)/cos(Ery), Ttot(1, 1)/cos(Ery));
|
||
|
|
|
||
|
|
%% Verification using the Simscape model
|
||
|
|
% All stages are initialized as rigid bodies to avoid any guiding error
|
||
|
|
initializeGround( 'type', 'rigid');
|
||
|
|
initializeGranite( 'type', 'rigid');
|
||
|
|
initializeTy( 'type', 'rigid');
|
||
|
|
initializeRy( 'type', 'rigid');
|
||
|
|
initializeRz( 'type', 'rigid');
|
||
|
|
initializeMicroHexapod('type', 'rigid');
|
||
|
|
|
||
|
|
initializeLoggingConfiguration('log', 'all');
|
||
|
|
|
||
|
|
initializeReferences('Dy_amplitude', Dy, ...
|
||
|
|
'Ry_amplitude', Ry, ...
|
||
|
|
'Rz_amplitude', Rz);
|
||
|
|
|
||
|
|
initializeDisturbances('enable', false);
|
||
|
|
|
||
|
|
set_param(conf_simulink, 'StopTime', '0.5');
|
||
|
|
|
||
|
|
% Simulation is performed
|
||
|
|
sim(mdl);
|
||
|
|
|
||
|
|
% Sample's motion is computed from "external metrology"
|
||
|
|
T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end); simout.y.z.Data(end)]; [0,0,0,1]];
|
||
|
|
sim_dx = T_sim(1,4);
|
||
|
|
sim_dy = T_sim(2,4);
|
||
|
|
sim_dz = T_sim(3,4);
|
||
|
|
sim_ry = atan2( T_sim(1, 3), sqrt(T_sim(1, 1)^2 + T_sim(1, 2)^2));
|
||
|
|
sim_rx = atan2(-T_sim(2, 3)/cos(Ery), T_sim(3, 3)/cos(Ery));
|
||
|
|
sim_rz = atan2(-T_sim(1, 2)/cos(Ery), T_sim(1, 1)/cos(Ery));
|