153 lines
6.4 KiB
Matlab
153 lines
6.4 KiB
Matlab
%% 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
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Simscape model name
|
|
mdl = 'rotating_model';
|
|
|
|
%% Load "Generic" system dynamics
|
|
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
|
|
|
|
% Effect of the rotation speed on the IFF plant dynamics
|
|
% The transfer functions from actuator forces $[F_u,\ F_v]$ to the measured force sensors $[f_u,\ f_v]$ are identified for several rotating velocities and shown in Figure ref:fig:rotating_iff_bode_plot_effect_rot.
|
|
|
|
% As was expected from the derived equations of motion:
|
|
% - when $0 < \Omega < \omega_0$: the low frequency gain is no longer zero and two (non-minimum phase) real zero appears at low frequency.
|
|
% The low frequency gain increases with $\Omega$.
|
|
% A pair of (minimum phase) complex conjugate zeros appears between the two complex conjugate poles that are split further apart as $\Omega$ increases.
|
|
% - when $\omega_0 < \Omega$: the low frequency pole becomes unstable.
|
|
|
|
|
|
%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
|
|
figure;
|
|
freqs = logspace(-2, 1, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
% Magnitude
|
|
ax1 = nexttile([2, 1]);
|
|
hold on;
|
|
for i = 1:length(Wzs)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
|
|
title('Direct terms: $f_u/F_u$, $f_v/F_v$');
|
|
ylim([1e-3, 1e2]);
|
|
|
|
ax2 = nexttile([2, 1]);
|
|
hold on;
|
|
for i = 1:length(Wzs)
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('fv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0$', Wzs(i)))
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
title('Coupling Terms: $f_u/F_v$, $f_v/F_u$');
|
|
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
ldg.ItemTokenSize = [10, 1];
|
|
ylim([1e-3, 1e2]);
|
|
|
|
ax3 = nexttile;
|
|
hold on;
|
|
for i = 1:length(Wzs)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
|
yticks(-180:90:180);
|
|
ylim([-180 180]);
|
|
xticks([1e-2,1e-1,1,1e1])
|
|
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
|
|
|
ax4 = nexttile;
|
|
hold on;
|
|
for i = 1:length(Wzs)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('fv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [rad/s]'); set(gca, 'YTickLabel',[]);
|
|
yticks(-180:90:180);
|
|
ylim([-180 180]);
|
|
xticks([1e-2,1e-1,1,1e1])
|
|
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
linkaxes([ax1,ax2],'y');
|
|
|
|
|
|
|
|
% #+name: fig:rotating_iff_diagram
|
|
% #+caption: Control diagram for decentralized Integral Force Feedback
|
|
% #+RESULTS:
|
|
% [[file:figs/rotating_iff_diagram.png]]
|
|
|
|
% The decentralized IFF controller $\bm{K}_F$ corresponds to a diagonal controller with integrators:
|
|
% #+name: eq:Kf_pure_int
|
|
% \begin{equation}
|
|
% \begin{aligned}
|
|
% \mathbf{K}_{F}(s) &= \begin{bmatrix} K_{F}(s) & 0 \\ 0 & K_{F}(s) \end{bmatrix} \\
|
|
% K_{F}(s) &= g \cdot \frac{1}{s}
|
|
% \end{aligned}
|
|
% \end{equation}
|
|
|
|
% In order to see how the IFF controller affects the poles of the closed loop system, a Root Locus plot (Figure ref:fig:rotating_root_locus_iff_pure_int) is constructed as follows: the poles of the closed-loop system are drawn in the complex plane as the controller gain $g$ varies from $0$ to $\infty$ for the two controllers $K_{F}$ simultaneously.
|
|
% As explained in cite:preumont08_trans_zeros_struc_contr_with,skogestad07_multiv_feedb_contr, the closed-loop poles start at the open-loop poles (shown by $\tikz[baseline=-0.6ex] \node[cross out, draw=black, minimum size=1ex, line width=2pt, inner sep=0pt, outer sep=0pt] at (0, 0){};$) for $g = 0$ and coincide with the transmission zeros (shown by $\tikz[baseline=-0.6ex] \draw[line width=2pt, inner sep=0pt, outer sep=0pt] (0,0) circle[radius=3pt];$) as $g \to \infty$.
|
|
|
|
% #+begin_important
|
|
% Whereas collocated IFF is usually associated with unconditional stability cite:preumont91_activ, this property is lost due to gyroscopic effects as soon as the rotation velocity in non-null.
|
|
% This can be seen in the Root Locus plot (Figure ref:fig:rotating_root_locus_iff_pure_int) where poles corresponding to the controller are bound to the right half plane implying closed-loop system instability.
|
|
% #+end_important
|
|
|
|
% Physically, this can be explained like so: at low frequency, the loop gain is very large due to the pure integrator in $K_{F}$ and the finite gain of the plant (Figure ref:fig:rotating_iff_bode_plot_effect_rot).
|
|
% The control system is thus canceling the spring forces which makes the suspended platform no able to hold the payload against centrifugal forces, hence the instability.
|
|
|
|
|
|
%% Root Locus for the Decentralized Integral Force Feedback controller
|
|
figure;
|
|
|
|
Kiff = 1/s*eye(2);
|
|
|
|
gains = logspace(-2, 4, 300);
|
|
Wz_i = [1,3,4];
|
|
|
|
hold on;
|
|
for i = 1:length(Wz_i)
|
|
plot(real(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(i,:), ...
|
|
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
|
|
plot(real(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(i,:), ...
|
|
'HandleVisibility', 'off','MarkerSize',8);
|
|
for g = gains
|
|
cl_poles = pole(feedback(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff, -1));
|
|
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
|
|
'HandleVisibility', 'off','MarkerSize',4);
|
|
end
|
|
end
|
|
hold off;
|
|
axis square;
|
|
xlim([-1.8, 0.2]); ylim([0, 2]);
|
|
xticks([-1, 0])
|
|
xticklabels({'-$\omega_0$', '$0$'})
|
|
yticks([0, 1, 2])
|
|
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})
|
|
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
leg = legend('location', 'northwest', 'FontSize', 8);
|
|
leg.ItemTokenSize(1) = 8;
|