384 lines
11 KiB
Mathematica
384 lines
11 KiB
Mathematica
|
%% Clear Workspace and Close figures
|
||
|
|
clear; close all; clc;
|
||
|
|
|
||
|
|
%% Intialize Laplace variable
|
||
|
|
s = zpk('s');
|
||
|
|
|
||
|
|
%% Add useful folders to the path
|
||
|
|
addpath('test_bench_apa300ml/');
|
||
|
|
addpath('png/');
|
||
|
|
addpath('mat/');
|
||
|
|
addpath('src/');
|
||
|
|
|
||
|
|
%% Frequency vector used for many plots
|
||
|
|
freqs = 2*logspace(0, 3, 1000);
|
||
|
|
|
||
|
|
%% Open Simscape Model
|
||
|
|
options = linearizeOptions;
|
||
|
|
options.SampleTime = 0;
|
||
|
|
|
||
|
|
% Name of the Simulink File
|
||
|
|
mdl = 'test_bench_apa300ml';
|
||
|
|
|
||
|
|
open(mdl)
|
||
|
|
|
||
|
|
%% Initialize the structure with default values
|
||
|
|
n_hexapod = struct();
|
||
|
|
n_hexapod.actuator = initializeAPA(...
|
||
|
|
'type', '2dof', ...
|
||
|
|
'Ga', 1, ... % Actuator constant [N/V]
|
||
|
|
'Gs', 1); % Sensor constant [V/m]
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % DAC Voltage
|
||
|
|
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
|
||
|
|
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||
|
|
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Interferometer
|
||
|
|
|
||
|
|
%% Run the linearization
|
||
|
|
Ga = linearize(mdl, io, 0.0, options);
|
||
|
|
Ga.InputName = {'Va'};
|
||
|
|
Ga.OutputName = {'Vs', 'de', 'da'};
|
||
|
|
|
||
|
|
%% Bode plot of the transfer function from u to taum
|
||
|
|
freqs = logspace(1, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:45:360);
|
||
|
|
ylim([-180, 0])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
|
||
|
|
%% Bode plot of the transfer function from Va to de and da
|
||
|
|
freqs = logspace(1, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Ga('da', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Interferometer')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
legend('location', 'southwest');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('da', 'Va'), freqs, 'Hz'))))
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:45:360);
|
||
|
|
ylim([-180, 0])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
|
||
|
|
%% Load Data
|
||
|
|
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
|
||
|
|
|
||
|
|
%% Initialize a 2DoF APA with Ga=Gs=1
|
||
|
|
n_hexapod.actuator = initializeAPA(...
|
||
|
|
'type', '2dof', ...
|
||
|
|
'ga', 1, ...
|
||
|
|
'gs', 1);
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
|
||
|
|
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
|
||
|
|
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||
|
|
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Attocube
|
||
|
|
|
||
|
|
%% Identification
|
||
|
|
Gs = linearize(mdl, io, 0.0, options);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'Vs', 'de', 'da'};
|
||
|
|
|
||
|
|
%% Estimated Actuator Constant
|
||
|
|
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(Gs('de', 'Va')); % [N/V]
|
||
|
|
|
||
|
|
%% Estimated Sensor Constant
|
||
|
|
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
|
||
|
|
|
||
|
|
%% Set the identified constants
|
||
|
|
n_hexapod.actuator = initializeAPA(...
|
||
|
|
'type', '2dof', ...
|
||
|
|
'ga', ga, ... % Actuator gain [N/V]
|
||
|
|
'gs', gs); % Sensor gain [V/m]
|
||
|
|
|
||
|
|
%% Identify again the dynamics with correct Ga,Gs
|
||
|
|
Gs = linearize(mdl, io, 0.0, options);
|
||
|
|
Gs = Gs*exp(-Ts*s);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'Vs', 'de', 'da'};
|
||
|
|
|
||
|
|
%% Bode plot of the transfer function from u to de
|
||
|
|
freqs = logspace(1,4,1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
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]);
|
||
|
|
|
||
|
|
%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
|
||
|
|
freqs = logspace(1,4,1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
|
||
|
|
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]);
|
||
|
|
|
||
|
|
%% Initialize the APA as a flexible body
|
||
|
|
n_hexapod.actuator = initializeAPA(...
|
||
|
|
'type', 'flexible', ...
|
||
|
|
'ga', 1, ...
|
||
|
|
'gs', 1);
|
||
|
|
|
||
|
|
%% Identify the dynamics
|
||
|
|
Gs = linearize(mdl, io, 0.0, options);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'Vs', 'de', 'da'};
|
||
|
|
|
||
|
|
%% Actuator Constant
|
||
|
|
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(Gs('de', 'Va')); % [N/V]
|
||
|
|
|
||
|
|
%% Sensor Constant
|
||
|
|
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
|
||
|
|
|
||
|
|
%% Set the identified constants
|
||
|
|
n_hexapod.actuator = initializeAPA(...
|
||
|
|
'type', 'flexible', ...
|
||
|
|
'ga', ga, ... % Actuator gain [N/V]
|
||
|
|
'gs', gs); % Sensor gain [V/m]
|
||
|
|
|
||
|
|
%% Identify with updated constants
|
||
|
|
Gs = linearize(mdl, io, 0.0, options);
|
||
|
|
Gs = Gs*exp(-Ts*s);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'Vs', 'de', 'da'};
|
||
|
|
|
||
|
|
%% Bode plot of the transfer function from V_a to d_e (both Simscape and measured FRF)
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-9, 1e-3]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
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]);
|
||
|
|
|
||
|
|
%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
|
||
|
|
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]);
|
||
|
|
|
||
|
|
%% Optimized parameters
|
||
|
|
n_hexapod.actuator = initializeAPA('type', '2dof', ...
|
||
|
|
'Ga', -32.2, ...
|
||
|
|
'Gs', 0.088, ...
|
||
|
|
'k', ones(6,1)*0.38e6, ...
|
||
|
|
'ke', ones(6,1)*1.75e6, ...
|
||
|
|
'ka', ones(6,1)*3e7, ...
|
||
|
|
'c', ones(6,1)*1.3e2, ...
|
||
|
|
'ce', ones(6,1)*1e1, ...
|
||
|
|
'ca', ones(6,1)*1e1 ...
|
||
|
|
);
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
|
||
|
|
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
|
||
|
|
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||
|
|
|
||
|
|
%% Identification with optimized parameters
|
||
|
|
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'Vs', 'de'};
|
||
|
|
|
||
|
|
%% Comparison of the experimental data and Simscape Model
|
||
|
|
freqs = 5*logspace(0, 3, 1000);
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
|
||
|
|
ax1b = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
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]);
|
||
|
|
|
||
|
|
ax2b = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(apa_nums)
|
||
|
|
plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
|
||
|
|
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,ax1b,ax2b],'x');
|
||
|
|
xlim([10, 2e3]);
|