626 lines
27 KiB
Matlab
626 lines
27 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>>
|
|
|
|
% Because the payload is vertically guided without friction, the hysteresis of the APA can be estimated from the motion of the payload.
|
|
% A quasi static[fn:9] sinusoidal excitation $V_a$ with an offset of $65\,V$ (halfway between $-20\,V$ and $150\,V$) and with an amplitude varying from $4\,V$ up to $80\,V$ is generated using the DAC.
|
|
% For each excitation amplitude, the vertical displacement $d_e$ of the mass is measured and displayed as a function of the applied voltage in Figure ref:fig:test_apa_meas_hysteresis.
|
|
% This is the typical behavior expected from a PZT stack actuator, where the hysteresis increases as a function of the applied voltage amplitude [[cite:&fleming14_desig_model_contr_nanop_system chap. 1.4]].
|
|
|
|
|
|
%% 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
|
|
|
|
%% 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>>
|
|
|
|
% 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 $\Delta d_e$ is measured using the encoder.
|
|
% The APA stiffness can then be estimated from equation eqref:eq:test_apa_stiffness, with $g \approx 9.8\,m/s^2$ the acceleration of gravity.
|
|
|
|
% \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 that the displacement does not return to the initial position after the mass is removed (probably due to piezoelectric hysteresis).
|
|
% These two effects induce some uncertainties in the measured stiffness.
|
|
|
|
% The stiffnesses are computed for all APAs from the two displacements $d_1$ and $d_2$ (see Figure ref:fig:test_apa_meas_stiffness_time) leading to two stiffness estimations $k_1$ and $k_2$.
|
|
% These estimated stiffnesses are summarized in Table ref:tab:test_apa_measured_stiffnesses and are found to be close to the specified nominal stiffness of the APA300ML $k = 1.8\,N/\mu m$.
|
|
|
|
|
|
%% 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]');
|
|
|
|
|
|
|
|
% #+attr_latex: :options [b]{0.57\linewidth}
|
|
% #+begin_minipage
|
|
% #+name: fig:test_apa_meas_stiffness_time
|
|
% #+caption: Measured displacement when adding (at $t \approx 3\,s$) and removing (at $t \approx 13\,s$) the mass
|
|
% #+attr_latex: :width 0.9\linewidth :float nil
|
|
% [[file:figs/test_apa_meas_stiffness_time.png]]
|
|
% #+end_minipage
|
|
% \hfill
|
|
% #+attr_latex: :options [b]{0.37\linewidth}
|
|
% #+begin_minipage
|
|
% #+name: tab:test_apa_measured_stiffnesses
|
|
% #+caption: Measured axial stiffnesses (in $N/\mu m$)
|
|
% #+attr_latex: :environment tabularx :width 0.6\linewidth :align Xcc
|
|
% #+attr_latex: :center t :booktabs t :float nil
|
|
% #+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 |
|
|
% #+end_minipage
|
|
|
|
% 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 obtained stiffness is $k \approx 2\,N/\mu m$ which is close to the values found in the documentation and using the "static deflection" method.
|
|
|
|
|
|
% It is important to note that changes to the electrical impedance connected to the piezoelectric stacks affect the mechanical compliance (or stiffness) of the piezoelectric stack [[cite:&reza06_piezoel_trans_vibrat_contr_dampin chap. 2]].
|
|
|
|
% To estimate this effect for the APA300ML, its stiffness is estimated 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$ while 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>>
|
|
|
|
|
|
%% 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');
|
|
|
|
|
|
|
|
% In this section, the dynamics from the excitation voltage $u$ to the encoder measured displacement $d_e$ and to the force sensor voltage $V_s$ is identified.
|
|
|
|
% First, the dynamics from $u$ to $d_e$ for the six APA300ML are compared in Figure ref:fig:test_apa_frf_encoder.
|
|
% The obtained frequency response functions are similar to those of a (second order) mass-spring-damper system with:
|
|
% - A "stiffness line" indicating a static gain equal to $\approx -17\,\mu m/V$.
|
|
% The negative sign comes from the fact that an increase in voltage stretches the piezoelectric stack which 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 additional resonances appear. These additional resonances might be due to the limited stiffness of the encoder support or from the limited compliance of the APA support.
|
|
% The flexible modes studied in section ref:ssec:test_apa_spurious_resonances seem not to impact the measured axial motion of the actuator.
|
|
|
|
% The dynamics from $u$ to the measured voltage across the sensor stack $V_s$ for the six APA300ML are compared in Figure ref:fig:test_apa_frf_force.
|
|
|
|
% A lightly damped resonance (pole) is observed at $95\,\text{Hz}$ and a lightly damped anti-resonance (zero) at $41\,\text{Hz}$.
|
|
% No additional resonances are present up to at least $2\,\text{kHz}$ indicating that Integral Force Feedback can be applied without stability issues from high-frequency flexible modes.
|
|
% The zero at $41\,\text{Hz}$ seems to be non-minimum phase (the phase /decreases/ by 180 degrees whereas it should have /increased/ by 180 degrees for a minimum phase zero).
|
|
% This is investigated in Section ref:ssec:test_apa_non_minimum_phase.
|
|
|
|
% As illustrated by the Root Locus plot, the poles of the /closed-loop/ system converges to the zeros of the /open-loop/ plant as the feedback gain increases.
|
|
% The significance of this behavior varies with the type of sensor used, as explained in [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition chap. 7.6]].
|
|
% Considering the transfer function from $u$ to $V_s$, if a controller with a very high gain is applied such that the sensor stack voltage $V_s$ is kept at zero, the sensor (and by extension, the actuator stacks since they are in series) experiences negligible stress and strain.
|
|
% Consequently, the closed-loop system virtually corresponds to one in which the piezoelectric stacks are absent, leaving only the mechanical shell.
|
|
% From this analysis, it can be inferred that the axial stiffness of the shell is $k_{\text{shell}} = m \omega_0^2 = 5.7 \cdot (2\pi \cdot 41)^2 = 0.38\,N/\mu m$ (which is close to what is found using a finite element model).
|
|
|
|
% All the identified dynamics of the six APA300ML (both when looking at the encoder in Figure ref:fig:test_apa_frf_encoder and at the force sensor in Figure ref:fig:test_apa_frf_force) are almost identical, indicating good manufacturing repeatability for the piezoelectric stacks and the mechanical shell.
|
|
|
|
|
|
%% 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', 'southwest', '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]);
|
|
|
|
%% 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]);
|
|
|
|
% Non Minimum Phase Zero?
|
|
% <<ssec:test_apa_non_minimum_phase>>
|
|
|
|
% It was surprising to observe a non-minimum phase zero on the transfer function from $u$ to $V_s$ (Figure ref:fig:test_apa_frf_force).
|
|
% It was initially thought that this non-minimum phase behavior was an artifact arising from the measurement.
|
|
% A longer measurement was performed using different excitation signals (noise, slow sine sweep, etc.) to determine if the phase behavior of the zero changes (Figure ref:fig:test_apa_non_minimum_phase).
|
|
% The coherence (Figure ref:fig:test_apa_non_minimum_phase_coherence) is good even in the vicinity of the lightly damped zero, and the phase (Figure ref:fig:test_apa_non_minimum_phase_zoom) clearly indicates non-minimum phase behavior.
|
|
|
|
% Such non-minimum phase zero when using load cells has also been observed on other mechanical systems [[cite:&spanos95_soft_activ_vibrat_isolat;&thayer02_six_axis_vibrat_isolat_system;&hauge04_sensor_contr_space_based_six]].
|
|
% It could be induced to small non-linearity in the system, but the reason for this non-minimum phase for the APA300ML is not yet clear.
|
|
|
|
% However, this is not so important here because the zero is lightly damped (i.e. very close to the imaginary axis), and the closed loop poles (see the Root Locus plot in Figure ref:fig:test_apa_iff_root_locus) should not be unstable, except for very large controller gains that will never be applied in practice.
|
|
|
|
|
|
%% Long measurement
|
|
long_noise = load('frf_struts_align_3_noise_long.mat', 't', 'u', 'Vs');
|
|
|
|
% Long window for fine frequency axis
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(10/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
% Transfer function estimation
|
|
[frf_noise, f] = tfestimate(long_noise.u, long_noise.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[coh_noise, ~] = mscohere(long_noise.u, long_noise.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Bode plot of the FRF from u to de
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
nexttile();
|
|
hold on;
|
|
plot(f, coh_noise, '.-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
hold off;
|
|
xlim([38, 45]);
|
|
ylim([0, 1]);
|
|
|
|
%% Bode plot of the FRF from u to de
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(frf_noise), '.-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(frf_noise), '.-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([38, 45]);
|
|
|
|
% 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 forms a high-pass filter with the capacitance of the piezoelectric stack (capacitance estimated at $\approx 5\,\mu F$).
|
|
|
|
% As explained before, this is done to limit the voltage offset due to the input bias current of the ADC as well as to limit the low frequency gain.
|
|
|
|
% The (low frequency) transfer function from $u$ to $V_s$ with and without this resistor were measured and compared in Figure ref:fig:test_apa_effect_resistance.
|
|
% It is confirmed that the added resistor has the effect of adding a high-pass filter with a cut-off frequency of $\approx 0.39\,\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(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
|
|
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
|
|
plot(f, abs(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', 'RC model');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([2e-1, 1e0]);
|
|
yticks([0.2, 0.5, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(frf_wo_k));
|
|
plot(f, 180/pi*angle(frf_wi_k));
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', 'RC');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360); ylim([-5, 60]);
|
|
yticks([0, 15, 30, 45, 60]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([0.2, 8]);
|
|
xticks([0.2, 0.5, 1, 2, 5]);
|
|
|
|
% Integral Force Feedback
|
|
% <<ssec:test_apa_iff_locus>>
|
|
|
|
|
|
%% 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);
|
|
|
|
|
|
|
|
% To implement the Integral Force Feedback strategy, the measured frequency response function from $u$ to $V_s$ (Figure ref:fig:test_apa_frf_force) is fitted using the transfer function shown in equation eqref:eq:test_apa_iff_manual_fit.
|
|
% The parameters were manually tuned, and the obtained 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}
|
|
|
|
% A comparison between the identified plant and the manually tuned transfer function is shown 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). Note that a minimum-phase zero is identified here even though the coherence is not good around the frequency of the zero.
|
|
% #+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 a 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}{s + 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: Implementation of Integral Force Feedback in the Speedgoat. The damped plant has a new input $u\prime$
|
|
% [[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 were then fitted by second order transfer functions[fn:10].
|
|
% A comparison between the identified damped dynamics and the fitted second-order transfer functions is shown in Figure ref:fig:test_apa_identified_damped_plants for different gains $g$.
|
|
% It is clear that a large amount of damping is added when the gain is increased and that the frequency of the pole is shifted to lower frequencies.
|
|
|
|
% The evolution of the pole in the complex plane as a function of the controller gain $g$ (i.e. the "root locus") is computed in two cases.
|
|
% First using the IFF plant model eqref:eq:test_apa_iff_manual_fit and the implemented controller eqref:eq:test_apa_Kiff_formula.
|
|
% Second using 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 fitted using only a second-order transfer function.
|
|
|
|
|
|
%% 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_e/u^\prime$ [m/V]');
|
|
xlim([10, 1e3]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
%% 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
|
|
xlabel('Real Part')
|
|
ylabel('Imaginary Part')
|
|
axis equal
|
|
ylim([0, 610]);
|
|
xlim([-300,0]);
|
|
legend('location', 'southwest', 'FontSize', 8);
|