571 lines
22 KiB
Matlab
571 lines
22 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
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
% Hysteresis
|
|
% <<ssec:test_apa_hysteresis>>
|
|
|
|
% As the payload is vertically guided without friction, the hysteresis of the APA can be estimated from the motion of the payload.
|
|
|
|
% A quasi static sinusoidal excitation $V_a$ with an offset of $65\,V$ (halfway between $-20\,V$ and $150\,V$), and an amplitude varying from $4\,V$ up to $80\,V$.
|
|
|
|
% For each excitation amplitude, the vertical displacement $d_e$ of the mass is measured and displayed as a function of the applied voltage..
|
|
|
|
|
|
%% Load measured data - hysteresis
|
|
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'u', 'de');
|
|
|
|
% Initial time set to zero
|
|
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);
|
|
|
|
ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes
|
|
|
|
|
|
|
|
% The measured displacements as a function of the output voltages are shown in Figure ref:fig:test_apa_meas_hysteresis.
|
|
% It is interesting to see that the hysteresis is increasing with the excitation amplitude.
|
|
|
|
|
|
%% Measured displacement as a function of the output voltage
|
|
figure;
|
|
|
|
hold on;
|
|
for i = [6,5,4,2]
|
|
i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
|
|
plot(20*apa_hyst.u(i_lim), 1e6*detrend(apa_hyst.de(i_lim), 0), ...
|
|
'DisplayName', sprintf('$V_a = 65 + %.0f \\sin (\\omega t) \\ [V]$', 20*ampls(i)))
|
|
end
|
|
hold off;
|
|
xlabel('Stack Voltage $V_a$ [V]'); ylabel('Displacement $d_e$ [$\mu$m]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([-20, 150]);
|
|
ylim([-120, 120]);
|
|
|
|
% Axial stiffness
|
|
% <<ssec:test_apa_stiffness>>
|
|
|
|
% In order to estimate the stiffness of the APA, a weight with known mass $m_a = 6.4\,\text{kg}$ is added on top of the suspended granite and the deflection $d_e$ is measured using the encoder.
|
|
|
|
% The APA stiffness can then be estimated from equation eqref:eq:test_apa_stiffness.
|
|
|
|
% \begin{equation} \label{eq:test_apa_stiffness}
|
|
% k_{\text{apa}} = \frac{m_a g}{\Delta d_e}
|
|
% \end{equation}
|
|
|
|
|
|
%% Load data for stiffness measurement
|
|
apa_nums = [1 2 4 5 6 8];
|
|
apa_mass = {};
|
|
for i = 1:length(apa_nums)
|
|
apa_mass(i) = {load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', apa_nums(i)), 't', 'de')};
|
|
% The initial displacement is set to zero
|
|
apa_mass{i}.de = apa_mass{i}.de - mean(apa_mass{i}.de(apa_mass{i}.t<11));
|
|
end
|
|
|
|
added_mass = 6.4; % Added mass [kg]
|
|
|
|
|
|
|
|
% The measured displacement $d_e$ as a function of time is shown in Figure ref:fig:test_apa_meas_stiffness_time.
|
|
% It can be seen that there are some drifts in the measured displacement (probably due to piezoelectric creep) and the that displacement does not come back to the initial position after the mass is removed (probably due to piezoelectric hysteresis).
|
|
% These two effects induce some uncertainties in the measured stiffness.
|
|
|
|
|
|
%% Plot the deflection at a function of time
|
|
figure;
|
|
hold on;
|
|
plot(apa_mass{2}.t(1:100:end)-apa_mass{2}.t(1), 1e6*apa_mass{2}.de(1:100:end), 'k-');
|
|
plot([0,20], [-0.4, -0.4], 'k--', 'LineWidth', 0.5)
|
|
plot([0,20], [-4.5, -4.5], 'k--', 'LineWidth', 0.5)
|
|
plot([0,20], [-37.4, -37.4], 'k--', 'LineWidth', 0.5)
|
|
% first stroke for stiffness measurements
|
|
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
|
|
anArrow.Parent = gca;
|
|
anArrow.Position = [2, -0.4, 0, -37];
|
|
text(2.5, -20, sprintf('$d_1$'), 'horizontalalignment', 'left');
|
|
|
|
% second stroke for stiffness measurements
|
|
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
|
|
anArrow.Parent = gca;
|
|
anArrow.Position = [18, -37.4, 0, 32.9];
|
|
text(18.5, -20, sprintf('$d_2$'), 'horizontalalignment', 'left');
|
|
|
|
% annotation('textarrow',[],y,'String',' Growth ','FontSize',13,'Linewidth',2)
|
|
hold off;
|
|
xlabel('Time [s]'); ylabel('Displacement $d_e$ [$\mu$m]');
|
|
|
|
|
|
|
|
% #+name: tab:test_apa_measured_stiffnesses
|
|
% #+caption: Measured stiffnesses (in $N/\mu m$)
|
|
% #+attr_latex: :environment tabularx :width 0.2\linewidth :align ccc
|
|
% #+attr_latex: :center t :booktabs t :float t
|
|
% #+RESULTS:
|
|
% | APA | $k_1$ | $k_2$ |
|
|
% |-----+-------+-------|
|
|
% | 1 | 1.68 | 1.9 |
|
|
% | 2 | 1.69 | 1.9 |
|
|
% | 4 | 1.7 | 1.91 |
|
|
% | 5 | 1.7 | 1.93 |
|
|
% | 6 | 1.7 | 1.92 |
|
|
% | 8 | 1.73 | 1.98 |
|
|
|
|
% The stiffness can also be computed using equation eqref:eq:test_apa_res_freq by knowing the main vertical resonance frequency $\omega_z \approx 95\,\text{Hz}$ (estimated by the dynamical measurements shown in section ref:ssec:test_apa_meas_dynamics) and the suspended mass $m_{\text{sus}} = 5.7\,\text{kg}$.
|
|
|
|
% \begin{equation} \label{eq:test_apa_res_freq}
|
|
% \omega_z = \sqrt{\frac{k}{m_{\text{sus}}}}
|
|
% \end{equation}
|
|
|
|
% The obtain stiffness is $k \approx 2\,N/\mu m$ which is close to the values found in the documentation and by the "static deflection" method.
|
|
|
|
|
|
% However, changes in the electrical impedance connected to the piezoelectric stacks impacts the mechanical compliance (or stiffness) of the piezoelectric stack [[cite:&reza06_piezoel_trans_vibrat_contr_dampin chap. 2]].
|
|
|
|
% To estimate this effect, the stiffness of the APA if measured using the "static deflection" method in two cases:
|
|
% - $k_{\text{os}}$: piezoelectric stacks left unconnected (or connect to the high impedance ADC)
|
|
% - $k_{\text{sc}}$: piezoelectric stacks short circuited (or connected to the voltage amplifier with small output impedance)
|
|
|
|
% The open-circuit stiffness is estimated at $k_{\text{oc}} \approx 2.3\,N/\mu m$ and the closed-circuit stiffness $k_{\text{sc}} \approx 1.7\,N/\mu m$.
|
|
|
|
|
|
%% Load Data
|
|
add_mass_oc = load('frf_data_1_add_mass_open_circuit.mat', 't', 'de');
|
|
add_mass_cc = load('frf_data_1_add_mass_closed_circuit.mat', 't', 'de');
|
|
|
|
%% Zero displacement at initial time
|
|
add_mass_oc.de = add_mass_oc.de - mean(add_mass_oc.de(add_mass_oc.t<11));
|
|
add_mass_cc.de = add_mass_cc.de - mean(add_mass_cc.de(add_mass_cc.t<11));
|
|
|
|
%% Estimation of the stiffness in Open Circuit and Closed-Circuit
|
|
apa_k_oc = 9.8 * added_mass / (mean(add_mass_oc.de(add_mass_oc.t > 12 & add_mass_oc.t < 12.5)) - mean(add_mass_oc.de(add_mass_oc.t > 20 & add_mass_oc.t < 20.5)));
|
|
apa_k_sc = 9.8 * added_mass / (mean(add_mass_cc.de(add_mass_cc.t > 12 & add_mass_cc.t < 12.5)) - mean(add_mass_cc.de(add_mass_cc.t > 20 & add_mass_cc.t < 20.5)));
|
|
|
|
% Dynamics
|
|
% <<ssec:test_apa_meas_dynamics>>
|
|
|
|
% In this section, the dynamics of the system from the excitation voltage $u$ to encoder measured displacement $d_e$ and to the force sensor voltage $V_s$ is identified.
|
|
|
|
|
|
%% Identification using sweep sine (low frequency)
|
|
load('frf_data_sweep.mat');
|
|
load('frf_data_noise_hf.mat');
|
|
|
|
%% Sampling Frequency
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Fs = 1/Ts; % Sampling Frequency [Hz]
|
|
|
|
%% "Hanning" windows used for the spectral analysis:
|
|
Nfft = floor(2/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Separation of frequencies: low freqs using sweep sine, and high freq using noise
|
|
% Only used to have the frequency vector "f"
|
|
[~, f] = tfestimate(apa_sweep{1}.u, apa_sweep{1}.de, win, Noverlap, Nfft, 1/Ts);
|
|
i_lf = f <= 350;
|
|
i_hf = f > 350;
|
|
|
|
%% FRF estimation of the transfer function from u to de
|
|
enc_frf = zeros(length(f), length(apa_nums));
|
|
for i = 1:length(apa_nums)
|
|
[frf_lf, ~] = tfestimate(apa_sweep{i}.u, apa_sweep{i}.de, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.de, win, Noverlap, Nfft, 1/Ts);
|
|
enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
%% FRF estimation of the transfer function from u to Vs
|
|
iff_frf = zeros(length(f), length(apa_nums));
|
|
for i = 1:length(apa_nums)
|
|
[frf_lf, ~] = tfestimate(apa_sweep{i}.u, apa_sweep{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
%% Save the identified dynamics for further analysis
|
|
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
|
|
|
|
|
|
|
|
% The obtained transfer functions for the 6 APA between the excitation voltage $u$ and the encoder displacement $d_e$ are shown in Figure ref:fig:test_apa_frf_encoder.
|
|
% The obtained transfer functions are close to a mass-spring-damper system.
|
|
% The following can be observed:
|
|
% - A "stiffness line" indicating a static gain equal to $\approx -17\,\mu m/V$.
|
|
% The minus sign comes from the fact that an increase in voltage stretches the piezoelectric stack that then reduces the height of the APA
|
|
% - A lightly damped resonance at $95\,\text{Hz}$
|
|
% - A "mass line" up to $\approx 800\,\text{Hz}$, above which some resonances appear. These additional resonances might be coming from the limited stiffness of the encoder support or from the limited compliance of the APA support.
|
|
|
|
|
|
%% Plot the FRF from u to de
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:length(apa_nums)
|
|
plot(f, abs(enc_frf(:, i)), ...
|
|
'DisplayName', sprintf('APA %i', apa_nums(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-8, 1e-3]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(apa_nums)
|
|
plot(f, 180/pi*angle(enc_frf(:, i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([10, 2e3]);
|
|
|
|
|
|
|
|
% #+name: fig:test_apa_frf_encoder
|
|
% #+caption: Estimated Frequency Response Function from generated voltage $u$ to the encoder displacement $d_e$ for the 6 APA300ML
|
|
% #+RESULTS:
|
|
% [[file:figs/test_apa_frf_encoder.png]]
|
|
|
|
% The dynamics from $u$ to the measured voltage across the sensor stack $V_s$ is also identified and shown in Figure ref:fig:test_apa_frf_force.
|
|
|
|
% A lightly damped resonance is observed at $95\,\text{Hz}$ and a lightly damped anti-resonance at $41\,\text{Hz}$.
|
|
% No additional resonances is present up to at least $2\,\text{kHz}$ indicating at Integral Force Feedback can be applied without stability issues from high frequency flexible modes.
|
|
|
|
% As illustrated by the Root Locus, the poles of the closed-loop system converges to the zeros of the open-loop plant.
|
|
% Suppose that a controller with a very high gain is implemented such that the voltage $V_s$ across the sensor stack is zero.
|
|
% In that case, because of the very high controller gain, no stress and strain is present on the sensor stack (and on the actuator stacks are well, as they are both in series).
|
|
% Such closed-loop system would therefore virtually corresponds to a system for which the piezoelectric stacks have been removed and just the mechanical shell is kept.
|
|
% From this analysis, the axial stiffness of the shell can be estimated to be $k_{\text{shell}} = 5.7 \cdot (2\pi \cdot 41)^2 = 0.38\,N/\mu m$.
|
|
% # TODO - Compare with FEM result
|
|
|
|
% Such reasoning can lead to very interesting insight into the system just from an open-loop identification.
|
|
|
|
|
|
%% Plot the FRF from u to Vs
|
|
figure;
|
|
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile;
|
|
hold on;
|
|
for i = 1:length(apa_nums)
|
|
plot(f, abs(iff_frf(:, i)), ...
|
|
'DisplayName', sprintf('APA %i', apa_nums(i)));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-2, 1e2]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:length(apa_nums)
|
|
plot(f, 180/pi*angle(iff_frf(:, i)));
|
|
end
|
|
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]);
|
|
|
|
% Effect of the resistor on the IFF Plant
|
|
% <<ssec:test_apa_resistance_sensor_stack>>
|
|
|
|
% A resistor $R \approx 80.6\,k\Omega$ is added in parallel with the sensor stack which has the effect to form a high pass filter with the capacitance of the stack.
|
|
|
|
% As explain before, this is done for two reasons:
|
|
% 1. Limit the voltage offset due to the input bias current of the ADC
|
|
% 2. Limit the low frequency gain
|
|
|
|
% The (low frequency) transfer function from $u$ to $V_s$ with and without this resistor have been measured and are compared in Figure ref:fig:test_apa_effect_resistance.
|
|
% It is confirmed that the added resistor as the effect of adding an high pass filter with a cut-off frequency of $\approx 0.35\,\text{Hz}$.
|
|
|
|
|
|
%% Load the data
|
|
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'u'); % With the resistor
|
|
wo_k = load('frf_data_1_sweep_lf.mat', 't', 'Vs', 'u'); % Without the resistor
|
|
|
|
%% Large Hanning window for good low frequency estimate
|
|
Nfft = floor(50/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Compute the transfer functions from u to Vs
|
|
[frf_wo_k, f] = tfestimate(wo_k.u, wo_k.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_wi_k, ~] = tfestimate(wi_k.u, wi_k.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Model for the high pass filter
|
|
C = 5.1e-6; % Sensor Stack capacitance [F]
|
|
R = 80.6e3; % Parallel Resistor [Ohm]
|
|
|
|
f0 = 1/(2*pi*R*C); % Crossover frequency of RC HPF [Hz]
|
|
|
|
G_hpf = 0.6*(s/2*pi*f0)/(1 + s/2*pi*f0);
|
|
|
|
%% Compare the HPF model and the measured FRF
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
|
|
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([1e-1, 1e0]);
|
|
legend('location', 'southeast')
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(frf_wo_k));
|
|
plot(f, 180/pi*angle(frf_wi_k));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360); ylim([-45, 90]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([0.2, 8]);
|
|
|
|
% Integral Force Feedback
|
|
% <<ssec:test_apa_iff_locus>>
|
|
|
|
% This test bench can also be used to estimate the damping added by the implementation of an Integral Force Feedback strategy.
|
|
|
|
|
|
%% Load identification Data
|
|
data = load("2023-03-17_11-28_iff_plant.mat");
|
|
|
|
%% Spectral Analysis setup
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(5/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Compute the transfer function from applied force to measured rotation
|
|
[G_iff, f] = tfestimate(data.id_plant, data.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
|
|
|
|
% First, the transfer function eqref:eq:test_apa_iff_manual_fit is manually tuned to match the identified dynamics from generated voltage $u$ to the measured sensor stack voltage $V_s$ in Section ref:ssec:test_apa_meas_dynamics.
|
|
|
|
% The obtained parameter values are $\omega_{\textsc{hpf}} = 0.4\, \text{Hz}$, $\omega_{z} = 42.7\, \text{Hz}$, $\xi_{z} = 0.4\,\%$, $\omega_{p} = 95.2\, \text{Hz}$, $\xi_{p} = 2\,\%$ and $g_0 = 0.64$.
|
|
|
|
% \begin{equation} \label{eq:test_apa_iff_manual_fit}
|
|
% G_{\textsc{iff},m}(s) = g_0 \cdot \frac{1 + 2 \xi_z \frac{s}{\omega_z} + \frac{s^2}{\omega_z^2}}{1 + 2 \xi_p \frac{s}{\omega_p} + \frac{s^2}{\omega_p^2}} \cdot \frac{s}{\omega_{\textsc{hpf}} + s}
|
|
% \end{equation}
|
|
|
|
% The comparison between the identified plant and the manually tuned transfer function is done in Figure ref:fig:test_apa_iff_plant_comp_manual_fit.
|
|
|
|
|
|
%% Basic manually tuned model
|
|
w0z = 2*pi*42.7; % Zero frequency
|
|
xiz = 0.004; % Zero damping
|
|
|
|
w0p = 2*pi*95.2; % Pole frequency
|
|
xip = 0.02; % Pole damping
|
|
|
|
G_iff_model = exp(-2*s*Ts)*0.64*(1 + 2*xiz/w0z*s + s^2/w0z^2)/(1 + 2*xip/w0p*s + s^2/w0p^2)*(s/(s+2*pi*0.4));
|
|
|
|
%% Identified IFF plant and manually tuned model of the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_iff), 'color', colors(2,:), 'DisplayName', 'Identified plant')
|
|
plot(f, abs(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--', 'DisplayName', 'Manual fit')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_iff), 'color', colors(2,:));
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360);
|
|
ylim([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([0.2, 1e3]);
|
|
|
|
|
|
|
|
% #+name: fig:test_apa_iff_plant_comp_manual_fit
|
|
% #+caption: Identified IFF plant and manually tuned model of the plant (a time delay of $200\,\mu s$ is added to the model of the plant to better match the identified phase)
|
|
% #+RESULTS:
|
|
% [[file:figs/test_apa_iff_plant_comp_manual_fit.png]]
|
|
|
|
% The implemented Integral Force Feedback Controller transfer function is shown in equation eqref:eq:test_apa_Kiff_formula.
|
|
% It contains an high pass filter (cut-off frequency of $2\,\text{Hz}$) to limit the low frequency gain, a low pass filter to add integral action above $20\,\text{Hz}$, a second low pass filter to add robustness to high frequency resonances and a tunable gain $g$.
|
|
|
|
% \begin{equation} \label{eq:test_apa_Kiff_formula}
|
|
% K_{\textsc{iff}}(s) = -10 \cdot g \cdot \frac{s}{s + 2\pi \cdot 2} \cdot \frac{1}{1 + 2\pi \cdot 20} \cdot \frac{1}{s + 2\pi\cdot 2000}
|
|
% \end{equation}
|
|
|
|
|
|
%% Integral Force Feedback Controller
|
|
K_iff = -10*(1/(s + 2*pi*20)) * ... % LPF: provides integral action above 20Hz
|
|
(s/(s + 2*pi*2)) * ... % HPF: limit low frequency gain
|
|
(1/(1 + s/2/pi/2e3)); % LPF: more robust to high frequency resonances
|
|
|
|
|
|
|
|
% To estimate how the dynamics of the APA changes when the Integral Force Feedback controller is implemented, the test bench shown in Figure ref:fig:test_apa_iff_schematic is used.
|
|
% The transfer function from the "damped" plant input $u\prime$ to the encoder displacement $d_e$ is identified for several IFF controller gains $g$.
|
|
|
|
% #+name: fig:test_apa_iff_schematic
|
|
% #+caption: Figure caption
|
|
% [[file:figs/test_apa_iff_schematic.png]]
|
|
|
|
|
|
%% Load Data
|
|
data = load("2023-03-17_14-10_damped_plants_new.mat");
|
|
|
|
%% Spectral Analysis setup
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(1/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Get the frequency vector
|
|
[~, f] = tfestimate(data.data(1).id_plant(1:end), data.data(1).dL(1:end), win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Gains used for analysis are between 1 and 50
|
|
i_kept = [5:10]
|
|
|
|
%% Identify the damped plant from u' to de for different IFF gains
|
|
G_dL_frf = {zeros(1,length(i_kept))};
|
|
|
|
for i = 1:length(i_kept)
|
|
[G_dL, ~] = tfestimate(data.data(i_kept(i)).id_plant(1:end), data.data(i_kept(i)).dL(1:end), win, Noverlap, Nfft, 1/Ts);
|
|
G_dL_frf(i) = {G_dL};
|
|
end
|
|
|
|
|
|
|
|
% The identified dynamics are then fitted by second order transfer functions.
|
|
% The comparison between the identified damped dynamics and the fitted second order transfer functions is done in Figure ref:fig:test_apa_identified_damped_plants for different gains $g$.
|
|
% It is clear that large amount of damping is added when the gain is increased and that the frequency of the pole is shifted to lower frequencies.
|
|
|
|
|
|
%% Fit the data with 2nd order transfer function using vectfit3
|
|
opts = struct();
|
|
|
|
opts.stable = 1; % Enforce stable poles
|
|
opts.asymp = 1; % Force D matrix to be null
|
|
opts.relax = 1; % Use vector fitting with relaxed non-triviality constraint
|
|
opts.skip_pole = 0; % Do NOT skip pole identification
|
|
opts.skip_res = 0; % Do NOT skip identification of residues (C,D,E)
|
|
opts.cmplx_ss = 0; % Create real state space model with block diagonal A
|
|
|
|
opts.spy1 = 0; % No plotting for first stage of vector fitting
|
|
opts.spy2 = 0; % Create magnitude plot for fitting of f(s)
|
|
|
|
Niter = 100; % Number of iteration.
|
|
N = 2; % Order of approximation
|
|
poles = [-25 - 1i*60, -25 + 1i*60]; % First get for the pole location
|
|
|
|
G_dL_id = {zeros(1,length(i_kept))};
|
|
|
|
% Identification just between two frequencies
|
|
f_keep = (f>5 & f<200);
|
|
|
|
for i = 1:length(i_kept)
|
|
%% Estimate resonance frequency and damping
|
|
for iter = 1:Niter
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_dL_frf{i}(f_keep).', 1i*2*pi*f(f_keep)', poles, ones(size(f(f_keep)))', opts);
|
|
end
|
|
G_dL_id(i) = {ss(G_est.A, G_est.B, G_est.C, G_est.D)};
|
|
end
|
|
|
|
%% Identified dynamics from u' to de for different IFF gains
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
for i = 1:length(i_kept)
|
|
plot(f, abs(G_dL_frf{i}), 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %.0f', data.gains(i_kept(i))))
|
|
plot(f, abs(squeeze(freqresp(G_dL_id{i}, f, 'Hz'))), '--', 'color', [colors(i,:), 1], 'HandleVisibility', 'off')
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_L/V_a$ [m/V]');
|
|
xlim([10, 1e3]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
|
|
|
|
% #+name: fig:test_apa_identified_damped_plants
|
|
% #+caption: Identified dynamics (solid lines) and fitted transfer functions (dashed lines) from $u\prime$ to $d_e$ for different IFF gains
|
|
% #+RESULTS:
|
|
% [[file:figs/test_apa_identified_damped_plants.png]]
|
|
|
|
% The evolution of the pole in the complex plane as a function of the controller gain $g$ (i.e. the "root locus") is computed:
|
|
% - using the IFF plant model eqref:eq:test_apa_iff_manual_fit and the implemented controller eqref:eq:test_apa_Kiff_formula
|
|
% - from the fitted transfer functions of the damped plants experimentally identified for several controller gains
|
|
|
|
% The two obtained root loci are compared in Figure ref:fig:test_apa_iff_root_locus and are in good agreement considering that the damped plants were only fitted using a second order transfer function.
|
|
|
|
|
|
%% Root Locus of the APA300ML with Integral Force Feedback
|
|
% Comparison between the computed root locus from the plant model and the root locus estimated from the damped plant pole identification
|
|
gains = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
G_iff_poles = pole(pade(G_iff_model));
|
|
i = imag(G_iff_poles) > 100; % Only keep relevant poles
|
|
plot(real(G_iff_poles(i)), imag(G_iff_poles(i)), 'kx', ...
|
|
'DisplayName', '$g = 0$');
|
|
G_iff_zeros = tzero(G_iff_model);
|
|
i = imag(G_iff_zeros) > 100; % Only keep relevant zeros
|
|
plot(real(G_iff_zeros(i)), imag(G_iff_zeros(i)), 'ko', ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(pade(G_iff_model), g*K_iff, 1));
|
|
i = imag(clpoles) > 100; % Only keep relevant poles
|
|
plot(real(clpoles(i)), imag(clpoles(i)), 'k.', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
for i = 1:length(i_kept)
|
|
plot(real(pole(G_dL_id{i})), imag(pole(G_dL_id{i})), 'x', 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %1.f', data.gains(i_kept(i))));
|
|
end
|
|
ylim([0, 700]);
|
|
xlim([-600,100]);
|
|
xlabel('Real Part')
|
|
ylabel('Imaginary Part')
|
|
axis square
|
|
legend('location', 'northwest');
|