326 lines
10 KiB
Mathematica
326 lines
10 KiB
Mathematica
|
%% Clear Workspace and Close figures
|
||
|
|
clear; close all; clc;
|
||
|
|
|
||
|
|
%% Intialize Laplace variable
|
||
|
|
s = zpk('s');
|
||
|
|
|
||
|
|
% Importation of measured FRF curves
|
||
|
|
% We load the measured FRF and Coherence matrices.
|
||
|
|
% We also load the geometric parameters of the station: solid bodies considered and the position of the accelerometers.
|
||
|
|
|
||
|
|
|
||
|
|
load('mat/frf_coh_matrices.mat', 'FRFs', 'COHs', 'freqs');
|
||
|
|
load('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');
|
||
|
|
|
||
|
|
% 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}_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 1, 2, ..., 6 corresponds to the 6 solid bodies.
|
||
|
|
% #+end_important
|
||
|
|
|
||
|
|
|
||
|
|
FRFs_O = zeros(length(solid_names)*6, 3, 801);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% 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\}$.
|
||
|
|
|
||
|
|
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
|
||
|
|
|
||
|
|
% Analysis of some FRF in the global coordinates
|
||
|
|
% First, we can compare the motions of the 6 solid bodies in one direction (figure [[fig:frf_all_bodies_one_direction]])
|
||
|
|
|
||
|
|
% We can also compare all the DOFs of one solid body (figure [[fig:frf_one_body_all_directions]]).
|
||
|
|
|
||
|
|
|
||
|
|
exc_names = {'$F_x$', '$F_y$', '$F_z$'};
|
||
|
|
DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
|
||
|
|
solids_i = 1:6;
|
||
|
|
dir_i = 1;
|
||
|
|
exc_dir = 1;
|
||
|
|
|
||
|
|
figure;
|
||
|
|
|
||
|
|
ax1 = subaxis(2, 1, 1);
|
||
|
|
hold on;
|
||
|
|
for solid_i = solids_i
|
||
|
|
plot(freqs, abs(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 'DisplayName', solid_names{solid_i});
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
ylabel('Amplitude');
|
||
|
|
legend('Location', 'northwest');
|
||
|
|
title(sprintf('FRF between %s and %s', exc_names{exc_dir}, DOFs{dir_i}));
|
||
|
|
|
||
|
|
ax2 = subaxis(2, 1, 2);
|
||
|
|
hold on;
|
||
|
|
for solid_i = solids_i
|
||
|
|
plot(freqs, mod(180+180/pi*phase(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 360)-180);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
ylim([-180, 180]); yticks(-180:90:180);
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
set(gca, 'xscale', 'log');
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 200]);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% #+NAME: fig:frf_all_bodies_one_direction
|
||
|
|
% #+CAPTION: FRFs of all the 6 solid bodies in one direction
|
||
|
|
% [[file:figs/frf_all_bodies_one_direction.png]]
|
||
|
|
|
||
|
|
|
||
|
|
DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
|
||
|
|
solid_i = 3;
|
||
|
|
dirs_i = 1:6;
|
||
|
|
exc_dir = 1;
|
||
|
|
|
||
|
|
figure;
|
||
|
|
|
||
|
|
ax1 = subplot(2, 1, 1);
|
||
|
|
hold on;
|
||
|
|
for dir_i = dirs_i
|
||
|
|
plot(freqs, abs(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 'DisplayName', DOFs{dir_i});
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
ylabel('Amplitude');
|
||
|
|
legend('Location', 'northwest');
|
||
|
|
title(sprintf('Motion of %s due to %s', solid_names{solid_i}, exc_names{exc_dir}));
|
||
|
|
|
||
|
|
ax2 = subplot(2, 1, 2);
|
||
|
|
hold on;
|
||
|
|
for dir_i = dirs_i
|
||
|
|
plot(freqs, mod(180+180/pi*phase(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 360)-180);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
ylim([-180, 180]); yticks(-180:90:180);
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
set(gca, 'xscale', 'log');
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 200]);
|
||
|
|
|
||
|
|
% Comparison of the relative motion of solid bodies
|
||
|
|
% Now that the motion of all the solid bodies are expressed in the same frame, we should be able to *compare them*.
|
||
|
|
% This can be used to determine what joints direction between two solid bodies is stiff enough that we can fix this DoF.
|
||
|
|
% This could help reduce the order of the model and simplify the extraction of the model parameters from the measurements.
|
||
|
|
|
||
|
|
% We decide to plot the "normalized relative motion" between solid bodies $i$ and $j$:
|
||
|
|
% \[ 0 < \Delta_{ij, x} = \frac{\left| D_{i,x} - D_{j,x} \right|}{|D_{i,x}| + |D_{j,x}|} < 1 \]
|
||
|
|
|
||
|
|
% Then, if $\Delta_{ij,x} \ll 0$ in the frequency band of interest, we have that $D_{ix} \approx D_{jx}$ and we can neglect that DOF between the two solid bodies $i$ and $j$.
|
||
|
|
|
||
|
|
% This normalized relative motion is shown on figure [[fig:relative_motion_comparison]] for all the directions and for all the adjacent pair of solid bodies.
|
||
|
|
|
||
|
|
|
||
|
|
DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
|
||
|
|
|
||
|
|
dirs_i = 1:6;
|
||
|
|
exc_dir = 1;
|
||
|
|
|
||
|
|
figure;
|
||
|
|
|
||
|
|
for i = 2:6
|
||
|
|
subaxis(3, 2, i);
|
||
|
|
hold on;
|
||
|
|
for dir_i = dirs_i
|
||
|
|
H = (squeeze(FRFs_O((i-1)*6+dir_i, exc_dir, :))-squeeze(FRFs_O((i-2)*6+dir_i, exc_dir, :)))./(abs(squeeze(FRFs_O((i-1)*6+dir_i, exc_dir, :)))+abs(squeeze(FRFs_O((i-2)*6+dir_i, exc_dir, :))));
|
||
|
|
plot(freqs, abs(H));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlim([1, 200]); ylim([0, 1]);
|
||
|
|
% xlabel('Frequency [Hz]'); ylabel('Relative Motion');
|
||
|
|
title(sprintf('Normalized motion %s - %s', solid_names{i-1}, solid_names{i}));
|
||
|
|
if i > 4
|
||
|
|
xlabel('Frequency [Hz]');
|
||
|
|
else
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
for i = 1:length(dirs_i)
|
||
|
|
legend_names{i} = DOFs{dirs_i(i)};
|
||
|
|
end
|
||
|
|
lgd = legend(legend_names);
|
||
|
|
|
||
|
|
hL = subplot(3, 2, 1);
|
||
|
|
poshL = get(hL,'position');
|
||
|
|
|
||
|
|
set(lgd,'position', poshL);
|
||
|
|
axis(hL, 'off');
|
||
|
|
|
||
|
|
% Verify that we find the original FRF from the FRF in the global coordinates
|
||
|
|
% We have computed the Frequency Response Functions Matrix =FRFs_O= representing the response of the 6 solid bodies in their 6 DOFs.
|
||
|
|
|
||
|
|
% 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.
|
||
|
|
|
||
|
|
% 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
|
||
|
|
|
||
|
|
|
||
|
|
FRF_recovered = zeros(size(FRFs));
|
||
|
|
|
||
|
|
% For each excitation direction
|
||
|
|
for exc_dir = 1:3
|
||
|
|
% For each solid
|
||
|
|
for solid_i = 1:length(solid_names)
|
||
|
|
v0 = squeeze(FRFs_O((solid_i-1)*6+1:(solid_i-1)*6+3, exc_dir, :));
|
||
|
|
W0 = squeeze(FRFs_O((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, :)';
|
||
|
|
posX = [0 pos(3) -pos(2); -pos(3) 0 pos(1) ; pos(2) -pos(1) 0];
|
||
|
|
|
||
|
|
FRF_recovered(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 with the recovered FRF from the global FRF matrix in the common frame.
|
||
|
|
|
||
|
|
% The FRF for the 4 accelerometers on the Hexapod are compared on figure [[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 [[fig:recovered_frf_comparison_ty]].
|
||
|
|
% The FRF are matching well until 100Hz.
|
||
|
|
|
||
|
|
|
||
|
|
exc_names = {'$F_x$', '$F_y$', '$F_z$'};
|
||
|
|
DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
|
||
|
|
|
||
|
|
solid_i = 6;
|
||
|
|
exc_dir = 1;
|
||
|
|
|
||
|
|
accs_i = solids.(solid_names{solid_i});
|
||
|
|
|
||
|
|
figure;
|
||
|
|
|
||
|
|
for i = 1:length(accs_i)
|
||
|
|
acc_i = accs_i(i);
|
||
|
|
|
||
|
|
subaxis(2, 2, i);
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
for dir_i = 1:3
|
||
|
|
plot(freqs, abs(squeeze(FRFs(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'DisplayName', DOFs{dir_i});
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1)
|
||
|
|
for dir_i = 1:3
|
||
|
|
plot(freqs, abs(squeeze(FRF_recovered(3*(acc_i-1)+dir_i, exc_dir, :))), '--', 'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
if i > 2
|
||
|
|
xlabel('Frequency [Hz]');
|
||
|
|
else
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
end
|
||
|
|
|
||
|
|
if rem(i, 2) == 1
|
||
|
|
ylabel('Amplitude');
|
||
|
|
end
|
||
|
|
|
||
|
|
xlim([1, 200]);
|
||
|
|
title(sprintf('Accelerometer %i', accs_i(i)));
|
||
|
|
legend('location', 'northwest');
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% #+NAME: fig:recovered_frf_comparison_hexa
|
||
|
|
% #+CAPTION: Comparison of the original FRF with the recovered ones - Hexapod
|
||
|
|
% [[file:figs/recovered_frf_comparison_hexa.png]]
|
||
|
|
|
||
|
|
|
||
|
|
exc_names = {'$F_x$', '$F_y$', '$F_z$'};
|
||
|
|
DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
|
||
|
|
|
||
|
|
solid_i = 3;
|
||
|
|
exc_dir = 1;
|
||
|
|
|
||
|
|
accs_i = solids.(solid_names{solid_i});
|
||
|
|
|
||
|
|
figure;
|
||
|
|
|
||
|
|
for i = 1:length(accs_i)
|
||
|
|
acc_i = accs_i(i);
|
||
|
|
|
||
|
|
subaxis(2, 2, i);
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
for dir_i = 1:3
|
||
|
|
plot(freqs, abs(squeeze(FRFs(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'DisplayName', DOFs{dir_i});
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1)
|
||
|
|
for dir_i = 1:3
|
||
|
|
plot(freqs, abs(squeeze(FRF_recovered(3*(acc_i-1)+dir_i, exc_dir, :))), '--', 'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
if i > 2
|
||
|
|
xlabel('Frequency [Hz]');
|
||
|
|
else
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
end
|
||
|
|
|
||
|
|
if rem(i, 2) == 1
|
||
|
|
ylabel('Amplitude');
|
||
|
|
end
|
||
|
|
|
||
|
|
xlim([1, 200]);
|
||
|
|
title(sprintf('Accelerometer %i', accs_i(i)));
|
||
|
|
legend('location', 'northwest');
|
||
|
|
end
|
||
|
|
|
||
|
|
% Saving of the FRF expressed in the global coordinates
|
||
|
|
|
||
|
|
save('mat/frf_o.mat', 'FRFs_O');
|