2025-02-04 14:10:54 +01:00
|
|
|
% Matlab Init :noexport:ignore:
|
|
|
|
|
|
|
|
|
|
%% test_id31_2_open_loop_plant.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
|
|
|
|
|
|
|
|
|
|
%% Data directory
|
|
|
|
|
data_dir = './mat/';
|
|
|
|
|
|
|
|
|
|
% Simulink Model name
|
|
|
|
|
mdl = 'nass_model_id31';
|
|
|
|
|
|
|
|
|
|
%% Colors for the figures
|
|
|
|
|
colors = colororder;
|
|
|
|
|
|
|
|
|
|
%% Frequency Vector
|
|
|
|
|
freqs = logspace(log10(1), log10(2e3), 1000);
|
|
|
|
|
|
|
|
|
|
%% Sampling Time
|
|
|
|
|
Ts = 1e-4;
|
|
|
|
|
|
|
|
|
|
%% Specifications for Experiments
|
|
|
|
|
specs_dz_peak = 50; % [nm]
|
|
|
|
|
specs_dy_peak = 100; % [nm]
|
|
|
|
|
specs_ry_peak = 0.85; % [urad]
|
|
|
|
|
specs_dz_rms = 15; % [nm RMS]
|
|
|
|
|
specs_dy_rms = 30; % [nm RMS]
|
|
|
|
|
specs_ry_rms = 0.25; % [urad RMS]
|
|
|
|
|
|
|
|
|
|
% Open-Loop Plant Identification
|
|
|
|
|
% <<ssec:test_id31_open_loop_plant_first_id>>
|
|
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% The dynamics of the plant is first identified for a fixed spindle angle (at $0\,\text{deg}$) and without any payload.
|
|
|
|
|
% The model dynamics is also identified under the same conditions.
|
2025-02-04 14:10:54 +01:00
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% A comparison between the model and the measured dynamics is presented in Figure ref:fig:test_id31_first_id.
|
2025-02-04 14:10:54 +01:00
|
|
|
% A good match can be observed for the diagonal dynamics (except the high frequency modes which are not modeled).
|
2025-02-06 17:05:29 +01:00
|
|
|
% However, the coupling of the transfer function from command signals $\bm{u}$ to the estimated strut motion from the external metrology $\bm{\epsilon\mathcal{L}}$ is larger than expected (Figure ref:fig:test_id31_first_id_int).
|
2025-02-04 14:10:54 +01:00
|
|
|
|
|
|
|
|
% The experimental time delay estimated from the FRF (Figure ref:fig:test_id31_first_id_int) is larger than expected.
|
2025-02-06 17:05:29 +01:00
|
|
|
% After investigation, it was found that the additional delay was due to a digital processing unit[fn:test_id31_3] that was used to get the interferometers' signals in the Speedgoat.
|
2025-02-04 14:10:54 +01:00
|
|
|
% This issue was later solved.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
%% Identify the plant dynamics using the Simscape model
|
|
|
|
|
|
|
|
|
|
% Initialize each Simscape model elements
|
|
|
|
|
initializeGround();
|
|
|
|
|
initializeGranite();
|
|
|
|
|
initializeTy();
|
|
|
|
|
initializeRy();
|
|
|
|
|
initializeRz();
|
|
|
|
|
initializeMicroHexapod();
|
|
|
|
|
initializeNanoHexapod('flex_bot_type', '2dof', ...
|
|
|
|
|
'flex_top_type', '3dof', ...
|
|
|
|
|
'motion_sensor_type', 'plates', ...
|
|
|
|
|
'actuator_type', '2dof');
|
|
|
|
|
initializeSample('type', '0');
|
|
|
|
|
|
|
|
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
|
|
|
initializeDisturbances('enable', false);
|
|
|
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
|
initializeController('type', 'open-loop');
|
|
|
|
|
initializeReferences();
|
|
|
|
|
|
|
|
|
|
% Input/Output definition
|
|
|
|
|
clear io; io_i = 1;
|
|
|
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [V]
|
|
|
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vs'); io_i = io_i + 1; % Force Sensors voltages [V]
|
|
|
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors [m]
|
|
|
|
|
|
|
|
|
|
% With no payload
|
|
|
|
|
Gm = exp(-1e-4*s)*linearize(mdl, io);
|
|
|
|
|
Gm.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
%% Identify the plant from experimental data
|
|
|
|
|
|
|
|
|
|
% Load identification data
|
|
|
|
|
data = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat');
|
|
|
|
|
|
|
|
|
|
% Frequency analysis parameters
|
|
|
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
|
|
|
Nfft = floor(2.0/Ts);
|
|
|
|
|
win = hanning(Nfft);
|
|
|
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
|
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
|
|
|
|
|
G_iff = zeros(length(f), 6, 6); % Force sensor outputs
|
|
|
|
|
G_int = zeros(length(f), 6, 6); % Estimated strut errors
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
Vs = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
dL = [data.(sprintf("uL%i", i_strut)).dL1 ; data.(sprintf("uL%i", i_strut)).dL2 ; data.(sprintf("uL%i", i_strut)).dL3 ; data.(sprintf("uL%i", i_strut)).dL4 ; data.(sprintf("uL%i", i_strut)).dL5 ; data.(sprintf("uL%i", i_strut)).dL6]';
|
|
|
|
|
|
|
|
|
|
G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, Vs, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
ax1 = nexttile([2,1]);
|
|
|
|
|
hold on;
|
|
|
|
|
for i = 1:5
|
|
|
|
|
for j = i+1:6
|
|
|
|
|
plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
|
|
|
|
|
'DisplayName', '$-\epsilon\mathcal{L}_i/u_i$ meas');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
|
|
|
|
'DisplayName', '$-\epsilon\mathcal{L}_i/u_i$ model');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
|
|
|
|
'DisplayName', '$-\epsilon\mathcal{L}_i/u_j$ meas');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
|
|
|
'DisplayName', '$-\epsilon\mathcal{L}_i/u_j$ model');
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
|
|
|
ylim([2e-9, 2e-4]);
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
ax2 = nexttile;
|
|
|
|
|
hold on;
|
|
|
|
|
for i = 1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
|
|
|
|
|
end
|
|
|
|
|
for i = 1:6
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
|
|
|
|
|
end
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
|
|
|
hold off;
|
|
|
|
|
yticks(-360:90:360);
|
|
|
|
|
ylim([-90, 180])
|
|
|
|
|
|
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
|
xlim([1, 1e3]);
|
|
|
|
|
|
|
|
|
|
%% Comparison between the measured dynamics and the model dynamics - Force Sensors
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
ax1 = nexttile([2,1]);
|
|
|
|
|
hold on;
|
|
|
|
|
for i = 1:5
|
|
|
|
|
for j = i+1:6
|
|
|
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_iff(:,1, 1)), 'color', [colors(1,:)], ...
|
|
|
|
|
'DisplayName', '$V_{s,i}/u_i$ meas');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
|
|
|
|
'DisplayName', '$V_{s,i}/u_i$ model');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:)], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
|
|
|
|
'DisplayName', '$V_{s,i}/u_j$ meas');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
|
|
|
'DisplayName', '$V_{s,i}/u_j$ model');
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
|
|
|
ylim([5e-5, 4e1]);
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
ax2 = nexttile;
|
|
|
|
|
hold on;
|
|
|
|
|
for i = 1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [colors(1,:)]);
|
|
|
|
|
end
|
|
|
|
|
for i = 1:6
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
|
|
|
|
|
end
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
|
|
|
hold off;
|
|
|
|
|
yticks(-360:90:360);
|
|
|
|
|
ylim([-90, 180])
|
|
|
|
|
|
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
|
xlim([1, 1e3]);
|
|
|
|
|
|
|
|
|
|
% Better Angular Alignment
|
|
|
|
|
% <<ssec:test_id31_open_loop_plant_rz_alignment>>
|
|
|
|
|
|
|
|
|
|
% One possible explanation of the increased coupling observed in Figure ref:fig:test_id31_first_id_int is the poor alignment between the external metrology axes (i.e. the interferometer supports) and the nano-hexapod axes.
|
|
|
|
|
% To estimate this alignment, a decentralized low-bandwidth feedback controller based on the nano-hexapod encoders was implemented.
|
|
|
|
|
% This allowed to perform two straight movements of the nano-hexapod along its $x$ and $y$ axes.
|
2025-02-06 17:05:29 +01:00
|
|
|
% During these two movements, external metrology measurements were recorded and the results are shown in Figure ref:fig:test_id31_Rz_align_error_and_correct.
|
|
|
|
|
% It was found that there was a misalignment of 2.7 degrees (rotation along the vertical axis) between the interferometer axes and nano-hexapod axes.
|
2025-02-04 14:10:54 +01:00
|
|
|
% This was corrected by adding an offset to the spindle angle.
|
|
|
|
|
% After alignment, the same movement was performed using the nano-hexapod while recording the signal of the external metrology.
|
|
|
|
|
% Results shown in Figure ref:fig:test_id31_Rz_align_correct are indeed indicating much better alignment.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
%% Load Data
|
|
|
|
|
data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2);
|
|
|
|
|
data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3);
|
|
|
|
|
data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1);
|
|
|
|
|
data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2);
|
|
|
|
|
|
|
|
|
|
% Estimation of Rz misalignment
|
|
|
|
|
p1 = polyfit(data_1_dx.Dx_int_filtered, data_1_dx.Dy_int_filtered, 1);
|
|
|
|
|
p2 = polyfit(data_1_dy.Dx_int_filtered, data_1_dy.Dy_int_filtered, 1);
|
|
|
|
|
|
|
|
|
|
Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~3 degrees
|
|
|
|
|
|
|
|
|
|
%% Estimation of the Rz misalignment
|
|
|
|
|
figure;
|
|
|
|
|
hold on;
|
|
|
|
|
plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'Measurement')
|
|
|
|
|
plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off')
|
|
|
|
|
plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$\\epsilon_{R_z} = %.1f$ deg', Rz_error*180/pi))
|
|
|
|
|
plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off')
|
|
|
|
|
hold off;
|
|
|
|
|
xlabel('Interf $D_x$ [$\mu$m]');
|
|
|
|
|
ylabel('Interf $D_y$ [$\mu$m]');
|
|
|
|
|
axis equal
|
|
|
|
|
xlim([-10, 10]); ylim([-10, 10]);
|
|
|
|
|
xticks([-10:5:10]); yticks([-10:5:10]);
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
% Estimation of Rz misalignment after correcting the Rz angle
|
|
|
|
|
p1 = polyfit(data_2_dx.Dx_int_filtered, data_2_dx.Dy_int_filtered, 1);
|
|
|
|
|
p2 = polyfit(data_2_dy.Dx_int_filtered, data_2_dy.Dy_int_filtered, 1);
|
|
|
|
|
|
|
|
|
|
Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~0.2 degrees
|
|
|
|
|
|
|
|
|
|
%% Estimation of the Rz misalignment after correcting the Rz offset
|
|
|
|
|
figure;
|
|
|
|
|
hold on;
|
|
|
|
|
plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(5,:), 'DisplayName', 'Measurement')
|
|
|
|
|
plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(5,:), 'HandleVisibility', 'off')
|
|
|
|
|
plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$\\epsilon_{R_z} = %.1f$ deg', Rz_error*180/pi))
|
|
|
|
|
plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off')
|
|
|
|
|
hold off;
|
|
|
|
|
xlabel('Interf $D_x$ [$\mu$m]');
|
|
|
|
|
ylabel('Interf $D_y$ [$\mu$m]');
|
|
|
|
|
axis equal
|
|
|
|
|
xlim([-10, 10]); ylim([-10, 10]);
|
|
|
|
|
xticks([-10:5:10]); yticks([-10:5:10]);
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% #+name: fig:test_id31_Rz_align_error_and_correct
|
2025-02-04 14:10:54 +01:00
|
|
|
% #+caption: Measurement of the Nano-Hexapod axes in the frame of the external metrology. Before alignment (\subref{fig:test_id31_Rz_align_error}) and after alignment (\subref{fig:test_id31_Rz_align_correct}).
|
|
|
|
|
% #+attr_latex: :options [htbp]
|
|
|
|
|
% #+begin_figure
|
|
|
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_error}Before alignment}
|
|
|
|
|
% #+attr_latex: :options {0.49\textwidth}
|
|
|
|
|
% #+begin_subfigure
|
|
|
|
|
% #+attr_latex: :scale 1
|
|
|
|
|
% [[file:figs/test_id31_Rz_align_error.png]]
|
|
|
|
|
% #+end_subfigure
|
|
|
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_correct}After alignment}
|
|
|
|
|
% #+attr_latex: :options {0.49\textwidth}
|
|
|
|
|
% #+begin_subfigure
|
|
|
|
|
% #+attr_latex: :scale 1
|
|
|
|
|
% [[file:figs/test_id31_Rz_align_correct.png]]
|
|
|
|
|
% #+end_subfigure
|
|
|
|
|
% #+end_figure
|
|
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% The dynamics of the plant was identified again after fine alignment and compared with the model dynamics in Figure ref:fig:test_id31_first_id_int_better_rz_align.
|
|
|
|
|
% Compared to the initial identification shown in Figure ref:fig:test_id31_first_id_int, the obtained coupling was decreased and was close to the coupling obtained with the multi-body model.
|
|
|
|
|
% At low frequency (below $10\,\text{Hz}$), all off-diagonal elements have an amplitude $\approx 100$ times lower than the diagonal elements, indicating that a low bandwidth feedback controller can be implemented in a decentralized manner (i.e. $6$ SISO controllers).
|
|
|
|
|
% Between $650\,\text{Hz}$ and $1000\,\text{Hz}$, several modes can be observed, which are due to flexible modes of the top platform and the modes of the two spheres adjustment mechanism.
|
|
|
|
|
% The flexible modes of the top platform can be passively damped, whereas the modes of the two reference spheres should not be present in the final application.
|
2025-02-04 14:10:54 +01:00
|
|
|
|
|
|
|
|
|
|
|
|
|
%% Identification of the plant after Rz alignment
|
|
|
|
|
data_align = load('2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat');
|
|
|
|
|
|
|
|
|
|
G_int_align = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
nexttile();
|
|
|
|
|
hold on;
|
|
|
|
|
for i = 1:5
|
|
|
|
|
for j = i+1:6
|
|
|
|
|
plot(f, abs(G_int_align(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(1,:)], ...
|
|
|
|
|
'DisplayName', '$\epsilon\mathcal{L}_i/u_i$ meas');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
|
|
|
|
'DisplayName', '$\epsilon\mathcal{L}_i/u_i$ model');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int_align(:,i, i)), 'color', [colors(1,:)], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
|
|
|
|
'DisplayName', '$\epsilon\mathcal{L}_i/u_j$ meas');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
|
|
|
'DisplayName', '$\epsilon\mathcal{L}_i/u_j$ model');
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
|
|
|
xlim([1, 1e3]); ylim([2e-9, 2e-4]);
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
% Effect of Payload Mass
|
|
|
|
|
% <<ssec:test_id31_open_loop_plant_mass>>
|
|
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% To determine how the system dynamics changes with the payload, open-loop identification was performed for four payload conditions shown in Figure ref:fig:test_id31_picture_masses.
|
|
|
|
|
% The obtained direct terms are compared with the model dynamics in Figure ref:fig:test_id31_comp_simscape_diag_masses.
|
|
|
|
|
% It was found that the model well predicts the measured dynamics under all payload conditions.
|
|
|
|
|
% Therefore, the model can be used for model-based control is necessary.
|
2025-02-04 14:10:54 +01:00
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% It is interesting to note that the anti-resonances in the force sensor plant now appear as minimum-phase, as the model predicts (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses).
|
2025-02-04 14:10:54 +01:00
|
|
|
|
|
|
|
|
% #+name: fig:test_id31_picture_masses
|
|
|
|
|
% #+caption: The four tested payload conditions. (\subref{fig:test_id31_picture_mass_m0}) without payload. (\subref{fig:test_id31_picture_mass_m1}) with $13\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m2}) with $26\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m3}) with $39\,\text{kg}$ payload.
|
|
|
|
|
% #+attr_latex: :options [htbp]
|
|
|
|
|
% #+begin_figure
|
|
|
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m0}$m=0\,\text{kg}$}
|
|
|
|
|
% #+attr_latex: :options {0.24\textwidth}
|
|
|
|
|
% #+begin_subfigure
|
|
|
|
|
% #+attr_latex: :width 0.99\linewidth
|
|
|
|
|
% [[file:figs/test_id31_picture_mass_m0.jpg]]
|
|
|
|
|
% #+end_subfigure
|
|
|
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m1}$m=13\,\text{kg}$}
|
|
|
|
|
% #+attr_latex: :options {0.24\textwidth}
|
|
|
|
|
% #+begin_subfigure
|
|
|
|
|
% #+attr_latex: :width 0.99\linewidth
|
|
|
|
|
% [[file:figs/test_id31_picture_mass_m1.jpg]]
|
|
|
|
|
% #+end_subfigure
|
|
|
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m2}$m=26\,\text{kg}$}
|
|
|
|
|
% #+attr_latex: :options {0.24\textwidth}
|
|
|
|
|
% #+begin_subfigure
|
|
|
|
|
% #+attr_latex: :width 0.99\linewidth
|
|
|
|
|
% [[file:figs/test_id31_picture_mass_m2.jpg]]
|
|
|
|
|
% #+end_subfigure
|
|
|
|
|
% #+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m3}$m=39\,\text{kg}$}
|
|
|
|
|
% #+attr_latex: :options {0.24\textwidth}
|
|
|
|
|
% #+begin_subfigure
|
|
|
|
|
% #+attr_latex: :width 0.99\linewidth
|
|
|
|
|
% [[file:figs/test_id31_picture_mass_m3.jpg]]
|
|
|
|
|
% #+end_subfigure
|
|
|
|
|
% #+end_figure
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
%% Identify the model dynamics for all payload conditions
|
|
|
|
|
% Initialize each Simscape model elements
|
|
|
|
|
initializeGround();
|
|
|
|
|
initializeGranite();
|
|
|
|
|
initializeTy();
|
|
|
|
|
initializeRy();
|
|
|
|
|
initializeRz();
|
|
|
|
|
initializeMicroHexapod();
|
|
|
|
|
initializeNanoHexapod('flex_bot_type', '2dof', ...
|
|
|
|
|
'flex_top_type', '3dof', ...
|
|
|
|
|
'motion_sensor_type', 'plates', ...
|
|
|
|
|
'actuator_type', '2dof');
|
|
|
|
|
initializeSample('type', '0');
|
|
|
|
|
|
|
|
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
|
|
|
initializeDisturbances('enable', false);
|
|
|
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
|
initializeController('type', 'open-loop');
|
|
|
|
|
initializeReferences();
|
|
|
|
|
|
|
|
|
|
% Input/Output definition
|
|
|
|
|
clear io; io_i = 1;
|
|
|
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
|
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vs'); io_i = io_i + 1; % Force Sensors
|
|
|
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors
|
|
|
|
|
|
|
|
|
|
initializeSample('type', '0');
|
|
|
|
|
Gm_m0_Wz0 = linearize(mdl, io);
|
|
|
|
|
Gm_m0_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm_m0_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
initializeSample('type', '1');
|
|
|
|
|
Gm_m1_Wz0 = linearize(mdl, io);
|
|
|
|
|
Gm_m1_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm_m1_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
initializeSample('type', '2');
|
|
|
|
|
Gm_m2_Wz0 = linearize(mdl, io);
|
|
|
|
|
Gm_m2_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm_m2_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
initializeSample('type', '3');
|
|
|
|
|
Gm_m3_Wz0 = linearize(mdl, io);
|
|
|
|
|
Gm_m3_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm_m3_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
%% Identify the plant from experimental data - All payloads
|
|
|
|
|
|
|
|
|
|
% Load identification data
|
|
|
|
|
data_m0_Wz0 = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat');
|
|
|
|
|
data_m1_Wz0 = load('2023-08-08_18-57_ol_plant_m1_Wz0.mat');
|
|
|
|
|
data_m2_Wz0 = load('2023-08-08_17-12_ol_plant_m2_Wz0.mat');
|
|
|
|
|
data_m3_Wz0 = load('2023-08-08_18-20_ol_plant_m3_Wz0.mat');
|
|
|
|
|
|
|
|
|
|
% Sampling Time [s]
|
|
|
|
|
Ts = 1e-4;
|
|
|
|
|
|
|
|
|
|
% Hannning Windows
|
|
|
|
|
Nfft = floor(2.0/Ts);
|
|
|
|
|
win = hanning(Nfft);
|
|
|
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
|
|
|
|
|
|
% And we get the frequency vector
|
|
|
|
|
[~, f] = tfestimate(data_m0_Wz0.uL1.id_plant, data_m0_Wz0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
|
|
|
|
|
% No payload
|
|
|
|
|
G_iff_m0_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
|
|
|
|
|
G_iff_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
G_int_m0_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
% 1 "payload layer"
|
|
|
|
|
G_iff_m1_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
|
|
|
|
|
G_iff_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
G_int_m1_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
% 2 "payload layers"
|
|
|
|
|
G_iff_m2_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
|
|
|
|
|
G_iff_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
G_int_m2_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
% 3 "payload layers"
|
|
|
|
|
G_iff_m3_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
|
|
|
|
|
G_iff_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
G_int_m3_Wz0 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
ax1 = nexttile([2,1]);
|
|
|
|
|
hold on;
|
|
|
|
|
plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (0kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (13kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (26kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_int_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (39kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
|
|
|
|
|
'DisplayName', 'Model (0kg)');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
|
|
|
|
|
'DisplayName', 'Model (13kg)');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
|
|
|
|
|
'DisplayName', 'Model (26kg)');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
|
|
|
|
|
'DisplayName', 'Model (39kg)');
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
|
|
|
|
ylim([1e-8, 5e-4]);
|
|
|
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
ax2 = nexttile;
|
|
|
|
|
hold on;
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:))
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:))
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:))
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:))
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
|
|
|
hold off;
|
|
|
|
|
yticks(-360:90:360);
|
|
|
|
|
ylim([-90, 180])
|
|
|
|
|
|
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
|
xlim([10, 5e2]);
|
|
|
|
|
xticks([10, 20, 50, 100, 200, 500])
|
|
|
|
|
|
|
|
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
ax1 = nexttile([2,1]);
|
|
|
|
|
hold on;
|
|
|
|
|
plot(f, abs(G_iff_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (0kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_iff_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (13kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_iff_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (26kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(f, abs(G_iff_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
|
|
|
|
'DisplayName', 'Meas (39kg)');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
|
|
|
|
|
'DisplayName', 'Model (0kg)');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
|
|
|
|
|
'DisplayName', 'Model (13kg)');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
|
|
|
|
|
'DisplayName', 'Model (26kg)');
|
|
|
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
|
|
|
|
|
'DisplayName', 'Model (39kg)');
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
|
|
|
ylim([1e-2, 4e1]);
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
|
|
|
|
|
ax2 = nexttile;
|
|
|
|
|
hold on;
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
for i =1:6
|
|
|
|
|
plot(f, 180/pi*angle(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]);
|
|
|
|
|
end
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:))
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:))
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:))
|
|
|
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:))
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
|
|
|
hold off;
|
|
|
|
|
yticks(-360:90:360);
|
|
|
|
|
ylim([-90, 180])
|
|
|
|
|
|
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
|
|
|
xlim([10, 5e2]);
|
|
|
|
|
xticks([10, 20, 50, 100, 200, 500])
|
|
|
|
|
|
|
|
|
|
% Effect of Spindle Rotation
|
|
|
|
|
% <<ssec:test_id31_open_loop_plant_rotation>>
|
|
|
|
|
|
|
|
|
|
% To verify that all the kinematics in Figure ref:fig:test_id31_block_schematic_plant are correct and to check whether the system dynamics is affected by Spindle rotation of not, three identification experiments were performed: no spindle rotation, spindle rotation at $36\,\text{deg}/s$ and at $180\,\text{deg}/s$.
|
|
|
|
|
|
2025-02-06 17:05:29 +01:00
|
|
|
% The obtained dynamics from command signal $u$ to estimated strut error $\epsilon\mathcal{L}$ are displayed in Figure ref:fig:test_id31_effect_rotation.
|
2025-02-04 14:10:54 +01:00
|
|
|
% Both direct terms (Figure ref:fig:test_id31_effect_rotation_direct) and coupling terms (Figure ref:fig:test_id31_effect_rotation_coupling) are unaffected by the rotation.
|
2025-02-06 17:05:29 +01:00
|
|
|
% The same can be observed for the dynamics from command signal to encoders and to force sensors.
|
|
|
|
|
% This confirms that spindle's rotation has no significant effect on plant dynamics.
|
2025-02-04 14:10:54 +01:00
|
|
|
% This also indicates that the metrology kinematics is correct and is working in real time.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
%% Identify the model dynamics with Spindle rotation
|
|
|
|
|
initializeSample('type', '0');
|
|
|
|
|
initializeReferences(...
|
|
|
|
|
'Rz_type', 'rotating', ...
|
|
|
|
|
'Rz_period', 360/36); % 36 deg/s, 6rpm
|
|
|
|
|
Gm_m0_Wz36 = linearize(mdl, io, 0.1);
|
|
|
|
|
Gm_m0_Wz36.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm_m0_Wz36.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
initializeReferences(...
|
|
|
|
|
'Rz_type', 'rotating', ...
|
|
|
|
|
'Rz_period', 360/180); % 180 deg/s, 30rpm
|
|
|
|
|
Gm_m0_Wz180 = linearize(mdl, io, 0.1);
|
|
|
|
|
Gm_m0_Wz180.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
|
|
|
|
Gm_m0_Wz180.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
|
|
|
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
|
|
|
|
|
|
|
|
|
%% Identify the plant from experimental data - Effect of rotation
|
|
|
|
|
|
|
|
|
|
% Load identification data
|
|
|
|
|
data_m0_Wz36 = load('2023-08-08_16-28_ol_plant_m0_Wz36.mat');
|
|
|
|
|
data_m0_Wz180 = load('2023-08-08_16-45_ol_plant_m0_Wz180.mat');
|
|
|
|
|
|
|
|
|
|
% Spindle Rotation at 36 deg/s
|
|
|
|
|
G_iff_m0_Wz36 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
|
|
|
|
|
G_iff_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
G_int_m0_Wz36 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
% Spindle Rotation at 180 deg/s
|
|
|
|
|
G_iff_m0_Wz180 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs6]';
|
|
|
|
|
|
|
|
|
|
G_iff_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
G_int_m0_Wz180 = zeros(length(f), 6, 6);
|
|
|
|
|
for i_strut = 1:6
|
|
|
|
|
eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L6]';
|
|
|
|
|
|
|
|
|
|
G_int_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
% The identified dynamics are then saved for further use.
|
|
|
|
|
save('./mat/test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m0_Wz36', 'Gm_m0_Wz180', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');
|
|
|
|
|
save('./mat/test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m0_Wz36', 'G_int_m0_Wz180', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', ...
|
|
|
|
|
'G_iff_m0_Wz0', 'G_iff_m0_Wz36', 'G_iff_m0_Wz180', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
|
|
|
|
|
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
nexttile();
|
|
|
|
|
hold on;
|
|
|
|
|
plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'DisplayName', '$\Omega_z = 0$');
|
|
|
|
|
plot(f, abs(G_int_m0_Wz36(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'DisplayName', '$\Omega_z = 36$ deg/s');
|
|
|
|
|
plot(f, abs(G_int_m0_Wz180(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'DisplayName', '$\Omega_z = 180$ deg/s');
|
|
|
|
|
for i = 2:6
|
|
|
|
|
plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
plot(f, abs(G_int_m0_Wz36(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
plot(f, abs(G_int_m0_Wz180(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off')
|
|
|
|
|
end
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
|
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
xlim([10, 1e3]); ylim([1e-8, 2e-4])
|
|
|
|
|
|
|
|
|
|
figure;
|
|
|
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
|
|
|
|
|
|
nexttile();
|
|
|
|
|
hold on;
|
|
|
|
|
plot(f, abs(G_int_m0_Wz0(:, 1, 2)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'DisplayName', '$\Omega_z = 0$');
|
|
|
|
|
plot(f, abs(G_int_m0_Wz36(:, 1, 2)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'DisplayName', '$\Omega_z = 36$ deg/s');
|
|
|
|
|
plot(f, abs(G_int_m0_Wz180(:, 1, 2)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'DisplayName', '$\Omega_z = 180$ deg/s');
|
|
|
|
|
for i = 1:5
|
|
|
|
|
for j = i+1:6
|
|
|
|
|
plot(f, abs(G_int_m0_Wz0(:, i, j)), 'color', [colors(1,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(f, abs(G_int_m0_Wz36(:, i, j)), 'color', [colors(2,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
plot(f, abs(G_int_m0_Wz180(:, i, j)), 'color', [colors(3,:), 0.5], ...
|
|
|
|
|
'HandleVisibility', 'off');
|
|
|
|
|
end
|
|
|
|
|
end
|
|
|
|
|
hold off;
|
|
|
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
|
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
|
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
|
xlim([10, 1e3]); ylim([1e-8, 2e-4])
|
