683 lines
26 KiB
Matlab
683 lines
26 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for data
|
|
addpath('./src/'); % Path for Functions
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Simscape model name
|
|
mdl = 'rotating_model';
|
|
|
|
%% 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)]);
|