% Matlab Init :noexport:ignore: %% 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); % Comparison with the measured dynamics % <> % The dynamics of the micro-station was measured by placing accelerometers on each stage and by impacting the translation stage with an instrumented hammer in three directions. % The obtained FRFs were then projected at the CoM of each stage. % To gain a first insight into the accuracy of the obtained model, the FRFs from the hammer impacts to the acceleration of each stage were extracted from the Simscape model and compared with the measurements in Figure ref:fig:ustation_comp_com_response. % Even though there is some similarity between the model and the measurements (similar overall shapes and amplitudes), it is clear that the Simscape model does not accurately represent the complex micro-station dynamics. % Tuning the numerous model parameters to better match the measurements is a highly non-linear optimization problem that is difficult to solve in practice. %% 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]) % Micro-station compliance % <> % As discussed in the previous section, the dynamics of the micro-station is complex, and tuning the multi-body model parameters to obtain a perfect match is difficult. % When considering the NASS, the most important dynamical characteristics of the micro-station is its compliance, as it can affect the plant dynamics. % Therefore, the adopted strategy is to accurately model the micro-station compliance. % The micro-station compliance was experimentally measured using the setup illustrated in Figure ref:fig:ustation_compliance_meas. % Four 3-axis accelerometers were fixed to the micro-hexapod top platform. % The micro-hexapod top platform was impacted at 10 different points. % For each impact position, 10 impacts were performed to average and improve the data quality. % #+name: fig:ustation_compliance_meas % #+caption: Schematic of the measurement setup used to estimate the compliance of the micro-station. The top platform of the positioning hexapod is shown with four 3-axis accelerometers (shown in red) are on top. 10 hammer impacts are performed at different locations (shown in blue). % [[file:figs/ustation_compliance_meas.png]] % To convert the 12 acceleration signals $a_{\mathcal{L}} = [a_{1x}\ a_{1y}\ a_{1z}\ a_{2x}\ \dots\ a_{4z}]$ to the acceleration expressed in the frame $\{\mathcal{X}\}$ $a_{\mathcal{X}} = [a_{dx}\ a_{dy}\ a_{dz}\ a_{rx}\ a_{ry}\ a_{rz}]$, a Jacobian matrix $\mathbf{J}_a$ is written based on the positions and orientations of the accelerometers eqref:eq:ustation_compliance_acc_jacobian. % \begin{equation}\label{eq:ustation_compliance_acc_jacobian} % \mathbf{J}_a = \begin{bmatrix} % 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 % \end{bmatrix} % \end{equation} % Then, the acceleration in the cartesian frame can be computed using eqref:eq:ustation_compute_cart_acc. % \begin{equation}\label{eq:ustation_compute_cart_acc} % a_{\mathcal{X}} = \mathbf{J}_a^\dagger \cdot a_{\mathcal{L}} % \end{equation} % Similar to what is done for the accelerometers, a Jacobian matrix $\mathbf{J}_F$ is computed eqref:eq:ustation_compliance_force_jacobian and used to convert the individual hammer forces $F_{\mathcal{L}}$ to force and torques $F_{\mathcal{X}}$ applied at the center of the micro-hexapod top plate (defined by frame $\{\mathcal{X}\}$ in Figure ref:fig:ustation_compliance_meas). % \begin{equation}\label{eq:ustation_compliance_force_jacobian} % \mathbf{J}_F = \begin{bmatrix} % 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 % \end{bmatrix} % \end{equation} % The equivalent forces and torques applied at center of $\{\mathcal{X}\}$ are then computed using eqref:eq:ustation_compute_cart_force. % \begin{equation}\label{eq:ustation_compute_cart_force} % F_{\mathcal{X}} = \mathbf{J}_F^t \cdot F_{\mathcal{L}} % \end{equation} % Using the two Jacobian matrices, the FRF from the 10 hammer impacts to the 12 accelerometer outputs can be converted to the FRF from 6 forces/torques applied at the origin of frame $\{\mathcal{X}\}$ to the 6 linear/angular accelerations of the top platform expressed with respect to $\{\mathcal{X}\}$. % These FRFs were then used for comparison with the Simscape model. % 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)); % The compliance of the micro-station multi-body model was extracted by computing the transfer function from forces/torques applied on the hexapod's top platform to the "absolute" motion of the top platform. % These results are compared with the measurements in Figure ref:fig:ustation_frf_compliance_model. % Considering the complexity of the micro-station compliance dynamics, the model compliance matches sufficiently well for the current application. %% 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], 'DisplayName', '$D_x/F_x$ - Measured') plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$D_y/F_y$ - Measured') plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '-', 'color', [colors(3,:), 0.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], 'DisplayName', '$R_x/M_x$ - Measured') plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$R_y/M_y$ - Measured') plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', [colors(3,:), 0.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])