572 lines
29 KiB
Matlab
572 lines
29 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
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector [Hz]
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
%% Load the PSD of disturbances
|
|
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
|
|
|
|
%% Load Plants Dynamics
|
|
load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
|
|
'G_vc_mid', 'G_md_mid', 'G_pz_mid', ...
|
|
'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
|
|
|
|
%% Load Damped Plants
|
|
load('uniaxial_damped_plants.mat', 'G_iff_vc_light', 'G_iff_md_light', 'G_iff_pz_light', ...
|
|
'G_rdc_vc_light', 'G_rdc_md_light', 'G_rdc_pz_light', ...
|
|
'G_dvf_vc_light', 'G_dvf_md_light', 'G_dvf_pz_light', ...
|
|
'G_iff_vc_mid', 'G_iff_md_mid', 'G_iff_pz_mid', ...
|
|
'G_rdc_vc_mid', 'G_rdc_md_mid', 'G_rdc_pz_mid', ...
|
|
'G_dvf_vc_mid', 'G_dvf_md_mid', 'G_dvf_pz_mid', ...
|
|
'G_iff_vc_heavy', 'G_iff_md_heavy', 'G_iff_pz_heavy', ...
|
|
'G_rdc_vc_heavy', 'G_rdc_md_heavy', 'G_rdc_pz_heavy', ...
|
|
'G_dvf_vc_heavy', 'G_dvf_md_heavy', 'G_dvf_pz_heavy');
|
|
|
|
%% Damped plant - Robustness to change of sample's mass
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_vc_mid( 'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:));
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:));
|
|
loglog(10.^(0.4*cos([0:0.01:2*pi])+log10(100)), ...
|
|
10.^(0.8*sin([0:0.01:2*pi]-pi/4)+log10(8e-8)), 'k--');
|
|
text(20, 4e-8, sprintf('Small\nInteraction'), 'horizontalalignment', 'center');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([5e-10, 1e-3]);
|
|
|
|
ax1b = nexttile();
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_mid( 'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
yticks(-360:90:360);
|
|
ylim([-200, 20]);
|
|
|
|
linkaxes([ax1,ax1b],'x');
|
|
xlim([1, 1e3]);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax2 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid( 'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:));
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:));
|
|
loglog(10.^(0.4*cos([0:0.01:2*pi])+log10(200)), ...
|
|
10.^(0.8*sin([0:0.01:2*pi]-pi/4)+log10(2e-8)), 'k--');
|
|
text(40, 1e-8, sprintf('Small\nInteraction'), 'horizontalalignment', 'center');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
ylim([5e-10, 1e-3]);
|
|
|
|
ax2b = nexttile();
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid( 'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
xticks([1e0, 1e1, 1e2]);
|
|
yticks(-360:90:360);
|
|
ylim([-200, 20]);
|
|
|
|
linkaxes([ax2,ax2b],'x');
|
|
xlim([1, 1e3]);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax3 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', '$m_s = 1\,kg$, OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$, IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_pz_mid( 'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,kg$, IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,kg$, IFF');
|
|
loglog(10.^(0.8*cos([0:0.01:2*pi])+log10(350)), ...
|
|
10.^(1.2*sin([0:0.01:2*pi])+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
|
|
text(200, 5e-7, sprintf('$\\mu$ Station\nCoupling'), 'horizontalalignment', 'center');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
ylim([5e-10, 1e-3]);
|
|
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
ldg.ItemTokenSize = [20, 1];
|
|
|
|
ax3b = nexttile();
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_mid( 'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
xticks([1e0, 1e1, 1e2]);
|
|
yticks(-360:90:360);
|
|
ylim([-200, 20]);
|
|
|
|
linkaxes([ax3,ax3b],'x');
|
|
xlim([1, 1e3]);
|
|
|
|
%% High Authority Controller - Soft Nano-Hexapod
|
|
% Lead to increase phase margin
|
|
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
wc = 2*pi*20; % Frequency with the maximum phase lead [rad/s]
|
|
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
|
|
|
% Lag at low frequency
|
|
H_lag = (s + 2*pi*5)/(s + 2*pi*0.01);
|
|
|
|
% Low Pass filter to increase robustness
|
|
H_lpf = 1/(1 + s/2/pi/200);
|
|
|
|
% High Authority Controller
|
|
K_hac_vc = 4e5 * ... % Gain
|
|
H_lead * ... % Lead
|
|
H_lag * ... % Lag
|
|
H_lpf; % LPF
|
|
K_hac_vc.InputName = {'d'};
|
|
K_hac_vc.OutputName = {'f'};
|
|
|
|
%% High Authority Controller - Mid Stiffness Nano-Hexapod
|
|
% Lead to increase phase margin
|
|
a = 4; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
wc = 2*pi*70; % Frequency with the maximum phase lead [rad/s]
|
|
|
|
H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
|
|
|
% Lag at low frequency
|
|
H_lag = ((s + 2*pi*15)/(s + 2*pi*0.01))^2;
|
|
|
|
% Low Pass filter to increase robustness
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
|
|
|
% High Authority Controller
|
|
K_hac_md = 3e6 * ... % Gain
|
|
H_lead * ... % Lead
|
|
H_lag * ... % Lag
|
|
H_lpf; % LPF
|
|
K_hac_md.InputName = {'d'};
|
|
K_hac_md.OutputName = {'f'};
|
|
|
|
%% High Authority Controller - Stiff Nano-Hexapod
|
|
% Lead to increase phase margin
|
|
a = 5; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s]
|
|
H_lead = ((1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))))^2;
|
|
|
|
% Integrator
|
|
H_int = 1/(s + 2*pi*0.01)^2;
|
|
|
|
% Low Pass filter to increase robustness
|
|
H_lpf = 1/(1 + s/2/pi/500);
|
|
|
|
% High Authority Controller
|
|
K_hac_pz = 6e12 * ... % Gain
|
|
H_lead * ... % Lead
|
|
H_int * ... % Lag
|
|
H_lpf; % LPF
|
|
K_hac_pz.InputName = {'d'};
|
|
K_hac_pz.OutputName = {'f'};
|
|
|
|
%% Save High Authority Controllers
|
|
save('./mat/uniaxial_high_authority_controllers.mat', ...
|
|
'K_hac_vc', 'K_hac_md', 'K_hac_pz');
|
|
|
|
%% Compute Loop gain for Nyquist Plot
|
|
L_vc_light = squeeze(freqresp(K_hac_vc*G_iff_vc_light('d', 'f'), freqs, 'Hz'));
|
|
L_vc_mid = squeeze(freqresp(K_hac_vc*G_iff_vc_mid( 'd', 'f'), freqs, 'Hz'));
|
|
L_vc_heavy = squeeze(freqresp(K_hac_vc*G_iff_vc_heavy('d', 'f'), freqs, 'Hz'));
|
|
|
|
L_md_light = squeeze(freqresp(K_hac_md*G_iff_md_light('d', 'f'), freqs, 'Hz'));
|
|
L_md_mid = squeeze(freqresp(K_hac_md*G_iff_md_mid( 'd', 'f'), freqs, 'Hz'));
|
|
L_md_heavy = squeeze(freqresp(K_hac_md*G_iff_md_heavy('d', 'f'), freqs, 'Hz'));
|
|
|
|
L_pz_light = squeeze(freqresp(K_hac_pz*G_iff_pz_light('d', 'f'), freqs, 'Hz'));
|
|
L_pz_mid = squeeze(freqresp(K_hac_pz*G_iff_pz_mid( 'd', 'f'), freqs, 'Hz'));
|
|
L_pz_heavy = squeeze(freqresp(K_hac_pz*G_iff_pz_heavy('d', 'f'), freqs, 'Hz'));
|
|
|
|
%% Nyquist Plot - Hight Authority Controller for all three nano-hexapod stiffnesses and all sample masses
|
|
figure;
|
|
hold on;
|
|
plot(real(L_vc_light), +imag(L_vc_light), '-', 'color', [colors(1,:), 0.5], 'DisplayName', '$k_n = 0.01\,N/\mu m$')
|
|
plot(real(L_vc_light), -imag(L_vc_light), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_vc_mid ), +imag(L_vc_mid ), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_vc_mid ), -imag(L_vc_mid ), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_vc_heavy), +imag(L_vc_heavy), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_vc_heavy), -imag(L_vc_heavy), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
|
|
|
|
plot(real(L_md_light), +imag(L_md_light), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$k_n = 1\,N/\mu m$')
|
|
plot(real(L_md_light), -imag(L_md_light), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_md_mid ), +imag(L_md_mid ), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_md_mid ), -imag(L_md_mid ), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_md_heavy), +imag(L_md_heavy), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_md_heavy), -imag(L_md_heavy), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
|
|
|
|
plot(real(L_pz_light), +imag(L_pz_light), '-', 'color', [colors(3,:), 0.5], 'DisplayName', '$k_n = 100\,N/\mu m$')
|
|
plot(real(L_pz_light), -imag(L_pz_light), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_pz_mid ), +imag(L_pz_mid ), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_pz_mid ), -imag(L_pz_mid ), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_pz_heavy), +imag(L_pz_heavy), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(real(L_pz_heavy), -imag(L_pz_heavy), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
|
axis square;
|
|
|
|
%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
|
|
figure;
|
|
hold on;
|
|
plot(real(L_vc_light), +imag(L_vc_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
|
|
plot(real(L_vc_light), -imag(L_vc_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
|
plot(real(L_vc_mid ), +imag(L_vc_mid ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
|
|
plot(real(L_vc_mid ), -imag(L_vc_mid ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
|
plot(real(L_vc_heavy), +imag(L_vc_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
|
|
plot(real(L_vc_heavy), -imag(L_vc_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
|
|
% Minimum modul margin
|
|
vc_mod_margin = min([min(abs(L_vc_light + 1)), min(abs(L_vc_mid + 1)), min(abs(L_vc_heavy + 1))]);
|
|
plot(-1 + vc_mod_margin*cos(linspace(0,2*pi,100)), vc_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', vc_mod_margin))
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
|
axis square;
|
|
|
|
%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
|
|
figure;
|
|
hold on;
|
|
plot(real(L_md_light), +imag(L_md_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
|
|
plot(real(L_md_light), -imag(L_md_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
|
plot(real(L_md_mid ), +imag(L_md_mid ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
|
|
plot(real(L_md_mid ), -imag(L_md_mid ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
|
plot(real(L_md_heavy), +imag(L_md_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
|
|
plot(real(L_md_heavy), -imag(L_md_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
|
|
% Minimum modul margin
|
|
md_mod_margin = min([min(abs(L_md_light + 1)), min(abs(L_md_mid + 1)), min(abs(L_md_heavy + 1))]);
|
|
plot(-1 + md_mod_margin*cos(linspace(0,2*pi,100)), md_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', md_mod_margin))
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
|
axis square;
|
|
|
|
%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
|
|
figure;
|
|
hold on;
|
|
plot(real(L_pz_light), +imag(L_pz_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
|
|
plot(real(L_pz_light), -imag(L_pz_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
|
plot(real(L_pz_mid ), +imag(L_pz_mid ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
|
|
plot(real(L_pz_mid ), -imag(L_pz_mid ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
|
|
plot(real(L_pz_heavy), +imag(L_pz_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
|
|
plot(real(L_pz_heavy), -imag(L_pz_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
|
|
% Minimum modul margin
|
|
pz_mod_margin = min([min(abs(L_pz_light + 1)), min(abs(L_pz_mid + 1)), min(abs(L_pz_heavy + 1))]);
|
|
plot(-1 + pz_mod_margin*cos(linspace(0,2*pi,100)), pz_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', pz_mod_margin))
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real'); ylabel('Imag');
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([-3.8, 0.2]); ylim([-2, 2]);
|
|
axis square;
|
|
|
|
%% Loop Gain - High Authority Controller - Relatively soft nano-hexapod
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(L_vc_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
|
|
plot(freqs, abs(L_vc_mid), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
|
|
plot(freqs, abs(L_vc_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e3]);
|
|
yticks([1e-2, 1, 1e2])
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(L_vc_light)), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(L_vc_mid )), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(L_vc_heavy)), 'color', colors(3,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360);
|
|
ylim([-225, -90]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 500]);
|
|
xticks([1, 10, 100]);
|
|
|
|
%% Loop Gain - High Authority Controller - Relatively stiff nano-hexapod
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(L_md_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
|
|
plot(freqs, abs(L_md_mid), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
|
|
plot(freqs, abs(L_md_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e3]);
|
|
yticks([1e-2, 1, 1e2])
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(L_md_light)), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(L_md_mid )), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(L_md_heavy)), 'color', colors(3,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360);
|
|
ylim([-225, -90]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 500]);
|
|
xticks([1, 10, 100]);
|
|
|
|
%% Loop Gain - High Authority Controller - Stiff nano-hexapod
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(L_pz_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
|
|
plot(freqs, abs(L_pz_mid), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
|
|
plot(freqs, abs(L_pz_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-3, 1e3]);
|
|
yticks([1e-2, 1, 1e2])
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*unwrap(angle(L_pz_light)), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(L_pz_mid )), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(L_pz_heavy)), 'color', colors(3,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360);
|
|
ylim([-225, -90]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 500]);
|
|
xticks([1, 10, 100]);
|
|
|
|
%% Compute Closed Loop Systems
|
|
G_hac_iff_vc_light = feedback(G_iff_vc_light, K_hac_vc, 'name', -1);
|
|
G_hac_iff_vc_mid = feedback(G_iff_vc_mid , K_hac_vc, 'name', -1);
|
|
G_hac_iff_vc_heavy = feedback(G_iff_vc_heavy, K_hac_vc, 'name', -1);
|
|
|
|
G_hac_iff_md_light = feedback(G_iff_md_light, K_hac_md, 'name', -1);
|
|
G_hac_iff_md_mid = feedback(G_iff_md_mid , K_hac_md, 'name', -1);
|
|
G_hac_iff_md_heavy = feedback(G_iff_md_heavy, K_hac_md, 'name', -1);
|
|
|
|
G_hac_iff_pz_light = feedback(G_iff_pz_light, K_hac_pz, 'name', -1);
|
|
G_hac_iff_pz_mid = feedback(G_iff_pz_mid , K_hac_pz, 'name', -1);
|
|
G_hac_iff_pz_heavy = feedback(G_iff_pz_heavy, K_hac_pz, 'name', -1);
|
|
|
|
%% Verify Stability
|
|
if not(isstable(G_hac_iff_vc_light) && isstable(G_hac_iff_vc_mid) && isstable(G_hac_iff_vc_heavy))
|
|
warning("One of the damped plant with VC and decentralized IFF is not stable.");
|
|
end
|
|
|
|
if not(isstable(G_hac_iff_md_light) && isstable(G_hac_iff_md_mid) && isstable(G_hac_iff_md_heavy))
|
|
warning("One of the damped plant with MD and decentralized IFF is not stable.");
|
|
end
|
|
|
|
if not(isstable(G_hac_iff_pz_light) && isstable(G_hac_iff_pz_mid) && isstable(G_hac_iff_pz_heavy))
|
|
warning("One of the damped plant with PZ and decentralized IFF is not stable.");
|
|
end
|
|
|
|
%% Change of sensitivity to disturbances with LAC and with HAC-LAC
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'd', 'fs'), freqs, 'Hz'))), 'k-');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(2,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
|
|
xlim([1, 500]);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'd', 'ft'), freqs, 'Hz'))), 'k-');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(2,:));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
|
|
xlim([1, 500]);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_md_mid( 'd', 'xf'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'HAC-IFF');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([1, 500]);
|
|
|
|
%% Cumulative Amplitude Spectrum for all three nano-hexapod stiffnesses - Comparison of OL, IFF and HAC-LAC cases
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5], 'DisplayName', 'OL');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5], 'HandleVisibility', 'off');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5], 'HandleVisibility', 'off');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5], 'DisplayName', 'IFF');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5], 'DisplayName', 'HAC-IFF');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([1, 500]);
|
|
ylim([2e-10, 3e-6])
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5]);
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
xlim([1, 500]);
|
|
ylim([2e-10, 3e-6])
|
|
|
|
figure;
|
|
hold on;
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [0,0,0,0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_iff_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(1,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5]);
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
|
|
psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
|
|
'color', [colors(2,:), 0.5]);
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
xlim([1, 500]);
|
|
ylim([2e-10, 3e-6])
|