308 lines
11 KiB
Matlab
308 lines
11 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./src/'); % Path for scripts
|
|
addpath('./mat/'); % Path for data
|
|
addpath('./STEPS/'); % Path for Simscape Model
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Linearization options
|
|
opts = linearizeOptions;
|
|
opts.SampleTime = 0;
|
|
|
|
%% Open Simscape Model
|
|
mdl = 'detail_fem_super_element'; % Name of the Simulink File
|
|
open(mdl); % Open Simscape Model
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
freqs = logspace(1,3,500); % Frequency vector [Hz]
|
|
|
|
%% Estimate "Sensor Constant" - (THP5H)
|
|
d33 = 680e-12; % Strain constant [m/V]
|
|
n = 160; % Number of layers per stack
|
|
eT = 4500*8.854e-12; % Permittivity under constant stress [F/m]
|
|
sD = 21e-12; % Compliance under constant electric displacement [m2/N]
|
|
|
|
gs = d33/(eT*sD*n); % Sensor Constant [V/m]
|
|
|
|
%% Estimate "Actuator Constant" - (THP5H)
|
|
d33 = 680e-12; % Strain constant [m/V]
|
|
n = 320; % Number of layers
|
|
|
|
cE = 1/sD; % Youngs modulus [N/m^2]
|
|
A = (10e-3)^2; % Area of the stacks [m^2]
|
|
L = 40e-3; % Length of the two stacks [m]
|
|
ka = cE*A/L; % Stiffness of the two stacks [N/m]
|
|
|
|
ga = d33*n*ka; % Actuator Constant [N/V]
|
|
|
|
%% Load reduced order model
|
|
K = readmatrix('APA95ML_K.CSV'); % order: 48
|
|
M = readmatrix('APA95ML_M.CSV');
|
|
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
|
|
|
|
%% Stiffness estimation
|
|
m = 0.0001; % block-free condition, no payload
|
|
k_support = 1e9;
|
|
c_support = 1e3;
|
|
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
G = linearize(mdl, io);
|
|
|
|
% The inverse of the DC gain of the transfer function
|
|
% from vertical force to vertical displacement is the axial stiffness of the APA
|
|
k_est = 1/dcgain(G); % [N/m]
|
|
|
|
%% Estimated compliance of the APA95ML
|
|
freqs = logspace(2, log10(5000), 1000);
|
|
|
|
% Get first resonance indice
|
|
i_max = find(abs(squeeze(freqresp(G, freqs(2:end), 'Hz'))) - abs(squeeze(freqresp(G, freqs(1:end-1), 'Hz'))) < 0, 1);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'DisplayName', 'Compliance');
|
|
plot([freqs(1), freqs(end)], [1/k_est, 1/k_est], 'k--', 'DisplayName', sprintf('$1/k$ ($k = %.0f N/\\mu m$)', 1e-6*k_est))
|
|
xline(freqs(i_max), '--', 'linewidth', 1, 'color', [0,0,0], 'DisplayName', sprintf('$f_0 = %.0f$ Hz', freqs(i_max)))
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([100, 5000]);
|
|
|
|
%% Estimation of the amplification factor and Stroke
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/d'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
G = linearize(mdl, io);
|
|
|
|
% Estimated amplification factor
|
|
ampl_factor = abs(dcgain(G(1,1))./dcgain(G(2,1)));
|
|
|
|
% Estimated stroke
|
|
apa_stroke = ampl_factor * 3 * 20e-6; % [m]
|
|
|
|
%% Experimental plant identification
|
|
% with PD200 amplifier (gain of 20) - 2 stacks as an actuator, 1 as a sensor
|
|
load('apa95ml_5kg_2a_1s.mat')
|
|
|
|
Va = 20*u; % Voltage amplifier gain: 20
|
|
|
|
% Spectral Analysis parameters
|
|
Ts = t(end)/(length(t)-1);
|
|
Nfft = floor(1/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
% Identification of the transfer function from Va to di
|
|
[G_y, f] = tfestimate(detrend(Va, 0), detrend(y, 0), win, Noverlap, Nfft, 1/Ts);
|
|
[G_Vs, ~] = tfestimate(detrend(Va, 0), detrend(v, 0), win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Plant Identification from Multi-Body model
|
|
% Load Reduced Order Matrices
|
|
K = readmatrix('APA95ML_K.CSV'); % order: 48
|
|
M = readmatrix('APA95ML_M.CSV');
|
|
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
|
|
|
|
m = 5.5; % Mass of the suspended granite [kg]
|
|
k_support = 4e7;
|
|
c_support = 3e2;
|
|
|
|
% Compute transfer functions
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Voltage accros piezo stacks [V]
|
|
io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
|
|
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor stack voltage [V]
|
|
|
|
Gm = linearize(mdl, io);
|
|
Gm.InputName = {'Va'};
|
|
Gm.OutputName = {'y', 'Vs'};
|
|
|
|
%% Comparison of the identified transfer function from Va to di to the multi-body model
|
|
freqs = logspace(1, 3, 500);
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_y), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measured FRF');
|
|
plot(freqs, abs(squeeze(freqresp(Gm('y', 'Va'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', 'Model')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $y/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-8, 1e-5]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_y), '-', 'color' , [colors(2,:), 0.5], 'linewidth', 2.5);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm('y', 'Va'), freqs, 'Hz'))), '--', 'color', colors(2,:))
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360);
|
|
ylim([-45, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 1e3]);
|
|
|
|
%% Comparison of the identified transfer function from Va to Vs to the multi-body model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_Vs), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', 'Measured FRF');
|
|
plot(freqs, abs(squeeze(freqresp(Gm('Vs', 'Va'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', 'Model')
|
|
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-3, 1e1]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_Vs), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm('Vs', 'Va'), freqs, 'Hz'))), '--', 'color', colors(1,:))
|
|
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, 1e3]);
|
|
|
|
%% Integral Force Feedback Controller
|
|
K_iff = (1/(s + 2*2*pi))*(s/(s + 0.5*2*pi));
|
|
K_iff.inputname = {'Vs'};
|
|
K_iff.outputname = {'u_iff'};
|
|
|
|
% New damped plant input
|
|
S1 = sumblk("u = u_iff + u_damp");
|
|
|
|
% Voltage amplifier with gain of 20
|
|
voltage_amplifier = tf(20);
|
|
voltage_amplifier.inputname = {'u'};
|
|
voltage_amplifier.outputname = {'Va'};
|
|
|
|
%% Load experimental data with IFF implemented with different gains
|
|
load('apa95ml_iff_test.mat', 'results');
|
|
|
|
% Tested gains
|
|
g_iff = [0, 10, 50, 100, 500, 1000];
|
|
|
|
% Spectral Analysis parameters
|
|
Ts = t(end)/(length(t)-1);
|
|
Nfft = floor(1/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Computed the identified FRF of the damped plants
|
|
tf_iff = {zeros(1, length(g_iff))};
|
|
for i=1:length(g_iff)
|
|
[tf_est, f] = tfestimate(results{i}.u, results{i}.y, win, Noverlap, Nfft, 1/Ts);
|
|
tf_iff(i) = {tf_est};
|
|
end
|
|
|
|
%% Estimate the damped plants from the multi-body model
|
|
Gm_iff = {zeros(1, length(g_iff))};
|
|
|
|
for i=1:length(g_iff)
|
|
K_iff_g = -K_iff*g_iff(i); K_iff_g.inputname = {'Vs'}; K_iff_g.outputname = {'u_iff'};
|
|
Gm_iff(i) = {connect(Gm, K_iff_g, S1, voltage_amplifier, {'u_damp'}, {'y'})};
|
|
end
|
|
|
|
%% Identify second order plants from the experimental data
|
|
% This is mandatory to estimate the experimental "poles"
|
|
% an place them in the root-locus plot
|
|
G_id = {zeros(1,length(results))};
|
|
|
|
f_start = 70; % [Hz]
|
|
f_end = 500; % [Hz]
|
|
|
|
for i = 1:length(results)
|
|
tf_id = tf_iff{i}(sum(f<f_start):length(f)-sum(f>f_end));
|
|
f_id = f(sum(f<f_start):length(f)-sum(f>f_end));
|
|
|
|
gfr = idfrd(tf_id, 2*pi*f_id, Ts);
|
|
G_id(i) = {procest(gfr,'P2UDZ')};
|
|
end
|
|
|
|
%% Comparison of the Root-Locus computed from the multi-body model and the identified closed-loop poles
|
|
gains = logspace(0, 5, 1000);
|
|
figure;
|
|
hold on;
|
|
plot(real( pole(Gm('Vs', 'Va'))), imag( pole(Gm('Vs', 'Va'))), 'kx', 'HandleVisibility', 'off');
|
|
plot(real(tzero(Gm('Vs', 'Va'))), imag(tzero(Gm('Vs', 'Va'))), 'ko', 'HandleVisibility', 'off');
|
|
for i = 1:length(gains)
|
|
cl_poles = pole(feedback(Gm('Vs', 'Va'), gains(i)*K_iff));
|
|
plot(real(cl_poles(imag(cl_poles)>100)), imag(cl_poles(imag(cl_poles)>100)), 'k.', 'HandleVisibility', 'off');
|
|
end
|
|
for i = 1:length(g_iff)
|
|
cl_poles = pole(Gm_iff{i});
|
|
plot(real(cl_poles(imag(cl_poles)>100)), imag(cl_poles(imag(cl_poles)>100)), '.', 'MarkerSize', 20, 'color', colors(i,:), 'HandleVisibility', 'off');
|
|
plot(real(pole(G_id{i})), imag(pole(G_id{i})), 'x', 'color', colors(i,:), 'DisplayName', sprintf('g = %0.f', g_iff(i)), 'DisplayName', sprintf('$g = %.0f$', g_iff(i)));
|
|
end
|
|
xlabel('Real Part');
|
|
ylabel('Imaginary Part');
|
|
axis equal;
|
|
ylim([-100, 2100]);
|
|
xlim([-2100,100]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
%% Experimental damped plant for several IFF gains and estimated damped plants from the model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2, 1]);
|
|
hold on;
|
|
plot(f, abs(tf_iff{1}), '-', 'DisplayName', '$g = 0$', 'color', [0,0,0, 0.5], 'linewidth', 2.5)
|
|
plot(f, abs(squeeze(freqresp(Gm_iff{1}, f, 'Hz'))), 'k--', 'HandleVisibility', 'off')
|
|
for i = 2:length(results)
|
|
plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)), 'color', [colors(i-1,:), 0.5], 'linewidth', 2.5)
|
|
end
|
|
for i = 2:length(results)
|
|
plot(f, abs(squeeze(freqresp(Gm_iff{i}, f, 'Hz'))), '--', 'color', colors(i-1,:), 'HandleVisibility', 'off')
|
|
end
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
ylabel('Amplitude $y/V_a$ [m/N]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-6, 2e-4]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(tf_iff{1}./squeeze(freqresp(exp(-s*2e-4), f, 'Hz'))), '-', 'color', [0,0,0, 0.5], 'linewidth', 2.5)
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gm_iff{1}, f, 'Hz'))), 'k--')
|
|
for i = 2:length(results)
|
|
plot(f, 180/pi*angle(tf_iff{i}./squeeze(freqresp(exp(-s*2e-4), f, 'Hz'))), '-', 'color', [colors(i-1,:), 0.5], 'linewidth', 2.5)
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gm_iff{i}, f, 'Hz'))), '--', 'color', colors(i-1,:))
|
|
end
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
hold off;
|
|
yticks(-360:45:360);
|
|
ylim([-10, 190]);
|
|
|
|
linkaxes([ax1,ax2], 'x');
|
|
xlim([150, 500]);
|