322 lines
13 KiB
Matlab
322 lines
13 KiB
Matlab
%% ustation_2_modeling.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
|
|
|
|
% Simulink Model name
|
|
mdl = 'ustation_simscape';
|
|
|
|
load('nass_model_conf_simulink.mat');
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(log10(10), log10(2e3), 1000);
|
|
|
|
%% Indentify the model dynamics from the 3 hammer imapcts
|
|
% To the motion of each solid body at their CoM
|
|
|
|
% All stages are initialized
|
|
initializeGround( 'type', 'rigid');
|
|
initializeGranite( 'type', 'flexible');
|
|
initializeTy( 'type', 'flexible');
|
|
initializeRy( 'type', 'flexible');
|
|
initializeRz( 'type', 'flexible');
|
|
initializeMicroHexapod('type', 'flexible');
|
|
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
initializeReferences();
|
|
initializeDisturbances('enable', false);
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Granite/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
% Run the linearization
|
|
G_ms = linearize(mdl, io, 0);
|
|
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
|
|
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
|
|
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
|
|
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
|
|
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
|
|
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
|
|
|
|
% Load estimated FRF at CoM
|
|
load('ustation_frf_matrix.mat', 'freqs');
|
|
load('ustation_frf_com.mat', 'frfs_CoM');
|
|
|
|
% Initialization of some variables to the figures
|
|
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
|
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'};
|
|
f = logspace(log10(10), log10(500), 1000);
|
|
|
|
%% Spindle - X response
|
|
n_stg = 5; % Measured Stage
|
|
n_dir = 1; % Measured DoF: x, y, z, Rx, Ry, Rz
|
|
n_exc = 1; % Hammer impact: x, y, z
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
|
text(50, 2e-2,{'$D_x/F_x$ - Spindle'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([10, 200]);
|
|
ylim([1e-6, 1e-1])
|
|
|
|
%% Hexapod - Y response
|
|
n_stg = 6; % Measured Stage
|
|
n_dir = 2; % Measured DoF: x, y, z, Rx, Ry, Rz
|
|
n_exc = 2; % Hammer impact: x, y, z
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
|
text(50, 2e-2,{'$D_y/F_y$ - Hexapod'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([10, 200]);
|
|
ylim([1e-6, 1e-1])
|
|
|
|
%% Tilt stage - Z response
|
|
n_stg = 4; % Measured Stage
|
|
n_dir = 3; % Measured DoF: x, y, z, Rx, Ry, Rz
|
|
n_exc = 3; % Hammer impact: x, y, z
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
|
text(50, 2e-2,{'$D_z/F_z$ - Tilt'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([10, 200]);
|
|
ylim([1e-6, 1e-1])
|
|
|
|
% Positions and orientation of accelerometers
|
|
% | *Num* | *Position* | *Orientation* | *Sensibility* | *Channels* |
|
|
% |-------+------------+---------------+---------------+------------|
|
|
% | 1 | [0, +A, 0] | [x, y, z] | 1V/g | 1-3 |
|
|
% | 2 | [-B, 0, 0] | [x, y, z] | 1V/g | 4-6 |
|
|
% | 3 | [0, -A, 0] | [x, y, z] | 0.1V/g | 7-9 |
|
|
% | 4 | [+B, 0, 0] | [x, y, z] | 1V/g | 10-12 |
|
|
%
|
|
|
|
% Measuerment channels
|
|
%
|
|
% | Acc Number | Dir | Channel Number |
|
|
% |------------+-----+----------------|
|
|
% | 1 | x | 1 |
|
|
% | 1 | y | 2 |
|
|
% | 1 | z | 3 |
|
|
% | 2 | x | 4 |
|
|
% | 2 | y | 5 |
|
|
% | 2 | z | 6 |
|
|
% | 3 | x | 7 |
|
|
% | 3 | y | 8 |
|
|
% | 3 | z | 9 |
|
|
% | 4 | x | 10 |
|
|
% | 4 | y | 11 |
|
|
% | 4 | z | 12 |
|
|
% | Hammer | | 13 |
|
|
|
|
% 10 measurements corresponding to 10 different Hammer impact locations
|
|
% | *Num* | *Direction* | *Position* | Accelerometer position | Jacobian number |
|
|
% |-------+-------------+------------+------------------------+-----------------|
|
|
% | 1 | -Y | [0, +A, 0] | 1 | -2 |
|
|
% | 2 | -Z | [0, +A, 0] | 1 | -3 |
|
|
% | 3 | X | [-B, 0, 0] | 2 | 4 |
|
|
% | 4 | -Z | [-B, 0, 0] | 2 | -6 |
|
|
% | 5 | Y | [0, -A, 0] | 3 | 8 |
|
|
% | 6 | -Z | [0, -A, 0] | 3 | -9 |
|
|
% | 7 | -X | [+B, 0, 0] | 4 | -10 |
|
|
% | 8 | -Z | [+B, 0, 0] | 4 | -12 |
|
|
% | 9 | -X | [0, -A, 0] | 3 | -7 |
|
|
% | 10 | -X | [0, +A, 0] | 1 | -1 |
|
|
|
|
|
|
%% Jacobian for accelerometers
|
|
% aX = Ja . aL
|
|
d = 0.14; % [m]
|
|
Ja = [1 0 0 0 0 -d;
|
|
0 1 0 0 0 0;
|
|
0 0 1 d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 1 0 0 0 -d;
|
|
0 0 1 0 d 0;
|
|
1 0 0 0 0 d;
|
|
0 1 0 0 0 0;
|
|
0 0 1 -d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 1 0 0 0 d;
|
|
0 0 1 0 -d 0];
|
|
|
|
Ja_inv = pinv(Ja);
|
|
|
|
%% Jacobian for hammer impacts
|
|
% F = Jf' tau
|
|
Jf = [0 -1 0 0 0 0;
|
|
0 0 -1 -d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 0 -1 0 -d 0;
|
|
0 1 0 0 0 0;
|
|
0 0 -1 d 0 0;
|
|
-1 0 0 0 0 0;
|
|
0 0 -1 0 d 0;
|
|
-1 0 0 0 0 -d;
|
|
-1 0 0 0 0 d]';
|
|
Jf_inv = pinv(Jf);
|
|
|
|
%% Load raw measurement data
|
|
% "Track1" to "Track12" are the 12 accelerometers
|
|
% "Track13" is the instrumented hammer force sensor
|
|
data = [
|
|
load('ustation_compliance_hammer_1.mat'), ...
|
|
load('ustation_compliance_hammer_2.mat'), ...
|
|
load('ustation_compliance_hammer_3.mat'), ...
|
|
load('ustation_compliance_hammer_4.mat'), ...
|
|
load('ustation_compliance_hammer_5.mat'), ...
|
|
load('ustation_compliance_hammer_6.mat'), ...
|
|
load('ustation_compliance_hammer_7.mat'), ...
|
|
load('ustation_compliance_hammer_8.mat'), ...
|
|
load('ustation_compliance_hammer_9.mat'), ...
|
|
load('ustation_compliance_hammer_10.mat')];
|
|
|
|
%% Prepare the FRF computation
|
|
Ts = 1e-3; % Sampling Time [s]
|
|
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
|
win = ones(Nfft, 1); % Rectangular window
|
|
[~, f] = tfestimate(data(1).Track13, data(1).Track1, win, [], Nfft, 1/Ts);
|
|
|
|
% Samples taken before and after the hammer "impact"
|
|
pre_n = floor(0.1/Ts);
|
|
post_n = Nfft - pre_n - 1;
|
|
|
|
%% Compute the FRFs for the 10 hammer impact locations.
|
|
% The FRF from hammer force the 12 accelerometers are computed
|
|
% for each of the 10 hammer impacts and averaged
|
|
G_raw = zeros(12,10,length(f));
|
|
|
|
for i = 1:10 % For each impact location
|
|
% Find the 10 impacts
|
|
[~, impacts_i] = find(diff(data(i).Track13 > 50)==1);
|
|
% Only keep the first 10 impacts if there are more than 10 impacts
|
|
if length(impacts_i)>10
|
|
impacts_i(11:end) = [];
|
|
end
|
|
|
|
% Average the FRF for the 10 impacts
|
|
for impact_i = impacts_i
|
|
i_start = impact_i - pre_n;
|
|
i_end = impact_i + post_n;
|
|
data_in = data(i).Track13(i_start:i_end); % [N]
|
|
% Remove hammer DC offset
|
|
data_in = data_in - mean(data_in(end-pre_n:end));
|
|
% Combine all outputs [m/s^2]
|
|
data_out = [data(i).Track1( i_start:i_end); ...
|
|
data(i).Track2( i_start:i_end); ...
|
|
data(i).Track3( i_start:i_end); ...
|
|
data(i).Track4( i_start:i_end); ...
|
|
data(i).Track5( i_start:i_end); ...
|
|
data(i).Track6( i_start:i_end); ...
|
|
data(i).Track7( i_start:i_end); ...
|
|
data(i).Track8( i_start:i_end); ...
|
|
data(i).Track9( i_start:i_end); ...
|
|
data(i).Track10(i_start:i_end); ...
|
|
data(i).Track11(i_start:i_end); ...
|
|
data(i).Track12(i_start:i_end)];
|
|
|
|
[frf, ~] = tfestimate(data_in, data_out', win, [], Nfft, 1/Ts);
|
|
G_raw(:,i,:) = squeeze(G_raw(:,i,:)) + 0.1*frf'./(-(2*pi*f').^2);
|
|
end
|
|
end
|
|
|
|
%% Compute transfer function in cartesian frame using Jacobian matrices
|
|
% FRF_cartesian = inv(Ja) * FRF * inv(Jf)
|
|
FRF_cartesian = pagemtimes(Ja_inv, pagemtimes(G_raw, Jf_inv));
|
|
|
|
%% Identification of the compliance of the micro-station
|
|
|
|
% Initialize simulation with default parameters (flexible elements)
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
|
|
initializeReferences();
|
|
initializeSimscapeConfiguration();
|
|
initializeLoggingConfiguration();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform
|
|
|
|
% Run the linearization
|
|
Gm = linearize(mdl, io, 0);
|
|
Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
|
|
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
|
|
|
|
%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(FRF_cartesian(1,1,:))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_x/F_x$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_y/F_y$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_z/F_z$ - Measured')
|
|
plot(f, abs(squeeze(freqresp(Gm(1,1), f, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(2,2), f, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(3,3), f, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$ - Model')
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
|
xlim([20, 500]); ylim([2e-10, 1e-6]);
|
|
xticks([20, 50, 100, 200, 500])
|
|
|
|
%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(FRF_cartesian(4,4,:))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_x/M_x$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_y/M_y$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_z/M_z$ - Measured')
|
|
plot(f, abs(squeeze(freqresp(Gm(4,4), f, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$R_x/M_x$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(5,5), f, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$R_y/M_y$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(6,6), f, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$R_z/M_z$ - Model')
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [rad/Nm]');
|
|
xlim([20, 500]); ylim([2e-7, 1e-4]);
|
|
xticks([20, 50, 100, 200, 500])
|