262 lines
11 KiB
Mathematica
262 lines
11 KiB
Mathematica
|
%% 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');
|
||
|
|
|
||
|
|
% Identify plants :noexport:
|
||
|
|
|
||
|
|
%% The rotating speed is set to $\Omega = 0.1 \omega_0$.
|
||
|
|
Wz = 0.1; % [rad/s]
|
||
|
|
|
||
|
|
%% Masses
|
||
|
|
ms = 0.5; % Sample mass [kg]
|
||
|
|
mn = 0.5; % Tuv mass [kg]
|
||
|
|
|
||
|
|
%% General Configuration
|
||
|
|
model_config = struct();
|
||
|
|
model_config.controller = "open_loop"; % Default: Open-Loop
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % [Fu, Fv]
|
||
|
|
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % [Fdu, Fdv]
|
||
|
|
io(io_i) = linio([mdl, '/xf'], 1, 'openinput'); io_i = io_i + 1; % [Dfx, Dfy]
|
||
|
|
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||
|
|
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
|
||
|
|
io(io_i) = linio([mdl, '/ext_metrology'], 1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
|
||
|
|
|
||
|
|
%% Identifying plant with parallel stiffness
|
||
|
|
|
||
|
|
model_config.Tuv_type = "parallel_k";
|
||
|
|
|
||
|
|
% Parallel stiffness
|
||
|
|
kp = 2*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
|
||
|
|
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||
|
|
|
||
|
|
% Tuv Stage
|
||
|
|
kn = 1 - kp; % Stiffness [N/m]
|
||
|
|
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||
|
|
|
||
|
|
% Linearize
|
||
|
|
G_kp = linearize(mdl, io, 0);
|
||
|
|
G_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||
|
|
G_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||
|
|
|
||
|
|
%% Identifying plant with no parallel stiffness
|
||
|
|
|
||
|
|
model_config.Tuv_type = "normal";
|
||
|
|
|
||
|
|
% Tuv Stage
|
||
|
|
kn = 1; % Stiffness [N/m]
|
||
|
|
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||
|
|
|
||
|
|
% Linearize
|
||
|
|
G = linearize(mdl, io, 0);
|
||
|
|
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||
|
|
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||
|
|
|
||
|
|
%% IFF Controller
|
||
|
|
Kiff = (2.2/(s + 0.1))*eye(2);
|
||
|
|
Kiff.InputName = {'fu', 'fv'};
|
||
|
|
Kiff.OutputName = {'Fu', 'Fv'};
|
||
|
|
|
||
|
|
%% IFF Controller with added stiffness
|
||
|
|
Kiff_kp = (2.2/(s + 0.1))*eye(2);
|
||
|
|
Kiff_kp.InputName = {'fu', 'fv'};
|
||
|
|
Kiff_kp.OutputName = {'Fu', 'Fv'};
|
||
|
|
|
||
|
|
%% Relative Damping Controller
|
||
|
|
Krdc = 2*s*eye(2);
|
||
|
|
Krdc.InputName = {'Du', 'Dv'};
|
||
|
|
Krdc.OutputName = {'Fu', 'Fv'};
|
||
|
|
|
||
|
|
% Root Locus
|
||
|
|
|
||
|
|
% Figure ref:fig:rotating_comp_techniques_root_locus shows the Root Locus plots for the two proposed IFF modifications as well as for relative damping control.
|
||
|
|
% While the two pairs of complex conjugate open-loop poles are identical for both IFF modifications, the transmission zeros are not.
|
||
|
|
% This means that the closed-loop behavior of both systems will differ when large control gains are used.
|
||
|
|
|
||
|
|
% One can observe that the closed loop poles corresponding to the system with added springs (in red) are bounded to the left half plane implying unconditional stability.
|
||
|
|
% This is not the case for the system where the controller is augmented with an HPF (in blue).
|
||
|
|
|
||
|
|
% It is interesting to note that the maximum added damping is very similar for both techniques.
|
||
|
|
|
||
|
|
|
||
|
|
%% Comparison of active damping techniques for rotating platform - Root Locus
|
||
|
|
figure;
|
||
|
|
|
||
|
|
gains = logspace(-2, 2, 500);
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
% IFF
|
||
|
|
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', 'IFF with HPF', 'MarkerSize', 8);
|
||
|
|
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(1,:), ...
|
||
|
|
'HandleVisibility', 'off', 'MarkerSize', 8);
|
||
|
|
for g = gains
|
||
|
|
cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
|
||
|
|
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:),'MarkerSize',4, ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
% IFF with parallel stiffness
|
||
|
|
plot(real(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), imag(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'x', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', 'IFF with $k_p$', 'MarkerSize', 8);
|
||
|
|
plot(real(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), imag(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'o', 'color', colors(2,:), ...
|
||
|
|
'HandleVisibility', 'off', 'MarkerSize', 8);
|
||
|
|
for g = gains
|
||
|
|
cl_poles = pole(feedback(G_kp({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff_kp));
|
||
|
|
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:),'MarkerSize',4, ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
% RDC
|
||
|
|
plot(real(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), imag(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', 'RDC', 'MarkerSize', 8);
|
||
|
|
plot(real(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), imag(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(3,:), ...
|
||
|
|
'HandleVisibility', 'off', 'MarkerSize', 8);
|
||
|
|
for g = gains
|
||
|
|
cl_poles = pole(feedback(G({'Du', 'Dv'}, {'Fu', 'Fv'}), g*Krdc));
|
||
|
|
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:),'MarkerSize',4, ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
axis square;
|
||
|
|
xlim([-1.15, 0.05]); ylim([0, 1.2]);
|
||
|
|
|
||
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
||
|
|
leg = legend('location', 'northwest', 'FontSize', 8);
|
||
|
|
leg.ItemTokenSize(1) = 12;
|
||
|
|
|
||
|
|
% Obtained Damped Plant
|
||
|
|
% The actively damped plants are computed for the three techniques and compared in Figure ref:fig:rotating_comp_techniques_dampled_plants.
|
||
|
|
|
||
|
|
% #+begin_important
|
||
|
|
% It is shown that while the diagonal (direct) terms of the damped plants are similar for the three active damping techniques, of off-diagonal (coupling) terms are not.
|
||
|
|
% Integral Force Feedback strategy is adding some coupling at low frequency which may negatively impact the positioning performances.
|
||
|
|
% #+end_important
|
||
|
|
|
||
|
|
|
||
|
|
%% Compute Damped plants
|
||
|
|
G_cl_iff = feedback(G, Kiff, 'name');
|
||
|
|
G_cl_iff_kp = feedback(G_kp, Kiff_kp, 'name');
|
||
|
|
G_cl_rdc = feedback(G, Krdc, 'name');
|
||
|
|
|
||
|
|
%% Comparison of the damped plants obtained with the three active damping techniques
|
||
|
|
figure;
|
||
|
|
freqs = logspace(-3, 2, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
% Magnitude
|
||
|
|
ax1 = nexttile([2, 1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', 'IFF + HPF')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', 'IFF + $k_p$')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', 'RDC')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
|
||
|
|
'DisplayName', 'Coupling')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', 'Coupling')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', 'Coupling')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', 'Coupling')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||
|
|
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
ldg.ItemTokenSize = [10, 1];
|
||
|
|
ylim([1e-6, 1e2])
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||
|
|
yticks(-180:90:180);
|
||
|
|
ylim([-180 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
|
||
|
|
% Transmissibility And Compliance
|
||
|
|
% The proposed active damping techniques are now compared in terms of closed-loop transmissibility and compliance.
|
||
|
|
|
||
|
|
% The transmissibility is here defined as the transfer function from a displacement of the rotating stage along $\vec{i}_x$ to the displacement of the payload along the same direction.
|
||
|
|
% It is used to characterize how much vibration is transmitted through the suspended platform to the payload.
|
||
|
|
|
||
|
|
% The compliance describes the displacement response of the payload to external forces applied to it.
|
||
|
|
% This is a useful metric when disturbances are directly applied to the payload.
|
||
|
|
% It is here defined as the transfer function from external forces applied on the payload along $\vec{i}_x$ to the displacement of the payload along the same direction.
|
||
|
|
|
||
|
|
% Very similar results are obtained for the two proposed IFF modifications in terms of transmissibility and compliance (Figure ref:fig:rotating_comp_techniques_transmissibility_compliance).
|
||
|
|
|
||
|
|
% #+begin_important
|
||
|
|
% Using IFF degrades the compliance at low frequency while using relative damping control degrades the transmissibility at high frequency.
|
||
|
|
% This is very well known characteristics of these common active damping techniques that holds when applied to rotating platforms.
|
||
|
|
% #+end_important
|
||
|
|
|
||
|
|
|
||
|
|
%% Comparison of the obtained transmissibilty and compliance for the three tested active damping techniques
|
||
|
|
freqs = logspace(-2, 2, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
% Transmissibility
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', 'IFF + HPF')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', 'IFF + $k_p$')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', 'RDC')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
|
||
|
|
% Compliance
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff( 'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', 'IFF + HPF')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', 'IFF + $k_p$')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(G_cl_rdc( 'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', 'RDC')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
|
||
|
|
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
ldg.ItemTokenSize = [10, 1];
|
||
|
|
xlim([freqs(1), freqs(end)]);
|