251 lines
11 KiB
Matlab
251 lines
11 KiB
Matlab
%% 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
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
|
|
|
|
% #+name: fig:modal_local_to_global_coordinates
|
|
% #+caption: Schematic of the measured motions of a solid body
|
|
% #+RESULTS:
|
|
% [[file:figs/modal_local_to_global_coordinates.png]]
|
|
|
|
% The motion of the rigid body of figure ref:fig:modal_local_to_global_coordinates can be described by its displacement $\vec{\delta}p = [\delta p_x,\ \delta p_y,\ \delta p_z]$ and (small) rotations $[\delta \Omega_x,\ \delta \Omega_y,\ \delta \Omega_z]$ with respect to a reference frame $\{O\}$.
|
|
|
|
% The motion $\vec{\delta} p_{i}$ of a point $p_i$ can be computed from $\vec{\delta} p$ and $\bm{\delta \Omega}$ using equation eqref:eq:modal_compute_point_response, with $\bm{\delta\Omega}$ defined in equation eqref:eq:modal_rotation_matrix.
|
|
|
|
% \begin{equation}\label{eq:modal_compute_point_response}
|
|
% \vec{\delta} p_{i} &= \vec{\delta} p + \bm{\delta \Omega} \cdot \vec{p}_{i} \\
|
|
% \end{equation}
|
|
|
|
% \begin{equation}\label{eq:modal_rotation_matrix}
|
|
% \bm{\delta\Omega} = \begin{bmatrix}
|
|
% 0 & -\delta\Omega_z & \delta\Omega_y \\
|
|
% \delta\Omega_z & 0 & -\delta\Omega_x \\
|
|
% -\delta\Omega_y & \delta\Omega_x & 0
|
|
% \end{bmatrix}
|
|
% \end{equation}
|
|
|
|
% Writing this in a matrix form for the four points gives eqref:eq:modal_cart_to_acc.
|
|
|
|
% \begin{equation}\label{eq:modal_cart_to_acc}
|
|
% \left[\begin{array}{c}
|
|
% \delta p_{1x} \\ \delta p_{1y} \\ \delta p_{1z} \\\hline \vdots \\\hline \delta p_{4x} \\ \delta p_{4y} \\ \delta p_{4z}
|
|
% \end{array}\right] =
|
|
% \left[\begin{array}{ccc|ccc}
|
|
% 1 & 0 & 0 & 0 & p_{1z} & -p_{1y} \\
|
|
% 0 & 1 & 0 & -p_{1z} & 0 & p_{1x} \\
|
|
% 0 & 0 & 1 & p_{1y} & -p_{1x} & 0 \\ \hline
|
|
% & \vdots & & & \vdots & \\ \hline
|
|
% 1 & 0 & 0 & 0 & p_{4z} & -p_{4y} \\
|
|
% 0 & 1 & 0 & -p_{4z} & 0 & p_{4x} \\
|
|
% 0 & 0 & 1 & p_{4y} & -p_{4x} & 0
|
|
% \end{array}\right] \left[\begin{array}{c}
|
|
% \delta p_x \\ \delta p_y \\ \delta p_z \\ \hline \delta\Omega_x \\ \delta\Omega_y \\ \delta\Omega_z
|
|
% \end{array}\right]
|
|
% \end{equation}
|
|
|
|
% Provided that the four sensors are properly located, the system of equation eqref:eq:modal_cart_to_acc can be solved by matrix inversion[fn:5].
|
|
% The motion of the solid body expressed in a chosen frame $\{O\}$ can be determined using equation eqref:eq:modal_determine_global_disp.
|
|
% Note that this matrix inversion is equivalent to resolving a mean square problem.
|
|
% Therefore, having more accelerometers permits to have a better approximation of the motion of the solid body.
|
|
|
|
% \begin{equation}
|
|
% \left[\begin{array}{c}
|
|
% \delta p_x \\ \delta p_y \\ \delta p_z \\ \hline \delta\Omega_x \\ \delta\Omega_y \\ \delta\Omega_z
|
|
% \end{array}\right] =
|
|
% \left[\begin{array}{ccc|ccc}
|
|
% 1 & 0 & 0 & 0 & p_{1z} & -p_{1y} \\
|
|
% 0 & 1 & 0 & -p_{1z} & 0 & p_{1x} \\
|
|
% 0 & 0 & 1 & p_{1y} & -p_{1x} & 0 \\ \hline
|
|
% & \vdots & & & \vdots & \\ \hline
|
|
% 1 & 0 & 0 & 0 & p_{4z} & -p_{4y} \\
|
|
% 0 & 1 & 0 & -p_{4z} & 0 & p_{4x} \\
|
|
% 0 & 0 & 1 & p_{4y} & -p_{4x} & 0
|
|
% \end{array}\right]^{-1} \left[\begin{array}{c}
|
|
% \delta p_{1x} \\ \delta p_{1y} \\ \delta p_{1z} \\\hline \vdots \\\hline \delta p_{4x} \\ \delta p_{4y} \\ \delta p_{4z}
|
|
% \end{array}\right] \label{eq:modal_determine_global_disp}
|
|
% \end{equation}
|
|
|
|
|
|
% From the CAD model, the position of the center of mass of each considered solid body is computed (see Table ref:tab:modal_com_solid_bodies).
|
|
% Then, the position of each accelerometer with respect to the center of mass of the corresponding solid body can easily be derived.
|
|
|
|
|
|
%% Load frequency response matrix
|
|
load('frf_matrix.mat', 'freqs', 'frf');
|
|
|
|
%% Load Accelerometer positions
|
|
acc_pos = readtable('mat/acc_pos.txt', 'ReadVariableNames', false);
|
|
acc_pos = table2array(acc_pos(:, 1:4));
|
|
[~, i] = sort(acc_pos(:, 1));
|
|
acc_pos = acc_pos(i, 2:4);
|
|
|
|
%% Accelerometers ID connected to each solid body
|
|
solids = {};
|
|
solids.gbot = [17, 18, 19, 20]; % bottom granite
|
|
solids.gtop = [13, 14, 15, 16]; % top granite
|
|
solids.ty = [9, 10, 11, 12]; % Ty stage
|
|
solids.ry = [5, 6, 7, 8]; % Ry stage
|
|
solids.rz = [21, 22, 23]; % Rz stage
|
|
solids.hexa = [1, 2, 3, 4]; % Hexapod
|
|
|
|
% Names of the solid bodies
|
|
solid_names = fields(solids);
|
|
|
|
%% Save the acceleromter positions are well as the solid bodies
|
|
save('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');
|
|
|
|
%% Extract the CoM of considered solid bodies
|
|
model_com = reshape(table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false)), [3, 6]);
|
|
|
|
|
|
|
|
% #+name: tab:modal_com_solid_bodies
|
|
% #+caption: Center of mass of considered solid bodies with respect to the "point of interest"
|
|
% #+attr_latex: :environment tabularx :width 0.6\linewidth :align lXXX
|
|
% #+attr_latex: :center t :booktabs t
|
|
% #+RESULTS:
|
|
% | | $X$ [mm] | $Y$ [mm] | $Z$ [mm] |
|
|
% |-------------------+----------+----------+----------|
|
|
% | Bottom Granite | 45 | 144 | -1251 |
|
|
% | Top granite | 52 | 258 | -778 |
|
|
% | Translation stage | 0 | 14 | -600 |
|
|
% | Tilt Stage | 0 | -5 | -628 |
|
|
% | Spindle | 0 | 0 | -580 |
|
|
% | Hexapod | -4 | 6 | -319 |
|
|
|
|
% Using eqref:eq:modal_determine_global_disp, the frequency response matrix $\mathbf{H}_\text{CoM}$ eqref:eq:modal_frf_matrix_com expressing the response at the center of mass of each solid body $D_i$ ($i$ from $1$ to $6$ for the $6$ considered solid bodies) can be computed from the initial acrshort:frf matrix $\mathbf{H}$.
|
|
|
|
% \begin{equation}\label{eq:modal_frf_matrix_com}
|
|
% \mathbf{H}_\text{CoM}(\omega_i) = \begin{bmatrix}
|
|
% \frac{D_{1,T_x}}{F_x}(\omega_i) & \frac{D_{1,T_x}}{F_y}(\omega_i) & \frac{D_{1,T_x}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1,T_y}}{F_x}(\omega_i) & \frac{D_{1,T_y}}{F_y}(\omega_i) & \frac{D_{1,T_y}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1,T_z}}{F_x}(\omega_i) & \frac{D_{1,T_z}}{F_y}(\omega_i) & \frac{D_{1,T_z}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1,R_x}}{F_x}(\omega_i) & \frac{D_{1,R_x}}{F_y}(\omega_i) & \frac{D_{1,R_x}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1,R_y}}{F_x}(\omega_i) & \frac{D_{1,R_y}}{F_y}(\omega_i) & \frac{D_{1,R_y}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1,R_z}}{F_x}(\omega_i) & \frac{D_{1,R_z}}{F_y}(\omega_i) & \frac{D_{1,R_z}}{F_z}(\omega_i) \\
|
|
% \frac{D_{2,T_x}}{F_x}(\omega_i) & \frac{D_{2,T_x}}{F_y}(\omega_i) & \frac{D_{2,T_x}}{F_z}(\omega_i) \\
|
|
% \vdots & \vdots & \vdots \\
|
|
% \frac{D_{6,R_z}}{F_x}(\omega_i) & \frac{D_{6,R_z}}{F_y}(\omega_i) & \frac{D_{6,R_z}}{F_z}(\omega_i)
|
|
% \end{bmatrix}
|
|
% \end{equation}
|
|
|
|
|
|
%% Frequency Response Matrix - Response expressed at the CoM of the solid bodies
|
|
frfs_CoM = zeros(length(solid_names)*6, 3, 801);
|
|
|
|
for solid_i = 1:length(solid_names)
|
|
% Number of accelerometers fixed to this solid body
|
|
solids_i = solids.(solid_names{solid_i});
|
|
|
|
% "Jacobian" matrix to go from accelerometer frame to CoM frame
|
|
A = zeros(3*length(solids_i), 6);
|
|
for i = 1:length(solids_i)
|
|
acc_i = solids_i(i);
|
|
|
|
acc_pos_com = acc_pos(acc_i, :).' - model_com(:, solid_i);
|
|
|
|
A(3*(i-1)+1:3*i, 1:3) = eye(3);
|
|
A(3*(i-1)+1:3*i, 4:6) = [ 0 acc_pos_com(3) -acc_pos_com(2) ;
|
|
-acc_pos_com(3) 0 acc_pos_com(1) ;
|
|
acc_pos_com(2) -acc_pos_com(1) 0];
|
|
end
|
|
|
|
for exc_dir = 1:3
|
|
frfs_CoM((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(frf((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
|
|
end
|
|
end
|
|
|
|
%% Save the computed FRF at the CoM
|
|
save('mat/frf_com.mat', 'frfs_CoM');
|
|
|
|
% Verification of solid body assumption
|
|
% <<ssec:modal_solid_body_assumption>>
|
|
|
|
% From the response of one solid body expressed by its 6 acrshortpl:dof (i.e. from $\mathbf{H}_{\text{CoM}}$), and using equation eqref:eq:modal_cart_to_acc, it is possible to compute the response of the same solid body at any considered position.
|
|
% In particular, the response at the location of the four accelerometers can be computed and compared with the original measurements $\mathbf{H}$.
|
|
% This is what is here done to check if solid body assumption is correct in the frequency band of interest.
|
|
|
|
|
|
%% Compute the FRF at the accelerometer location from the CoM reponses
|
|
frfs_A = zeros(size(frf));
|
|
|
|
% For each excitation direction
|
|
for exc_dir = 1:3
|
|
% For each solid
|
|
for solid_i = 1:length(solid_names)
|
|
v0 = squeeze(frfs_CoM((solid_i-1)*6+1:(solid_i-1)*6+3, exc_dir, :));
|
|
W0 = squeeze(frfs_CoM((solid_i-1)*6+4:(solid_i-1)*6+6, exc_dir, :));
|
|
|
|
% For each accelerometer attached to the current solid
|
|
for acc_i = solids.(solid_names{solid_i})
|
|
% We get the position of the accelerometer expressed in frame O
|
|
pos = acc_pos(acc_i, :).' - model_com(:, solid_i);
|
|
% pos = acc_pos(acc_i, :).';
|
|
posX = [0 pos(3) -pos(2); -pos(3) 0 pos(1) ; pos(2) -pos(1) 0];
|
|
|
|
frfs_A(3*(acc_i-1)+1:3*(acc_i-1)+3, exc_dir, :) = v0 + posX*W0;
|
|
end
|
|
end
|
|
end
|
|
|
|
|
|
|
|
% The comparison is made for the 4 accelerometers fixed to the micro-hexapod (Figure ref:fig:modal_comp_acc_solid_body_frf).
|
|
% The original frequency response functions and the ones computed from the CoM responses are well matching in the frequency range of interested.
|
|
% Similar results are obtained for the other solid bodies, indicating that the solid body assumption is valid, and that a multi-body model can be used to represent the dynamics of the micro-station.
|
|
% This also validates the reduction of the number of degrees of freedom from 69 (23 accelerometers with each 3 acrshort:dof) to 36 (6 solid bodies with 6 acrshort:dof).
|
|
|
|
|
|
%% Comparaison of the original accelerometer response and reconstructed response from the solid body response
|
|
exc_names = {'$F_x$', '$F_y$', '$F_z$'};
|
|
DOFs = {'x', 'y', 'z', '\theta_x', '\theta_y', '\theta_z'};
|
|
|
|
solid_i = 6; % Considered solid body
|
|
exc_dir = 1; % Excited direction
|
|
|
|
accs_i = solids.(solid_names{solid_i}); % Accelerometers fixed to this solid body
|
|
|
|
figure;
|
|
tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');
|
|
|
|
for i = 1:length(accs_i)
|
|
acc_i = accs_i(i);
|
|
nexttile();
|
|
|
|
hold on;
|
|
for dir_i = 1:3
|
|
plot(freqs, abs(squeeze(frf(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'DisplayName', sprintf('$a_{%i,%s}$', acc_i, DOFs{dir_i}));
|
|
end
|
|
set(gca,'ColorOrderIndex',1)
|
|
for dir_i = 1:3
|
|
plot(freqs, abs(squeeze(frfs_A(3*(acc_i-1)+dir_i, exc_dir, :))), '--', 'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
|
|
if i > 2
|
|
xlabel('Frequency [Hz]');
|
|
else
|
|
set(gca, 'XTickLabel',[]);
|
|
end
|
|
|
|
if rem(i, 2) == 1
|
|
ylabel('Amplitude [$\frac{m/s^2}{N}$]');
|
|
else
|
|
set(gca, 'YTickLabel',[]);
|
|
end
|
|
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
|
|
xlim([0, 200]); ylim([1e-6, 3e-2]);
|
|
xticks([0:20:200]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
end
|