379 lines
15 KiB
Matlab
379 lines
15 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;
|
|
|
|
% Frequency Response Matrix
|
|
% <<ssec:modal_frf_matrix>>
|
|
% All the Frequency Response Functions measured are combined into one big array called the Frequency Response Matrix.
|
|
|
|
% The frequency response matrix is an $n \times p \times q$ matrix with:
|
|
% - $n$ the number of measurements: $23 \times 3$ (23 accelerometers measuring 3 directions each)
|
|
% - $p$ the number of excitation inputs: $3$
|
|
% - $q$ the number of frequency points $\omega_i$
|
|
|
|
% Thus, the FRF matrix is an $69 \times 3 \times 801$ matrix.
|
|
|
|
% For each frequency point $\omega_i$, a 2D matrix is obtained:
|
|
% \begin{equation}
|
|
% \text{FRF}(\omega_i) = \begin{bmatrix}
|
|
% \frac{D_{1_x}}{F_x}(\omega_i) & \frac{D_{1_x}}{F_y}(\omega_i) & \frac{D_{1_x}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1_y}}{F_x}(\omega_i) & \frac{D_{1_y}}{F_y}(\omega_i) & \frac{D_{1_y}}{F_z}(\omega_i) \\
|
|
% \frac{D_{1_z}}{F_x}(\omega_i) & \frac{D_{1_z}}{F_y}(\omega_i) & \frac{D_{1_z}}{F_z}(\omega_i) \\
|
|
% \frac{D_{2_x}}{F_x}(\omega_i) & \frac{D_{2_x}}{F_y}(\omega_i) & \frac{D_{2_x}}{F_z}(\omega_i) \\
|
|
% \vdots & \vdots & \vdots \\
|
|
% \frac{D_{23_z}}{F_x}(\omega_i) & \frac{D_{23_z}}{F_y}(\omega_i) & \frac{D_{23_z}}{F_z}(\omega_i) \\
|
|
% \end{bmatrix}
|
|
% \end{equation}
|
|
|
|
|
|
%% Load frequency response matrix
|
|
load('frf_matrix.mat', 'freqs', 'frf');
|
|
|
|
%% 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');
|
|
|
|
|
|
|
|
% #+name: fig:aligned_accelerometers
|
|
% #+caption: Aligned measurement of the motion of a solid body
|
|
% #+RESULTS:
|
|
% [[file:figs/aligned_accelerometers.png]]
|
|
|
|
% The motion of the rigid body of figure ref:fig:aligned_accelerometers is defined by its displacement $\delta p$ and rotation $\vec{\Omega}$ with respect to the reference frame $\{O\}$.
|
|
|
|
% The motions at points $1$ and $2$ are:
|
|
% \begin{align*}
|
|
% \delta p_1 &= \delta p + \Omega \times p_1 \\
|
|
% \delta p_2 &= \delta p + \Omega \times p_2
|
|
% \end{align*}
|
|
|
|
% Taking only the $x$ direction:
|
|
% \begin{align*}
|
|
% \delta p_{x1} &= \delta p_x + \Omega_y p_{z1} - \Omega_z p_{y1} \\
|
|
% \delta p_{x2} &= \delta p_x + \Omega_y p_{z2} - \Omega_z p_{y2}
|
|
% \end{align*}
|
|
|
|
% However, we have $p_{1y} = p_{2y}$ and $p_{1z} = p_{2z}$ because of the co-linearity of the two sensors in the $x$ direction, and thus we obtain
|
|
% \begin{equation}
|
|
% \delta p_{x1} = \delta p_{x2}
|
|
% \end{equation}
|
|
|
|
% #+begin_important
|
|
% Two sensors that are measuring the motion of a rigid body in the direction of the line linking the two sensors should measure the same quantity.
|
|
% #+end_important
|
|
|
|
% We can verify that the rigid body assumption is correct by comparing the measurement of the sensors.
|
|
|
|
% From the table ref:tab:position_accelerometers, we can guess which sensors will give the same results in the X and Y directions.
|
|
|
|
% Comparison of such measurements in the X direction is shown on figure ref:fig:modal_solid_body_comp_x_dir.
|
|
% Similar result is obtained for the Y direction.
|
|
|
|
|
|
meas_dir = 1; % X
|
|
exc_dir = 1; % X
|
|
|
|
% Pair of accelerometers aligned in the X direction
|
|
acc_i = [1 , 4 ;
|
|
2 , 3 ;
|
|
5 , 8 ;
|
|
6 , 7 ;
|
|
9 , 12;
|
|
10, 11;
|
|
14, 15;
|
|
18, 19;
|
|
21, 23];
|
|
|
|
%% Comparaison of measured frequency response function for in the X directions for accelerometers aligned along X
|
|
figure;
|
|
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
for i = 1:size(acc_i, 1)
|
|
nexttile();
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frf(meas_dir+3*(acc_i(i, 1)-1), exc_dir, :))), ...
|
|
'DisplayName', sprintf('%i', acc_i(i, 1)))
|
|
plot(freqs, abs(squeeze(frf(meas_dir+3*(acc_i(i, 2)-1), exc_dir, :))), ...
|
|
'DisplayName', sprintf('%i', acc_i(i, 2)))
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
|
|
if i > 6
|
|
xlabel('Frequency [Hz]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
else
|
|
set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
end
|
|
|
|
if rem(i, 3) == 1
|
|
ylabel('Amplitude');
|
|
else
|
|
set(gca, 'YTickLabel',[]);
|
|
end
|
|
|
|
xlim([0, 200]); ylim([1e-5, 2e-2]);
|
|
end
|
|
|
|
% TODO Verification of the principle of reciprocity :noexport:
|
|
% <<ssec:modal_reciprocity_principle>>
|
|
% Because we expect our system to follow the principle of reciprocity.
|
|
% That is to say the response $X_j$ at some degree of freedom $j$ due to a force $F_k$ applied on DOF $k$ should be the same as the response $X_k$ due to a force $F_j$:
|
|
% \[ H_{jk} = \frac{X_j}{F_k} = \frac{X_k}{F_j} = H_{kj} \]
|
|
|
|
% This comes from the fact that we expect to have symmetric mass, stiffness and damping matrices.
|
|
|
|
% In order to access the quality of the data and the validity of the measured FRF, we then check that the reciprocity between $H_{jk}$ and $H_{kj}$ is of an acceptable level.
|
|
|
|
% We can verify this reciprocity using 3 different pairs of response/force.
|
|
|
|
|
|
dir_names = {'X', 'Y', 'Z'};
|
|
|
|
figure;
|
|
for i = 1:3
|
|
subplot(3, 1, i)
|
|
a = mod(i, 3)+1;
|
|
b = mod(i-2, 3)+1;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frf(3*(11-1)+a, b, :))), 'DisplayName', sprintf('$\\frac{F_%s}{D_%s}$', dir_names{a}, dir_names{b}));
|
|
plot(freqs, abs(squeeze(frf(3*(11-1)+b, a, :))), 'DisplayName', sprintf('$\\frac{F_%s}{D_%s}$', dir_names{b}, dir_names{a}));
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
|
|
if i == 3
|
|
xlabel('Frequency [Hz]');
|
|
else
|
|
set(gca, 'XTickLabel',[]);
|
|
end
|
|
if i == 2
|
|
ylabel('Amplitude [$\frac{m/s^2}{N}$]');
|
|
end
|
|
xlim([0, 200]);
|
|
legend('location', 'northwest');
|
|
end
|
|
|
|
% From accelerometer DOFs to solid body DOFs - Matlab Implementation
|
|
% First, we initialize a new FRF matrix =FRFs_O= which is an $n \times p \times q$ with:
|
|
% - $n$ is the number of DOFs of the considered 6 solid-bodies: $6 \times 6 = 36$
|
|
% - $p$ is the number of excitation inputs: $3$
|
|
% - $q$ is the number of frequency points $\omega_i$
|
|
|
|
% #+begin_important
|
|
% For each frequency point $\omega_i$, the FRF matrix =FRFs_O= is a $n\times p$ matrix:
|
|
% \begin{equation}
|
|
% \text{FRF}_\text{O}(\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}
|
|
% where $D_i$ corresponds to the solid body number i.
|
|
% #+end_important
|
|
|
|
% Then, as we know the positions of the accelerometers on each solid body, and we have the response of those accelerometers, we can use the equations derived in the previous section to determine the response of each solid body expressed in the frame $\{O\}$.
|
|
|
|
|
|
FRFs_O = zeros(length(solid_names)*6, 3, 801);
|
|
|
|
for solid_i = 1:length(solid_names)
|
|
solids_i = solids.(solid_names{solid_i});
|
|
|
|
A = zeros(3*length(solids_i), 6);
|
|
for i = 1:length(solids_i)
|
|
acc_i = solids_i(i);
|
|
|
|
A(3*(i-1)+1:3*i, 1:3) = eye(3);
|
|
A(3*(i-1)+1:3*i, 4:6) = [ 0 acc_pos(acc_i, 3) -acc_pos(acc_i, 2) ;
|
|
-acc_pos(acc_i, 3) 0 acc_pos(acc_i, 1) ;
|
|
acc_pos(acc_i, 2) -acc_pos(acc_i, 1) 0];
|
|
end
|
|
|
|
for exc_dir = 1:3
|
|
FRFs_O((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(FRFs((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
|
|
end
|
|
end
|
|
|
|
% Center of Mass of each solid body
|
|
% From solidworks, we can export the position of the center of mass of each solid body considered.
|
|
% These are summarized in Table ref:tab:modal_com_solid_bodies
|
|
|
|
|
|
%% Extract the CoM of considered solid bodies
|
|
model_com = reshape(table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false)), [3, 6]);
|
|
|
|
% From accelerometer DOFs to solid body DOFs - Expressed at the CoM
|
|
% First, we initialize a new FRF matrix which is an $n \times p \times q$ with:
|
|
% - $n$ is the number of DOFs of the considered 6 solid-bodies: $6 \times 6 = 36$
|
|
% - $p$ is the number of excitation inputs: $3$
|
|
% - $q$ is the number of frequency points $\omega_i$
|
|
|
|
% #+begin_important
|
|
% For each frequency point $\omega_i$, the FRF matrix is a $n\times p$ matrix:
|
|
% \begin{equation}
|
|
% \text{FRF}_\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}
|
|
% where 1, 2, ..., 6 corresponds to the 6 solid bodies.
|
|
% #+end_important
|
|
|
|
% Then, as we know the positions of the accelerometers on each solid body, and we have the response of those accelerometers, we can use the equations derived in the previous section to determine the response of each solid body expressed in their center of mass.
|
|
|
|
|
|
%% 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');
|
|
|
|
% Verify that we find the original FRF from the FRF in the global coordinates
|
|
% We have computed the Frequency Response Functions Matrix representing the response of the 6 solid bodies in their 6 DOFs with respect to their center of mass.
|
|
|
|
% From the response of one body in its 6 DOFs, we should be able to compute the FRF of each of its accelerometer fixed to it during the measurement, supposing that this stage is a solid body.
|
|
|
|
% We can then compare the result with the original measurements.
|
|
% This will help us to determine if:
|
|
% - the previous inversion used is correct
|
|
% - the solid body assumption is correct in the frequency band of interest
|
|
|
|
% From the translation $\delta p$ and rotation $\delta \Omega$ of a solid body and the positions $p_i$ of the accelerometers attached to it, we can compute the response that would have been measured by the accelerometers using the following formula:
|
|
% \begin{align*}
|
|
% \delta p_1 &= \delta p + \delta\Omega p_1\\
|
|
% \delta p_2 &= \delta p + \delta\Omega p_2\\
|
|
% \delta p_3 &= \delta p + \delta\Omega p_3\\
|
|
% \delta p_4 &= \delta p + \delta\Omega p_4
|
|
% \end{align*}
|
|
|
|
% Thus, we can obtain the FRF matrix =FRFs_A= that gives the responses of the accelerometers to the forces applied by the hammer.
|
|
|
|
% It is implemented in matlab as follow:
|
|
|
|
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
|
|
|
|
|
|
|
|
% We then compare the original FRF measured for each accelerometer =FRFs= with the "recovered" FRF =FRFs_A= from the global FRF matrix in the common frame.
|
|
|
|
% The FRF for the 4 accelerometers on the Hexapod are compared on figure ref:fig:recovered_frf_comparison_hexa.
|
|
% All the FRF are matching very well in all the frequency range displayed.
|
|
|
|
% The FRF for accelerometers located on the translation stage are compared on figure ref:fig:recovered_frf_comparison_ty.
|
|
% The FRF are matching well until 100Hz.
|
|
|
|
|
|
%% 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', 'Compact', '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', 'log'); set(gca, 'YScale', 'log');
|
|
xlim([5, 200]); ylim([1e-6, 1e-1]);
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
end
|