351 lines
13 KiB
Matlab
351 lines
13 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
|
|
addpath('./src/'); % Path for functions
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
% Effect of the Encoder on the measured dynamics
|
|
% <<ssec:test_struts_effect_encoder>>
|
|
|
|
|
|
%% Parameters for Frequency Analysis
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
|
win = hanning(Nfft); % Hanning window
|
|
Noverlap = floor(Nfft/2); % Overlap between frequency analysis
|
|
|
|
%% Measure FRF for Strut 1 - No encoder
|
|
% Load Data
|
|
leg_sweep = load('frf_data_leg_1_sweep.mat', 'u', 'Vs', 'de', 'da');
|
|
leg_noise_hf = load('frf_data_leg_1_noise_hf.mat', 'u', 'Vs', 'de', 'da');
|
|
|
|
% We get the frequency vector that will be the same for all the frequency domain analysis.
|
|
[~, f] = tfestimate(leg_sweep.u, leg_sweep.de, win, Noverlap, Nfft, 1/Ts);
|
|
i_lf = f <= 350; % Indices used for the low frequency
|
|
i_hf = f > 350; % Indices used for the high frequency
|
|
|
|
% Compute FRF function from u to da (interferometer)
|
|
[frf_sweep, ~] = tfestimate(leg_sweep.u, leg_sweep.da, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.u, leg_noise_hf.da, win, Noverlap, Nfft, 1/Ts);
|
|
int_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF
|
|
|
|
% Compute FRF function from u to Vs (force sensor)
|
|
[frf_sweep, ~] = tfestimate(leg_sweep.u, leg_sweep.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.u, leg_noise_hf.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
iff_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF
|
|
|
|
%% Measure FRF for Strut 1 - With encoder
|
|
% Load Data
|
|
leg_enc_sweep = load('frf_data_leg_coder_1_noise.mat', 'u', 'Vs', 'de', 'da');
|
|
leg_enc_noise_hf = load('frf_data_leg_coder_1_noise_hf.mat', 'u', 'Vs', 'de', 'da');
|
|
|
|
% Compute FRF function from u to da (interferometer)
|
|
[frf_sweep, ~] = tfestimate(leg_enc_sweep.u, leg_enc_sweep.da, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.u, leg_enc_noise_hf.da, win, Noverlap, Nfft, 1/Ts);
|
|
int_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF
|
|
|
|
% Compute FRF function from u to Vs (force sensor)
|
|
[frf_sweep, ~] = tfestimate(leg_enc_sweep.u, leg_enc_sweep.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.u, leg_enc_noise_hf.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
iff_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF
|
|
|
|
% Compute FRF function from u to de (encoder)
|
|
[frf_sweep, ~] = tfestimate(leg_enc_sweep.u, leg_enc_sweep.de, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.u, leg_enc_noise_hf.de, win, Noverlap, Nfft, 1/Ts);
|
|
enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)]; % Combine the FRF
|
|
|
|
|
|
|
|
% System identification is performed without the encoder fixed to the strut (Figure ref:fig:test_struts_bench_leg_front) and with one encoder fixed to the strut (Figure ref:fig:test_struts_bench_leg_coder).
|
|
|
|
% #+name: fig:test_struts_bench_leg_with_without_enc
|
|
% #+caption: Struts fixed to the test bench with clamped flexible joints. The coder can be fixed to the struts (\subref{fig:test_struts_bench_leg_coder}) or removed (\subref{fig:test_struts_bench_leg_front})
|
|
% #+attr_latex: :options [htbp]
|
|
% #+begin_figure
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_struts_bench_leg_coder}Strut with encoder}
|
|
% #+attr_latex: :options {0.5\textwidth}
|
|
% #+begin_subfigure
|
|
% #+attr_latex: :height 6cm
|
|
% [[file:figs/test_struts_bench_leg_coder.jpg]]
|
|
% #+end_subfigure
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_struts_bench_leg_front}Strut without encoder}
|
|
% #+attr_latex: :options {0.5\textwidth}
|
|
% #+begin_subfigure
|
|
% #+attr_latex: :height 6cm
|
|
% [[file:figs/test_struts_bench_leg_front.jpg]]
|
|
% #+end_subfigure
|
|
% #+end_figure
|
|
|
|
% The obtained frequency response functions are compared in Figure ref:fig:test_struts_effect_encoder.
|
|
% It is found that the encoder has very little effect on the transfer function from excitation voltage $u$ to the axial motion of the strut $d_a$ as measured by the interferometer (Figure ref:fig:test_struts_effect_encoder_int).
|
|
% This means that the axial motion of the strut is unaffected by the presence of the encoder.
|
|
% Similarly, it has very little effect on the transfer function from $u$ to the sensor stack voltage $V_s$ (Figure ref:fig:test_struts_effect_encoder_iff).
|
|
% This means that the integral force feedback control strategy should be as effective whether the encoders are fixed to the struts or not.
|
|
|
|
|
|
%% Plot the FRF from u to da with and without the encoder
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(int_with_enc_frf), '-', 'DisplayName', 'With encoder');
|
|
plot(f, abs(int_frf), '-', 'DisplayName', 'Without encoder');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_a/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-7, 1e-3]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(int_with_enc_frf), '-');
|
|
plot(f, 180/pi*angle(int_frf), '-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
xticks([1e1, 1e2, 1e3]);
|
|
|
|
%% Compare the IFF plant with and without the encoders
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(iff_with_enc_frf), 'DisplayName', 'With Encoder');
|
|
plot(f, abs(iff_frf), 'DisplayName', 'Without Encoder');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
ylim([1e-2, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(iff_with_enc_frf));
|
|
plot(f, 180/pi*angle(iff_frf));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
xticks([1e1, 1e2, 1e3]);
|
|
|
|
% Comparison of the encoder and interferometer
|
|
% <<ssec:test_struts_comp_enc_int>>
|
|
|
|
% The dynamics as measured by the encoder (i.e. $d_e/u$) and by the interferometers (i.e. $d_a/u$) are compared in Figure ref:fig:test_struts_comp_enc_int.
|
|
% The dynamics from the excitation voltage $u$ to the measured displacement by the encoder $d_e$ presents a behavior that is much more complex than the dynamics to the displacement as measured by the interferometer (comparison made in Figure ref:fig:test_struts_comp_enc_int).
|
|
% Three additional resonance frequencies can be observed at 197Hz, 290Hz and 376Hz.
|
|
% These resonance frequencies are matching the frequencies of the flexible modes that were studied in Section ref:sec:test_struts_flexible_modes.
|
|
|
|
% The good news is that these resonances are not impacting the axial motion of the strut (which is what is important for the hexapod positioning).
|
|
% However, these resonances are making the use of encoder fixed to the strut difficult from a control perspective.
|
|
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(enc_frf), 'DisplayName', '$d_e/u$');
|
|
plot(f, abs(int_with_enc_frf), 'DisplayName', '$d_a/u$');
|
|
text(85, 4e-4, {'93Hz'}, 'VerticalAlignment','middle','HorizontalAlignment','right')
|
|
text(200, 1.3e-4,{'197Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
text(300, 4e-6, {'290Hz'},'VerticalAlignment','bottom','HorizontalAlignment','left')
|
|
text(400, 4e-7,{'376Hz'},'VerticalAlignment','top','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
ylim([1e-7, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(enc_frf));
|
|
plot(f, 180/pi*angle(int_with_enc_frf));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
xticks([1e1, 1e2, 1e3]);
|
|
|
|
% Comparison of all the Struts
|
|
% <<ssec:test_struts_comp_all_struts>>
|
|
|
|
|
|
%% Numbers of the measured legs
|
|
strut_nums = [1 2 3 4 5];
|
|
|
|
%% Load the measurement data
|
|
% First identification (low frequency noise)
|
|
leg_noise = {};
|
|
for i = 1:length(strut_nums)
|
|
leg_noise(i) = {load(sprintf('frf_data_leg_coder_%i_noise.mat', strut_nums(i)), 'u', 'Vs', 'de', 'da')};
|
|
end
|
|
|
|
% Second identification (high frequency noise)
|
|
leg_noise_hf = {};
|
|
for i = 1:length(strut_nums)
|
|
leg_noise_hf(i) = {load(sprintf('frf_data_leg_coder_%i_noise_hf.mat', strut_nums(i)), 'u', 'Vs', 'de', 'da')};
|
|
end
|
|
|
|
%% Compute FRF - From u to de (encoder)
|
|
enc_frf = zeros(length(f), length(strut_nums));
|
|
|
|
for i = 1:length(strut_nums)
|
|
[frf_lf, ~] = tfestimate(leg_noise{i}.u, detrend(leg_noise{i}.de, 0), win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(leg_noise_hf{i}.u, detrend(leg_noise_hf{i}.de, 0), win, Noverlap, Nfft, 1/Ts);
|
|
enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
%% Compute FRF - From u to da (interferometer)
|
|
int_frf = zeros(length(f), length(strut_nums));
|
|
for i = 1:length(strut_nums)
|
|
[frf_lf, ~] = tfestimate(leg_noise{i}.u, leg_noise{i}.da, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(leg_noise_hf{i}.u, leg_noise_hf{i}.da, win, Noverlap, Nfft, 1/Ts);
|
|
int_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
%% Compute FRF - From u to Vs (force sensor)
|
|
iff_frf = zeros(length(f), length(strut_nums));
|
|
for i = 1:length(strut_nums)
|
|
[frf_lf, ~] = tfestimate(leg_noise{i}.u, leg_noise{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(leg_noise_hf{i}.u, leg_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
|
|
|
|
% Then, the dynamics of all the mounted struts (only 5 at the time of the experiment) were all measured using the same test bench.
|
|
% The obtained dynamics from $u$ to $d_a$ are compared in Figure ref:fig:test_struts_comp_interf_plants while is dynamics from $u$ to $V_s$ are compared in Figure ref:fig:test_struts_comp_iff_plants.
|
|
% Very good match can be observed between all the struts.
|
|
|
|
|
|
%% Plot the FRF from u to de (interferometer)
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(strut_nums)
|
|
plot(f, abs(int_frf(:, i)), ...
|
|
'DisplayName', sprintf('Leg %i', strut_nums(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_a/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
ylim([1e-8, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(strut_nums)
|
|
plot(f, 180/pi*angle(int_frf(:, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
xticks([1e1, 1e2, 1e3]);
|
|
|
|
%% Plot the FRF from u to Vs
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(strut_nums)
|
|
plot(f, abs(iff_frf(:, i)), ...
|
|
'DisplayName', sprintf('Leg %i', strut_nums(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-2, 1e2]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(strut_nums)
|
|
plot(f, 180/pi*angle(iff_frf(:, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
xticks([1e1, 1e2, 1e3]);
|
|
|
|
%% Bode plot of the FRF from u to de
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(strut_nums)
|
|
plot(f, abs(enc_frf(:, i)), ...
|
|
'DisplayName', sprintf('Leg %i', strut_nums(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
ylim([1e-8, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(strut_nums)
|
|
plot(f, 180/pi*angle(enc_frf(:, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
xticks([1e1, 1e2, 1e3]);
|
|
|
|
%% Save the estimated FRF for further analysis
|
|
save('./mat/meas_struts_frf.mat', 'f', 'enc_frf', 'int_frf', 'iff_frf', 'strut_nums');
|