add all files
This commit is contained in:
BIN
A2-nass-rotating-3dof-model/mat/nass_controllers.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/nass_controllers.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/mat/rotating_generic_plants.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/rotating_generic_plants.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/mat/rotating_nass_plants.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/rotating_nass_plants.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/mat/uniaxial_micro_station_parameters.mat
(Stored with Git LFS)
Normal file
BIN
A2-nass-rotating-3dof-model/mat/uniaxial_micro_station_parameters.mat
(Stored with Git LFS)
Normal file
Binary file not shown.
BIN
A2-nass-rotating-3dof-model/nass_rotating_model.slx
Normal file
BIN
A2-nass-rotating-3dof-model/nass_rotating_model.slx
Normal file
Binary file not shown.
187
A2-nass-rotating-3dof-model/rotating_1_system_description.m
Normal file
187
A2-nass-rotating-3dof-model/rotating_1_system_description.m
Normal file
@@ -0,0 +1,187 @@
|
||||
%% 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';
|
||||
|
||||
%% Model parameters for first analysis
|
||||
kn = 1; % Actuator Stiffness [N/m]
|
||||
mn = 1; % Payload Mass [kg]
|
||||
cn = 0.05; % Actuator Damping [N/(m/s)]
|
||||
|
||||
xin = cn/(2*sqrt(kn*mn)); % Modal Damping [-]
|
||||
w0n = sqrt(kn/mn); % Natural Frequency [rad/s]
|
||||
|
||||
%% Computation of the poles as a function of the rotating velocity
|
||||
Wzs = linspace(0, 2, 51); % Vector of rotation speeds [rad/s]
|
||||
|
||||
p_ws = zeros(4, length(Wzs));
|
||||
|
||||
for i = 1:length(Wzs)
|
||||
Wz = Wzs(i);
|
||||
|
||||
pole_G = pole(1/(((s^2)/(w0n^2) + 2*xin*s/w0n + 1 - (Wz^2)/(w0n^2))^2 + (2*Wz*s/(w0n^2))^2));
|
||||
[~, i_sort] = sort(imag(pole_G));
|
||||
p_ws(:, i) = pole_G(i_sort);
|
||||
end
|
||||
|
||||
clear pole_G;
|
||||
|
||||
%% Campbell diagram - Real and Imaginary parts of the poles as a function of the rotating velocity
|
||||
figure;
|
||||
hold on;
|
||||
plot(Wzs, real(p_ws(1, :)), '-', 'color', colors(1,:), 'DisplayName', '$p_{+}$')
|
||||
plot(Wzs, real(p_ws(4, :)), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(Wzs, real(p_ws(2, :)), '-', 'color', colors(2,:), 'DisplayName', '$p_{-}$')
|
||||
plot(Wzs, real(p_ws(3, :)), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
||||
plot(Wzs, zeros(size(Wzs)), 'k--', 'HandleVisibility', 'off')
|
||||
hold off;
|
||||
xlabel('Rotational Speed $\Omega$'); ylabel('Real Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
xlim([0, 2*w0n]);
|
||||
xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
|
||||
xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
|
||||
ylim([-3*xin, 3*xin]);
|
||||
yticks([-3*xin, -2*xin, -xin, 0, xin, 2*xin, 3*xin])
|
||||
yticklabels({'', '', '$-\xi\omega_0$', '$0$', ''})
|
||||
|
||||
figure
|
||||
hold on;
|
||||
plot(Wzs, imag(p_ws(1, :)), '-', 'color', colors(1,:))
|
||||
plot(Wzs, imag(p_ws(4, :)), '-', 'color', colors(1,:))
|
||||
plot(Wzs, imag(p_ws(2, :)), '-', 'color', colors(2,:))
|
||||
plot(Wzs, imag(p_ws(3, :)), '-', 'color', colors(2,:))
|
||||
plot(Wzs, zeros(size(Wzs)), 'k--')
|
||||
hold off;
|
||||
xlabel('Rotational Speed $\Omega$'); ylabel('Imaginary Part');
|
||||
xlim([0, 2*w0n]);
|
||||
xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
|
||||
xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
|
||||
ylim([-3*w0n, 3*w0n]);
|
||||
yticks([-3*w0n, -2*w0n, -w0n, 0, w0n, 2*w0n, 3*w0n])
|
||||
yticklabels({'', '', '$-\omega_0$', '$0$', '$\omega_0$', '', ''})
|
||||
|
||||
%% Identify the dynamics for several rotating velocities
|
||||
% Sample
|
||||
ms = 0.5; % Sample mass [kg]
|
||||
|
||||
% Tuv Stage
|
||||
kn = 1; % Stiffness [N/m]
|
||||
mn = 0.5; % Tuv mass [kg]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "normal"; % Default: 2DoF stage
|
||||
|
||||
% 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]
|
||||
|
||||
% Tested rotating velocities [rad/s]
|
||||
Wzs = [0, 0.1, 0.2, 0.7, 1.2]; % Vector of rotation speeds [rad/s]
|
||||
|
||||
Gs = {zeros(2, 2, length(Wzs))}; % Direct terms
|
||||
|
||||
for i = 1:length(Wzs)
|
||||
Wz = Wzs(i);
|
||||
|
||||
%% Linearize the model
|
||||
G = linearize(mdl, io, 0);
|
||||
|
||||
%% Input/Output definition
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Gs{:,:,i} = G;
|
||||
end
|
||||
|
||||
% Save All Identified Plants
|
||||
save('./mat/rotating_generic_plants.mat', 'Gs', 'Wzs');
|
||||
|
||||
%% Bode plot of the direct and coupling terms for several rotating velocities
|
||||
freqs = logspace(-1, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ylim([1e-2, 1e2]);
|
||||
yticks([1e-2,1e-1,1,1e1,1e2])
|
||||
yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('du', '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-1,1,1e1])
|
||||
xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}('dv', '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',[]); ylabel('Magnitude [m/N]');
|
||||
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [10, 1];
|
||||
ylim([1e-2, 1e2]);
|
||||
yticks([1e-2,1e-1,1,1e1,1e2])
|
||||
yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})
|
||||
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(Wzs)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dv', '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-1,1,1e1])
|
||||
xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
86
A2-nass-rotating-3dof-model/rotating_2_iff_pure_int.m
Normal file
86
A2-nass-rotating-3dof-model/rotating_2_iff_pure_int.m
Normal file
@@ -0,0 +1,86 @@
|
||||
%% 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');
|
||||
|
||||
%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
Wz_i = [1,3,4];
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
|
||||
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
|
||||
ylim([1e-3, 1e2]);
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i(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([0 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],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Root Locus for the Decentralized Integral Force Feedback controller
|
||||
Kiff = 1/s*eye(2);
|
||||
|
||||
gains = logspace(-2, 4, 300);
|
||||
Wz_i = [1,3,4];
|
||||
|
||||
figure;
|
||||
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;
|
||||
210
A2-nass-rotating-3dof-model/rotating_3_iff_hpf.m
Normal file
210
A2-nass-rotating-3dof-model/rotating_3_iff_hpf.m
Normal file
@@ -0,0 +1,210 @@
|
||||
%% 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');
|
||||
|
||||
%% Modified IFF - parameters
|
||||
g = 2; % Controller gain
|
||||
wi = 0.1; % HPF Cut-Off frequency [rad/s]
|
||||
|
||||
Kiff = (g/s)*eye(2); % Pure IFF
|
||||
Kiff_hpf = (g/(wi+s))*eye(2); % IFF with added HPF
|
||||
|
||||
%% Loop gain for the IFF with pure integrator and modified IFF with added high-pass filter
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
Wz_i = 2;
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))))
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Loop Gain');
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF,HPF')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-90 180]);
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
leg = legend('location', 'southwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% High-Pass Filter Cut-Off Frequency
|
||||
wis = [0.01, 0.05, 0.1]; % [rad/s]
|
||||
|
||||
%% Root Locus for the initial IFF and the modified IFF
|
||||
gains = logspace(-2, 4, 200);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for wi_i = 1:length(wis)
|
||||
wi = wis(wi_i);
|
||||
Kiff = (1/(wi+s))*eye(2);
|
||||
L(wi_i) = plot(nan, nan, '.', 'color', colors(wi_i,:), 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi));
|
||||
for g = gains
|
||||
clpoles = pole(feedback(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(wi_i,:),'MarkerSize',4);
|
||||
end
|
||||
plot(real(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
imag(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
'x', 'color', colors(wi_i,:),'MarkerSize',8);
|
||||
plot(real(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
imag(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
|
||||
'o', 'color', colors(wi_i,:),'MarkerSize',8);
|
||||
end
|
||||
hold off;
|
||||
axis equal;
|
||||
xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
|
||||
xticks([-2:1:2]); yticks([-2:1:2]);
|
||||
leg = legend(L, 'location', 'southwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
clear L
|
||||
|
||||
xlim([-1.25, 0.25]); ylim([-1.25, 1.25]);
|
||||
xticks([-1, 0])
|
||||
xticklabels({'$-\omega_0$', '$0$'})
|
||||
yticks([-1, 0, 1])
|
||||
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
|
||||
ytickangle(90)
|
||||
|
||||
%% Compute the optimal control gain
|
||||
wis = logspace(-2, 1, 100); % [rad/s]
|
||||
|
||||
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_gain = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
for wi_i = 1:length(wis)
|
||||
wi = wis(wi_i);
|
||||
Kiff = 1/(s + wi)*eye(2);
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((1/Wzs(2))^2 - 1));
|
||||
opt_xi(wi_i) = 1/xi_opt;
|
||||
opt_gain(wi_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Attainable damping ratio as a function of wi/w0. Corresponding control gain g_opt and g_max are also shown
|
||||
figure;
|
||||
yyaxis left
|
||||
plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$');
|
||||
plot(wis, wis*((1/Wzs(2))^2 - 1), '--', 'DisplayName', '$g_{max}$');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,10]);
|
||||
ylabel('Controller gain $g$');
|
||||
|
||||
xlabel('$\omega_i/\omega_0$');
|
||||
set(gca, 'XScale', 'log');
|
||||
legend('location', 'northeast', 'FontSize', 8);
|
||||
|
||||
%% Compute damped plant
|
||||
wis = [0.03, 0.1, 0.5]; % [rad/s]
|
||||
g = 2; % Gain
|
||||
|
||||
Gs_iff_hpf = {};
|
||||
|
||||
for i = 1:length(wis)
|
||||
Kiff_hpf = (g/(wis(i)+s))*eye(2); % IFF with added HPF
|
||||
Kiff_hpf.InputName = {'fu', 'fv'};
|
||||
Kiff_hpf.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
Gs_iff_hpf(i) = {feedback(Gs{2}, Kiff_hpf, 'name')};
|
||||
end
|
||||
|
||||
%% Effect of $\omega_i$ on the damped plant coupling
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i = 1:length(wis)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
|
||||
'DisplayName', sprintf('$d_u/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
|
||||
plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:), 0.5], ...
|
||||
'DisplayName', sprintf('$d_v/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 20;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:length(wis)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 0]);
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Effect of $\omega_i$ on the obtained compliance
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile();
|
||||
hold on;
|
||||
for i = 1:length(wis)
|
||||
plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fdx'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
|
||||
'DisplayName', sprintf('$d_{x}/F_{dx}$, $\\omega_i = %.2f \\omega_0$', wis(i)))
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{2}('Du', 'Fdx'), freqs, 'rad/s'))), 'k--', ...
|
||||
'DisplayName', '$d_{x}/F_{dx}$, OL')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 20;
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
339
A2-nass-rotating-3dof-model/rotating_4_iff_kp.m
Normal file
339
A2-nass-rotating-3dof-model/rotating_4_iff_kp.m
Normal file
@@ -0,0 +1,339 @@
|
||||
%% 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');
|
||||
|
||||
%% Tuv Stage
|
||||
mn = 0.5; % Tuv mass [kg]
|
||||
|
||||
%% Sample
|
||||
ms = 0.5; % Sample mass [kg]
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
%% 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, '/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]
|
||||
|
||||
Wz = 0.1; % The rotation speed [rad/s]
|
||||
|
||||
%% No parallel Stiffness
|
||||
kp = 0; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
G_no_kp = linearize(mdl, io, 0);
|
||||
G_no_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G_no_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Small parallel Stiffness
|
||||
kp = 0.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
G_low_kp = linearize(mdl, io, 0);
|
||||
G_low_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G_low_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Large parallel Stiffness
|
||||
kp = 1.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
G_high_kp = linearize(mdl, io, 0);
|
||||
G_high_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G_high_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Effect of the parallel stiffness on the IFF plant
|
||||
freqs = logspace(-2, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_no_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
|
||||
'DisplayName', '$k_p = 0$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
|
||||
'DisplayName', '$k_p < m\Omega^2$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-', ...
|
||||
'DisplayName', '$k_p > m\Omega^2$')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
|
||||
ylim([1e-4, 5e1]);
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-')
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([0 180]);
|
||||
hold off;
|
||||
xticks([1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Root Locus for IFF without parallel spring, with small parallel spring and with large parallel spring
|
||||
gains = logspace(-2, 2, 200);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(1,:), ...
|
||||
'DisplayName', '$k_p = 0$','MarkerSize',8);
|
||||
plot(real(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_no_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
|
||||
plot(real(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(2,:), ...
|
||||
'DisplayName', '$k_p < m\Omega^2$','MarkerSize',8);
|
||||
plot(real(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_low_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
|
||||
plot(real(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(3,:), ...
|
||||
'DisplayName', '$k_p > m\Omega^2$','MarkerSize',8);
|
||||
plot(real(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), imag(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G_high_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',4);
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
|
||||
xticks([-2, -1, 0])
|
||||
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
|
||||
yticks([-1, 0, 1])
|
||||
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 8;
|
||||
|
||||
%% Tested parallel stiffnesses
|
||||
kps = [2, 20, 40]*(mn + ms)*Wz^2;
|
||||
|
||||
%% Root Locus: Effect of the parallel stiffness on the attainable damping
|
||||
gains = logspace(-2, 4, 500);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for kp_i = 1:length(kps)
|
||||
kp = kps(kp_i); % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'x', 'color', colors(kp_i,:), ...
|
||||
'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/((mn+ms)*Wz^2)),'MarkerSize',8);
|
||||
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'o', 'color', colors(kp_i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), (g/s)*eye(2)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(kp_i,:),'MarkerSize',4, ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
axis square;
|
||||
% xlim([-1.15, 0.05]); ylim([0, 1.2]);
|
||||
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
|
||||
xticks([-2, -1, 0])
|
||||
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
|
||||
yticks([-1, 0, 1])
|
||||
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
|
||||
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'northwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 12;
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping
|
||||
alphas = logspace(-2, 0, 100);
|
||||
alphas(end) = []; % Remove last point
|
||||
|
||||
opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping
|
||||
opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain
|
||||
|
||||
Kiff = 1/s*eye(2);
|
||||
|
||||
for alpha_i = 1:length(alphas)
|
||||
kp = alphas(alpha_i);
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 2);
|
||||
opt_xi(alpha_i) = 1/xi_opt;
|
||||
opt_gain(alpha_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Attainable damping as a function of the stiffness ratio
|
||||
figure;
|
||||
yyaxis left
|
||||
plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,2.5]);
|
||||
ylabel('Controller gain $g$');
|
||||
|
||||
set(gca, 'XScale', 'log');
|
||||
legend('location', 'northeast', 'FontSize', 8);
|
||||
|
||||
xlabel('$k_p$');
|
||||
xlim([0.01, 1]);
|
||||
xticks([0.01, 0.1, 1])
|
||||
xticklabels({'$m\Omega^2$', '$10m\Omega^2$', '$100m\Omega^2$'})
|
||||
|
||||
%% Identify dynamics with parallel stiffness = 2mW^2
|
||||
Wz = 0.1; % [rad/s]
|
||||
kp = 2*(mn + ms)*Wz^2; % Parallel Stiffness [N/m]
|
||||
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
|
||||
kn = 1 - kp; % Stiffness [N/m]
|
||||
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
|
||||
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% IFF controller with pure integrator
|
||||
Kiff_kp = (2.2/s)*eye(2);
|
||||
Kiff_kp.InputName = {'fu', 'fv'};
|
||||
Kiff_kp.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% Compute the damped plant
|
||||
G_cl_iff_kp = feedback(G, Kiff_kp, 'name');
|
||||
|
||||
w0 = sqrt((kn+kp)/(mn+ms)); % Resonance frequency [rad/s]
|
||||
wis = w0*logspace(-2, 0, 100); % LPF cut-off [rad/s]
|
||||
|
||||
%% Computes the obtained damping as a function of the HPF cut-off frequency
|
||||
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
|
||||
for wi_i = 1:length(wis)
|
||||
Kiff_kp_hpf = (2.2/(s + wis(wi_i)))*eye(2);
|
||||
Kiff_kp_hpf.InputName = {'fu', 'fv'};
|
||||
Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
[~, xi] = damp(feedback(G, Kiff_kp_hpf, 'name'));
|
||||
opt_xi(wi_i) = min(xi);
|
||||
end
|
||||
|
||||
%% Effect of the high-pass filter cut-off frequency on the obtained damping
|
||||
figure;
|
||||
plot(wis, opt_xi, '-');
|
||||
set(gca, 'XScale', 'log');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
xlabel('$\omega_i/\omega_0$');
|
||||
|
||||
%% Compute the damped plant with added High-Pass Filter
|
||||
Kiff_kp_hpf = (2.2/(s + 0.1*w0))*eye(2);
|
||||
Kiff_kp_hpf.InputName = {'fu', 'fv'};
|
||||
Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
G_cl_iff_hpf_kp = feedback(G, Kiff_kp_hpf, 'name');
|
||||
|
||||
%% Bode plot of the direct and coupling terms for several rotating velocities
|
||||
freqs = logspace(-3, 1, 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', '$d_u/F_u$ - OL')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', '$d_u/F_u$ - IFF + $k_p$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
|
||||
'DisplayName', '$d_u/F_u$ - IFF + $k_p$ + HPF')
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros( 1,3), 0.5], ...
|
||||
'HandleVisibility', 'off')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize(1) = 10;
|
||||
|
||||
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_kp( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 90]);
|
||||
xticks([1e-3,1e-2,1e-1,1,1e1])
|
||||
xticklabels({'$0.001 \omega_0$', '$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
96
A2-nass-rotating-3dof-model/rotating_5_rdc.m
Normal file
96
A2-nass-rotating-3dof-model/rotating_5_rdc.m
Normal file
@@ -0,0 +1,96 @@
|
||||
%% 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');
|
||||
|
||||
%% Root Locus for Relative Damping Control
|
||||
Krdc = s*eye(2); % Relative damping controller
|
||||
|
||||
gains = logspace(-2, 2, 300); % Tested gains
|
||||
Wz_i = [1,3,4];
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(Wz_i)
|
||||
plot(real(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), imag(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(i,:), ...
|
||||
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
|
||||
plot(real(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), imag(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(i,:), ...
|
||||
'HandleVisibility', 'off','MarkerSize',8);
|
||||
for g = gains
|
||||
cl_poles = pole(feedback(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'}), g*Krdc, -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;
|
||||
|
||||
i = 2;
|
||||
|
||||
%% Relative Damping Controller
|
||||
Krdc = 2*s*eye(2);
|
||||
Krdc.InputName = {'Du', 'Dv'};
|
||||
Krdc.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
%% Compute the damped plant
|
||||
G_cl_rdc = feedback(Gs{i}, Krdc, 'name');
|
||||
|
||||
%% Damped plant using Relative Damping Control
|
||||
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(Gs{i}( 'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL - $G_d(1,1)$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
|
||||
'DisplayName', 'RDC - $G_d(1,1)$')
|
||||
plot(freqs, abs(squeeze(freqresp(Gs{i}( 'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'OL - $G_d(2,1)$')
|
||||
plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', 'RDC - $G_d(2,1)$')
|
||||
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(1) = 20;
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
213
A2-nass-rotating-3dof-model/rotating_6_act_damp_comparison.m
Normal file
213
A2-nass-rotating-3dof-model/rotating_6_act_damp_comparison.m
Normal file
@@ -0,0 +1,213 @@
|
||||
%% 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';
|
||||
|
||||
%% 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'};
|
||||
|
||||
%% Comparison of active damping techniques for rotating platform - Root Locus
|
||||
gains = logspace(-2, 2, 500);
|
||||
|
||||
figure;
|
||||
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;
|
||||
|
||||
%% 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
|
||||
freqs = logspace(-2, 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 15]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([1e-2,1e-1,1,1e1,1e2])
|
||||
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$', '$100 \omega_0$'})
|
||||
|
||||
%% Comparison of the obtained transmissibility and compliance for the three tested active damping techniques
|
||||
freqs = logspace(-2, 2, 1000);
|
||||
|
||||
% transmissibility
|
||||
figure;
|
||||
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
|
||||
figure;
|
||||
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)]);
|
||||
682
A2-nass-rotating-3dof-model/rotating_7_nano_hexapod.m
Normal file
682
A2-nass-rotating-3dof-model/rotating_7_nano_hexapod.m
Normal file
@@ -0,0 +1,682 @@
|
||||
%% 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';
|
||||
|
||||
%% Nano-Hexapod
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
|
||||
%% Light Sample
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "normal"; % Default: 2DoF stage
|
||||
|
||||
%% 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; % [Fdx, Fdy]
|
||||
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]
|
||||
|
||||
%% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_vc_norot = linearize(mdl, io, 0.0);
|
||||
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_vc_fast = linearize(mdl, io, 0.0);
|
||||
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_md_norot = linearize(mdl, io, 0.0);
|
||||
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_md_fast = linearize(mdl, io, 0.0);
|
||||
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_pz_norot = linearize(mdl, io, 0.0);
|
||||
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_pz_fast = linearize(mdl, io, 0.0);
|
||||
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Compute negative spring in [N/m]
|
||||
Kneg_light = (15+1)*(2*pi)^2;
|
||||
|
||||
%% Effect of rotation on the nano-hexapod dynamics
|
||||
freqs = logspace(0, 1, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
|
||||
'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(1,:), ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-6, 1e-2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), 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, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
% xlim([1, 1e3]);
|
||||
|
||||
%% Effect of rotation on the nano-hexapod dynamics
|
||||
freqs = logspace(1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
|
||||
'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(2,:), ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 1e-4])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(2,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), 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:90:360);
|
||||
ylim([-180, 0]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
%% Effect of rotation on the nano-hexapod dynamics
|
||||
freqs = logspace(2, 3, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
|
||||
'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(3,:), ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
|
||||
'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-6])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(3,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(3,:));
|
||||
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');
|
||||
|
||||
%% Compute the optimal control gain
|
||||
wis = logspace(-2, 3, 200); % [rad/s]
|
||||
|
||||
opt_iff_hpf_xi_vc = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_iff_hpf_gain_vc = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
opt_iff_hpf_xi_md = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_iff_hpf_gain_md = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
opt_iff_hpf_xi_pz = zeros(1, length(wis)); % Optimal simultaneous damping
|
||||
opt_iff_hpf_gain_pz = zeros(1, length(wis)); % Corresponding optimal gain
|
||||
|
||||
for wi_i = 1:length(wis)
|
||||
wi = wis(wi_i);
|
||||
Kiff = 1/(s + wi)*eye(2);
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G_vc_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_hpf_xi_vc(wi_i) = 1/xi_opt;
|
||||
opt_iff_hpf_gain_vc(wi_i) = g_opt;
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G_md_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_hpf_xi_md(wi_i) = 1/xi_opt;
|
||||
opt_iff_hpf_gain_md(wi_i) = g_opt;
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, G_pz_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_hpf_xi_pz(wi_i) = 1/xi_opt;
|
||||
opt_iff_hpf_gain_pz(wi_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Find optimal parameters with at least a gain margin of 2
|
||||
i_iff_hpf_vc = find(opt_iff_hpf_gain_vc < 0.5*(wis*((sqrt(1e4/16)/(2*pi))^2 - 1)));
|
||||
i_iff_hpf_vc = i_iff_hpf_vc(1);
|
||||
|
||||
i_iff_hpf_md = find(opt_iff_hpf_xi_md > 0.95*max(opt_iff_hpf_xi_md));
|
||||
i_iff_hpf_md = i_iff_hpf_md(end)+1;
|
||||
|
||||
i_iff_hpf_pz = find(opt_iff_hpf_xi_pz > 0.95*max(opt_iff_hpf_xi_pz));
|
||||
i_iff_hpf_pz = i_iff_hpf_pz(end)+1;
|
||||
|
||||
%% Optimal modified IFF parameters that yields maximum simultaneous damping
|
||||
figure;
|
||||
yyaxis left
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_xi_vc, '-', 'DisplayName', '$\xi_{cl}$');
|
||||
plot(wis(i_iff_hpf_vc), opt_iff_hpf_xi_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_gain_vc, '-', 'DisplayName', '$g_{opt}$');
|
||||
plot(wis, wis*((sqrt(1e4/16)/(2*pi))^2 - 1), '--', 'DisplayName', '$g_{max}$');
|
||||
plot(wis(i_iff_hpf_vc), opt_iff_hpf_gain_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,200]);
|
||||
xlabel('$\omega_i$ [rad/s]');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
ylabel('Controller gain $g$');
|
||||
set(gca, 'XScale', 'log');
|
||||
xticks([1e-2,1,1e2])
|
||||
legend('location', 'northwest', 'FontSize', 8);
|
||||
|
||||
figure;
|
||||
yyaxis left
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_xi_md, '-');
|
||||
plot(wis(i_iff_hpf_md), opt_iff_hpf_xi_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_gain_md, '-');
|
||||
plot(wis(i_iff_hpf_md), opt_iff_hpf_gain_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(wis, wis*((sqrt(1e6/16)/(2*pi))^2 - 1), '--');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1000]);
|
||||
xlabel('$\omega_i$ [rad/s]');
|
||||
ylabel('Controller gain $g$');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
set(gca, 'XScale', 'log');
|
||||
xticks([1e-2,1,1e2])
|
||||
|
||||
figure;
|
||||
yyaxis left
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_xi_pz, '-');
|
||||
plot(wis(i_iff_hpf_pz), opt_iff_hpf_xi_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
|
||||
yyaxis right
|
||||
hold on;
|
||||
plot(wis, opt_iff_hpf_gain_pz, '-');
|
||||
plot(wis(i_iff_hpf_pz), opt_iff_hpf_gain_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(wis, wis*((sqrt(1e8/16)/(2*pi))^2 - 1), '--');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,10000]);
|
||||
xlabel('$\omega_i$ [rad/s]');
|
||||
set(gca, 'YTickLabel',[]);
|
||||
ylabel('Controller gain $g$');
|
||||
set(gca, 'XScale', 'log');
|
||||
xticks([1e-2,1,1e2])
|
||||
|
||||
%% Maximum rotating velocity
|
||||
Wz = 2*pi; % [rad/s]
|
||||
|
||||
%% Minimum parallel stiffness
|
||||
kp_min = (mn + ms) * Wz^2; % [N/m]
|
||||
|
||||
%% Parameters for simulation
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
%% IFF Controller
|
||||
Kiff_vc = 1/(s + 0.1*sqrt(1e4/(mn+ms)))*eye(2); % IFF
|
||||
Kiff_md = 1/(s + 0.1*sqrt(1e6/(mn+ms)))*eye(2); % IFF
|
||||
Kiff_pz = 1/(s + 0.1*sqrt(1e8/(mn+ms)))*eye(2); % IFF
|
||||
|
||||
%% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - Voice Coil nano-hexapod
|
||||
kps_vc = logspace(log10(kp_min), log10(1e4), 100); % Tested parallel stiffnesses [N/m]
|
||||
kps_vc(end) = [];
|
||||
|
||||
opt_iff_kp_xi_vc = zeros(1, length(kps_vc)); % Optimal simultaneous damping
|
||||
opt_iff_kp_gain_vc = zeros(1, length(kps_vc)); % Corresponding optimal gain
|
||||
|
||||
for kp_i = 1:length(kps_vc)
|
||||
% Voice Coil Nano-Hexapod
|
||||
kp = kps_vc(kp_i);
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e4 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Giff_vc = linearize(mdl, io, 0);
|
||||
Giff_vc.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
Giff_vc.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Giff_vc({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_vc);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_kp_xi_vc(kp_i) = 1/xi_opt;
|
||||
opt_iff_kp_gain_vc(kp_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - APA nano-hexapod
|
||||
kps_md = logspace(log10(kp_min), log10(1e6), 100); % Tested parallel stiffnesses [N/m]
|
||||
kps_md(end) = [];
|
||||
|
||||
opt_iff_kp_xi_md = zeros(1, length(kps_md)); % Optimal simultaneous damping
|
||||
opt_iff_kp_gain_md = zeros(1, length(kps_md)); % Corresponding optimal gain
|
||||
|
||||
|
||||
for kp_i = 1:length(kps_md)
|
||||
% Voice Coil Nano-Hexapod
|
||||
kp = kps_md(kp_i);
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Giff_md = linearize(mdl, io, 0);
|
||||
Giff_md.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
Giff_md.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Giff_md({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_md);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_kp_xi_md(kp_i) = 1/xi_opt;
|
||||
opt_iff_kp_gain_md(kp_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
|
||||
kps_pz = logspace(log10(kp_min), log10(1e8), 100); % Tested parallel stiffnesses [N/m]
|
||||
kps_pz(end) = [];
|
||||
|
||||
opt_iff_kp_xi_pz = zeros(1, length(kps_pz)); % Optimal simultaneous damping
|
||||
opt_iff_kp_gain_pz = zeros(1, length(kps_pz)); % Corresponding optimal gain
|
||||
|
||||
|
||||
for kp_i = 1:length(kps_pz)
|
||||
% Voice Coil Nano-Hexapod
|
||||
kp = kps_pz(kp_i);
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Giff_pz = linearize(mdl, io, 0);
|
||||
Giff_pz.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
Giff_pz.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
fun = @(g)computeSimultaneousDamping(g, Giff_pz({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_pz);
|
||||
|
||||
[g_opt, xi_opt] = fminsearch(fun, 0.1);
|
||||
opt_iff_kp_xi_pz(kp_i) = 1/xi_opt;
|
||||
opt_iff_kp_gain_pz(kp_i) = g_opt;
|
||||
end
|
||||
|
||||
%% Find result with wanted parallel stiffness
|
||||
[~, i_kp_vc] = min(abs(kps_vc - 1e3));
|
||||
[~, i_kp_md] = min(abs(kps_md - 1e4));
|
||||
[~, i_kp_pz] = min(abs(kps_pz - 1e6));
|
||||
|
||||
%% Identify plants with choosen Parallel stiffnesses
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
% Voice Coil
|
||||
kp = 1e3;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_vc_kp_fast = linearize(mdl, io, 0);
|
||||
G_vc_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_vc_kp_norot = linearize(mdl, io, 0);
|
||||
G_vc_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% APA
|
||||
kp = 1e4;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_md_kp_fast = linearize(mdl, io, 0);
|
||||
G_md_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_md_kp_norot = linearize(mdl, io, 0);
|
||||
G_md_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% Piezo
|
||||
kp = 1e6;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_pz_kp_fast = linearize(mdl, io, 0);
|
||||
G_pz_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_pz_kp_norot = linearize(mdl, io, 0);
|
||||
G_pz_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
|
||||
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
|
||||
figure;
|
||||
hold on;
|
||||
plot(kps_vc, opt_iff_kp_xi_vc, '-', ...
|
||||
'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
|
||||
plot(kps_vc(i_kp_vc), opt_iff_kp_xi_vc(i_kp_vc), '.', ...
|
||||
'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(kps_md, opt_iff_kp_xi_md, '-', ...
|
||||
'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
|
||||
plot(kps_md(i_kp_md), opt_iff_kp_xi_md(i_kp_md), '.', ...
|
||||
'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(kps_pz, opt_iff_kp_xi_pz, '-', ...
|
||||
'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
|
||||
plot(kps_pz(i_kp_pz), opt_iff_kp_xi_pz(i_kp_pz), '.', ...
|
||||
'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
xlabel('$k_p [N/m]$');
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
set(gca, 'XScale', 'log');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
yticks([0:0.2:1])
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
xlim([kps_pz(1), kps_pz(end)])
|
||||
|
||||
%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
|
||||
rdc_gains = 2*logspace(1, 5, 200);
|
||||
% Obtained simultaneous damping
|
||||
rdc_xi_vc = zeros(1, length(rdc_gains));
|
||||
rdc_xi_md = zeros(1, length(rdc_gains));
|
||||
rdc_xi_pz = zeros(1, length(rdc_gains));
|
||||
|
||||
Krdc = s*eye(2);
|
||||
Krdc.InputName = {'Du', 'Dv'};
|
||||
Krdc.OutputName = {'Fu', 'Fv'};
|
||||
|
||||
for g_i = 1:length(rdc_gains)
|
||||
[~, xi] = damp(feedback(G_vc_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
|
||||
rdc_xi_vc(g_i) = min(xi);
|
||||
|
||||
[~, xi] = damp(feedback(G_md_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
|
||||
rdc_xi_md(g_i) = min(xi);
|
||||
|
||||
[~, xi] = damp(feedback(G_pz_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
|
||||
rdc_xi_pz(g_i) = min(xi);
|
||||
end
|
||||
|
||||
%% Optimal RDC
|
||||
[~, i_rdc_vc] = min(abs(rdc_xi_vc - 0.99));
|
||||
[~, i_rdc_md] = min(abs(rdc_xi_md - 0.99));
|
||||
[~, i_rdc_pz] = min(abs(rdc_xi_pz - 0.99));
|
||||
|
||||
Krdc_vc = rdc_gains(i_rdc_vc)*Krdc;
|
||||
Krdc_md = rdc_gains(i_rdc_md)*Krdc;
|
||||
Krdc_pz = rdc_gains(i_rdc_pz)*Krdc;
|
||||
|
||||
%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
|
||||
figure;
|
||||
hold on;
|
||||
plot(rdc_gains, rdc_xi_vc, '-', ...
|
||||
'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
|
||||
plot(rdc_gains(i_rdc_vc), rdc_xi_vc(i_rdc_vc), '.', ...
|
||||
'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(rdc_gains, rdc_xi_md, '-', ...
|
||||
'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
|
||||
plot(rdc_gains(i_rdc_md), rdc_xi_md(i_rdc_md), '.', ...
|
||||
'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
plot(rdc_gains, rdc_xi_pz, '-', ...
|
||||
'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
|
||||
plot(rdc_gains(i_rdc_pz), rdc_xi_pz(i_rdc_pz), '.', ...
|
||||
'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
xlabel('Relative Damping Controller gain $g$');
|
||||
ylabel('Damping Ratio $\xi$');
|
||||
set(gca, 'XScale', 'log');
|
||||
set(gca, 'YScale', 'lin');
|
||||
ylim([0,1]);
|
||||
yticks([0:0.2:1])
|
||||
xlim([rdc_gains(1), rdc_gains(end)])
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
|
||||
%% Closed-Loop Plants - IFF with HPF
|
||||
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
|
||||
G_vc_fast_iff_hpf = feedback(G_vc_fast, Kiff_hpf_vc, 'name');
|
||||
|
||||
G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
|
||||
G_md_fast_iff_hpf = feedback(G_md_fast, Kiff_hpf_md, 'name');
|
||||
|
||||
G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
|
||||
G_pz_fast_iff_hpf = feedback(G_pz_fast, Kiff_hpf_pz, 'name');
|
||||
|
||||
%% Closed-Loop Plants - IFF with Parallel Stiffness
|
||||
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
|
||||
G_vc_fast_iff_kp = feedback(G_vc_kp_fast, Kiff_kp_vc, 'name');
|
||||
|
||||
G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
|
||||
G_md_fast_iff_kp = feedback(G_md_kp_fast, Kiff_kp_md, 'name');
|
||||
|
||||
G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
|
||||
G_pz_fast_iff_kp = feedback(G_pz_kp_fast, Kiff_kp_pz, 'name');
|
||||
|
||||
%% Closed-Loop Plants - RDC
|
||||
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
|
||||
G_vc_fast_rdc = feedback(G_vc_fast, Krdc_vc, 'name');
|
||||
|
||||
G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
|
||||
G_md_fast_rdc = feedback(G_md_fast, Krdc_md, 'name');
|
||||
|
||||
G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
|
||||
G_pz_fast_rdc = feedback(G_pz_fast, Krdc_pz, 'name');
|
||||
|
||||
%% Comparison of the damped plants (direct and coupling terms) for the three proposed active damping techniques (IFF with HPF, IFF with $k_p$ and RDC) applied on the three nano-hexapod stiffnesses
|
||||
freqs_vc = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
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([freqs_vc(1), freqs_vc(end)]);
|
||||
|
||||
freqs_md = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-4])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
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([freqs_md(1), freqs_md(end)]);
|
||||
|
||||
freqs_pz = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)], ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)], ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)], ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)], ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-12, 1e-6])
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)]);
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)]);
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)]);
|
||||
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)]);
|
||||
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([freqs_pz(1), freqs_pz(end)]);
|
||||
470
A2-nass-rotating-3dof-model/rotating_8_nass.m
Normal file
470
A2-nass-rotating-3dof-model/rotating_8_nass.m
Normal file
@@ -0,0 +1,470 @@
|
||||
%% 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;
|
||||
|
||||
%% Nano-Hexapod on top of Micro-Station model
|
||||
mdl = 'nass_rotating_model';
|
||||
|
||||
%% Load micro-station parameters
|
||||
load('uniaxial_micro_station_parameters.mat')
|
||||
|
||||
%% Load controllers
|
||||
load('nass_controllers.mat');
|
||||
|
||||
%% System parameters
|
||||
mn = 15; % Nano-Hexapod mass [kg]
|
||||
ms = 1; % Sample Mass [kg]
|
||||
|
||||
% General Configuration
|
||||
model_config = struct();
|
||||
model_config.controller = "open_loop"; % Default: Open-Loop
|
||||
model_config.Tuv_type = "normal"; % Default: 2DoF stage
|
||||
|
||||
% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Forces [Fu, Fv]
|
||||
io(io_i) = linio([mdl, '/fd'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces on Sample [Fdx, Fdy]
|
||||
io(io_i) = linio([mdl, '/xf'], 1, 'openinput'); io_i = io_i + 1; % Floor Motion [Dfx, Dfy]
|
||||
io(io_i) = linio([mdl, '/ft'], 1, 'openinput'); io_i = io_i + 1; % Micro-Station Disturbances [Ftx, Fty]
|
||||
io(io_i) = linio([mdl, '/nano_hexapod'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
|
||||
io(io_i) = linio([mdl, '/nano_hexapod'], 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]
|
||||
|
||||
%% Identify plant without parallel stiffness
|
||||
% Voice Coil (i.e. soft) Nano-Hexapod
|
||||
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_vc_norot = linearize(mdl, io, 0.0);
|
||||
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_vc_fast = linearize(mdl, io, 0.0);
|
||||
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% APA (i.e. relatively stiff) Nano-Hexapod
|
||||
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_md_norot = linearize(mdl, io, 0.0);
|
||||
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_md_fast = linearize(mdl, io, 0.0);
|
||||
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% Piezoelectric (i.e. stiff) Nano-Hexapod
|
||||
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
Wz = 0; % Rotating Velocity [rad/s]
|
||||
G_pz_norot = linearize(mdl, io, 0.0);
|
||||
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 2*pi; % Rotating Velocity [rad/s]
|
||||
G_pz_fast = linearize(mdl, io, 0.0);
|
||||
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Identify plants with Parallel stiffnesses
|
||||
model_config.Tuv_type = "parallel_k"; % Default: 2DoF stage
|
||||
|
||||
% Voice Coil
|
||||
kp = 1e3;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_vc_kp_fast = linearize(mdl, io, 0);
|
||||
G_vc_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_vc_kp_norot = linearize(mdl, io, 0);
|
||||
G_vc_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% APA
|
||||
kp = 1e4;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_md_kp_fast = linearize(mdl, io, 0);
|
||||
G_md_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_md_kp_norot = linearize(mdl, io, 0);
|
||||
G_md_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
% Piezo
|
||||
kp = 1e5;
|
||||
cp = 2*0.001*sqrt((ms + mn)*kp);
|
||||
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
|
||||
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
|
||||
|
||||
% Identify dynamics
|
||||
Wz = 2*pi; % [rad/s]
|
||||
G_pz_kp_fast = linearize(mdl, io, 0);
|
||||
G_pz_kp_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
Wz = 0; % [rad/s]
|
||||
G_pz_kp_norot = linearize(mdl, io, 0);
|
||||
G_pz_kp_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
|
||||
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
|
||||
|
||||
%% Compute dampepd plants
|
||||
% Closed-Loop Plants - IFF with HPF
|
||||
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
|
||||
G_vc_fast_iff_hpf = feedback(G_vc_fast, Kiff_hpf_vc, 'name');
|
||||
|
||||
G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
|
||||
G_md_fast_iff_hpf = feedback(G_md_fast, Kiff_hpf_md, 'name');
|
||||
|
||||
G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
|
||||
G_pz_fast_iff_hpf = feedback(G_pz_fast, Kiff_hpf_pz, 'name');
|
||||
|
||||
% Closed-Loop Plants - IFF with Parallel Stiffness
|
||||
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
|
||||
G_vc_fast_iff_kp = feedback(G_vc_kp_fast, Kiff_kp_vc, 'name');
|
||||
|
||||
G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
|
||||
G_md_fast_iff_kp = feedback(G_md_kp_fast, Kiff_kp_md, 'name');
|
||||
|
||||
G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
|
||||
G_pz_fast_iff_kp = feedback(G_pz_kp_fast, Kiff_kp_pz, 'name');
|
||||
|
||||
% Closed-Loop Plants - RDC
|
||||
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
|
||||
G_vc_fast_rdc = feedback(G_vc_fast, Krdc_vc, 'name');
|
||||
|
||||
G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
|
||||
G_md_fast_rdc = feedback(G_md_fast, Krdc_md, 'name');
|
||||
|
||||
G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
|
||||
G_pz_fast_rdc = feedback(G_pz_fast, Krdc_pz, 'name');
|
||||
|
||||
%% Bode plot of the transfer function from nano-hexapod actuator to measured motion by the external metrology
|
||||
freqs_vc = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', zeros(1,3));
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([ -200, 20]);
|
||||
|
||||
linkaxes([ax,ax2],'x');
|
||||
xlim([freqs_vc(1), freqs_vc(end)]);
|
||||
xticks([1e-1, 1e0, 1e1]);
|
||||
|
||||
freqs_md = logspace(0, 3, 1000);
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-10, 1e-4])
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', zeros(1,3));
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([ -200, 20]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_md(1), freqs_md(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
freqs_pz = logspace(0, 3, 1000);
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
|
||||
'DisplayName', 'Coupling');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-12, 1e-6])
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', zeros(1,3));
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(1,:));
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(2,:));
|
||||
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(3,:));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
yticks(-360:90:360);
|
||||
ylim([ -200, 20]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs_pz(1), freqs_pz(end)]);
|
||||
xticks([1e0, 1e1, 1e2]);
|
||||
|
||||
%% Effect of Floor motion on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [15, 1];
|
||||
ylim([1e-4, 1e2]);
|
||||
|
||||
%% Effect of micro-station vibrations on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-12, 2e-7]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-12, 2e-7]);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
ylim([1e-12, 2e-7]);
|
||||
|
||||
%% Effect of sample forces on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ylim([1e-8, 1e-2])
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
|
||||
'DisplayName', 'OL');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', 'IFF + $k_p$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', 'IFF + HPF');
|
||||
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
|
||||
'DisplayName', 'RDC');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
|
||||
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
|
||||
xtickangle(0)
|
||||
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
ldg.ItemTokenSize = [20, 1];
|
||||
|
||||
linkaxes([ax1,ax2,ax3], 'y')
|
||||
ylim([1e-8, 1e-2])
|
||||
BIN
A2-nass-rotating-3dof-model/rotating_model.slx
Normal file
BIN
A2-nass-rotating-3dof-model/rotating_model.slx
Normal file
Binary file not shown.
@@ -0,0 +1,8 @@
|
||||
function [xi_min] = computeSimultaneousDamping(g, G, K)
|
||||
[~, xi] = damp(minreal(feedback(G, g*K), [], false));
|
||||
xi_min = 1/min(xi);
|
||||
|
||||
if xi_min < 0
|
||||
xi_min = 1e8;
|
||||
end
|
||||
end
|
||||
94
A2-nass-rotating-3dof-model/src/rootLocusPolesSorted.m
Normal file
94
A2-nass-rotating-3dof-model/src/rootLocusPolesSorted.m
Normal file
@@ -0,0 +1,94 @@
|
||||
function [poles] = rootLocusPolesSorted(G, K, gains, args)
|
||||
% rootLocusPolesSorted -
|
||||
%
|
||||
% Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - G, K, gains, args -
|
||||
%
|
||||
% Outputs:
|
||||
% - poles -
|
||||
|
||||
arguments
|
||||
G
|
||||
K
|
||||
gains
|
||||
args.minreal double {mustBeNumericOrLogical} = false
|
||||
args.p_half double {mustBeNumericOrLogical} = false
|
||||
args.d_max double {mustBeNumeric} = -1
|
||||
end
|
||||
|
||||
if args.minreal
|
||||
p1 = pole(minreal(feedback(G, gains(1)*K)));
|
||||
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
|
||||
p1 = p1(i_uniq);
|
||||
|
||||
poles = zeros(length(p1), length(gains));
|
||||
poles(:, 1) = p1;
|
||||
else
|
||||
p1 = pole(feedback(G, gains(1)*K));
|
||||
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
|
||||
p1 = p1(i_uniq);
|
||||
|
||||
poles = zeros(length(p1), length(gains));
|
||||
poles(:, 1) = p1;
|
||||
end
|
||||
|
||||
if args.minreal
|
||||
p2 = pole(minreal(feedback(G, gains(2)*K)));
|
||||
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
|
||||
p2 = p2(i_uniq);
|
||||
poles(:, 2) = p2;
|
||||
else
|
||||
p2 = pole(feedback(G, gains(2)*K));
|
||||
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
|
||||
p2 = p2(i_uniq);
|
||||
poles(:, 2) = p2;
|
||||
end
|
||||
|
||||
for g_i = 3:length(gains)
|
||||
% Estimated value of the poles
|
||||
poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
|
||||
|
||||
% New values for the poles
|
||||
poles_gi = pole(feedback(G, gains(g_i)*K));
|
||||
[~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
|
||||
poles_gi = poles_gi(i_uniq);
|
||||
|
||||
% Array of distances between all the poles
|
||||
poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));
|
||||
|
||||
% Get indices corresponding to distances from lowest to highest
|
||||
[~, c] = sort(min(poles_dist));
|
||||
|
||||
as = 1:length(poles_gi);
|
||||
|
||||
% for each column of poles_dist corresponding to the i'th pole
|
||||
% with closest previous poles
|
||||
for p_i = c
|
||||
% Get the indice a_i of the previous pole that is the closest
|
||||
% to pole c(p_i)
|
||||
[~, a_i] = min(poles_dist(:, p_i));
|
||||
|
||||
poles(as(a_i), g_i) = poles_gi(p_i);
|
||||
|
||||
% Remove old poles that are already matched
|
||||
% poles_gi(as(a_i), :) = [];
|
||||
poles_dist(a_i, :) = [];
|
||||
as(a_i) = [];
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
if args.d_max > 0
|
||||
poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
|
||||
end
|
||||
|
||||
if args.p_half
|
||||
poles = poles(1:round(end/2), :);
|
||||
end
|
||||
|
||||
[~, s_p] = sort(imag(poles(:,1)), 'descend');
|
||||
poles = poles(s_p, :);
|
||||
|
||||
poles = poles.';
|
||||
Reference in New Issue
Block a user