Tangle matlab files
This commit is contained in:
parent
3c97176b6b
commit
c644ae7f8a
23
matlab/nass_1_kinematics.m
Normal file
23
matlab/nass_1_kinematics.m
Normal file
@ -0,0 +1,23 @@
|
||||
%% 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
|
||||
addpath('./STEPS/'); % Path for STEPS
|
||||
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
||||
|
||||
%% Data directory
|
||||
data_dir = './mat/';
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
515
matlab/nass_2_active_damping.m
Normal file
515
matlab/nass_2_active_damping.m
Normal file
@ -0,0 +1,515 @@
|
||||
%% 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
|
||||
addpath('./STEPS/'); % Path for STEPS
|
||||
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
||||
|
||||
%% Data directory
|
||||
data_dir = './mat/';
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector [Hz]
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
% IFF Plant
|
||||
% <<ssec:nass_active_damping_plant>>
|
||||
|
||||
% Transfer functions from actuator forces $f_i$ to force sensor measurements $f_{mi}$ are computed using the multi-body model.
|
||||
% Figure ref:fig:nass_iff_plant_effect_kp examines how parallel stiffness affects the plant dynamics, with identification performed at maximum spindle velocity $\Omega_z = 360\,\text{deg/s}$ and with a payload mass of 25 kg.
|
||||
|
||||
% Without parallel stiffness (Figure ref:fig:nass_iff_plant_no_kp), the dynamics exhibits non-minimum phase zeros at low frequency, confirming predictions from the three-degree-of-freedom rotating model.
|
||||
% Adding parallel stiffness (Figure ref:fig:nass_iff_plant_kp) transforms these into minimum phase complex conjugate zeros, enabling unconditionally stable decentralized IFF implementation.
|
||||
|
||||
% Though both cases show significant coupling around resonances, stability is guaranteed by the collocated arrangement of actuators and sensors [[cite:&preumont08_trans_zeros_struc_contr_with]].
|
||||
|
||||
|
||||
%% Identify the IFF plant dynamics using the Simscape model
|
||||
|
||||
% Initialize each Simscape model elements
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeSimplifiedNanoHexapod();
|
||||
|
||||
% Initial Simscape Configuration
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeDisturbances('enable', false);
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
initializeController('type', 'open-loop');
|
||||
initializeReferences();
|
||||
|
||||
% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
||||
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N]
|
||||
|
||||
%% Identify for multi payload masses (no rotation)
|
||||
initializeReferences(); % No Spindle Rotation
|
||||
% 1kg Sample
|
||||
initializeSample('type', 'cylindrical', 'm', 1);
|
||||
G_iff_m1 = linearize(mdl, io);
|
||||
G_iff_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
||||
G_iff_m1.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
||||
|
||||
% 25kg Sample
|
||||
initializeSample('type', 'cylindrical', 'm', 25);
|
||||
G_iff_m25 = linearize(mdl, io);
|
||||
G_iff_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
||||
G_iff_m25.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
||||
|
||||
% 50kg Sample
|
||||
initializeSample('type', 'cylindrical', 'm', 50);
|
||||
G_iff_m50 = linearize(mdl, io);
|
||||
G_iff_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
||||
G_iff_m50.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
||||
|
||||
%% Effect of Rotation
|
||||
initializeReferences(...
|
||||
'Rz_type', 'rotating', ...
|
||||
'Rz_period', 1); % 360 deg/s
|
||||
initializeSample('type', 'cylindrical', 'm', 25);
|
||||
G_iff_m25_Rz = linearize(mdl, io, 0.1);
|
||||
G_iff_m25_Rz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
||||
G_iff_m25_Rz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
||||
|
||||
%% Effect of Rotation - No added parallel stiffness
|
||||
initializeSimplifiedNanoHexapod('actuator_kp', 0);
|
||||
initializeReferences(...
|
||||
'Rz_type', 'rotating', ...
|
||||
'Rz_period', 1); % 360 deg/s
|
||||
initializeSample('type', 'cylindrical', 'm', 25);
|
||||
G_iff_m25_Rz_no_kp = linearize(mdl, io, 0.1);
|
||||
G_iff_m25_Rz_no_kp.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
||||
G_iff_m25_Rz_no_kp.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
||||
|
||||
%% IFF Plant - Without parallel stiffness
|
||||
f = logspace(-1,3,1000);
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,1), f, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$')
|
||||
for i = 2:6
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz'))), 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-4, 1e1]);
|
||||
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz')))), 'color', colors(1,:));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-20, 200]);
|
||||
yticks([0:45:180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([f(1), f(end)]);
|
||||
|
||||
%% IFF Plant - With added parallel stiffness
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), f, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - $k_p = 50N/mm$')
|
||||
for i = 2:6
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
'DisplayName', '$f_{ni}/f_j$ - $k_p = 50N/mm$')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-4, 1e1]);
|
||||
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-20, 200]);
|
||||
yticks([0:45:180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([f(1), f(end)]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:nass_iff_plant_effect_kp
|
||||
% #+caption: Effect of stiffness parallel to the force sensor on the IFF plant with $\Omega_z = 360\,\text{deg/s}$ and payload mass of 25kg. The dynamics without parallel stiffness has non-minimum phase zeros at low frequency (\subref{fig:nass_iff_plant_no_kp}). The added parallel stiffness transforms the non-minimum phase zeros to complex conjugate zeros (\subref{fig:nass_iff_plant_kp})
|
||||
% #+attr_latex: :options [htbp]
|
||||
% #+begin_figure
|
||||
% #+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_no_kp}without parallel stiffness}
|
||||
% #+attr_latex: :options {0.48\textwidth}
|
||||
% #+begin_subfigure
|
||||
% #+attr_latex: :width 0.95\linewidth
|
||||
% [[file:figs/nass_iff_plant_no_kp.png]]
|
||||
% #+end_subfigure
|
||||
% #+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_kp}with parallel stiffness}
|
||||
% #+attr_latex: :options {0.48\textwidth}
|
||||
% #+begin_subfigure
|
||||
% #+attr_latex: :width 0.95\linewidth
|
||||
% [[file:figs/nass_iff_plant_kp.png]]
|
||||
% #+end_subfigure
|
||||
% #+end_figure
|
||||
|
||||
% The effect of rotation, shown in Figure ref:fig:nass_iff_plant_effect_rotation, is negligible as the actuator stiffness ($k_a = 1\,N/\mu m$) is large compared to the negative stiffness induced by gyroscopic effects (estimated from the 3DoF rotating model).
|
||||
|
||||
% Figure ref:fig:nass_iff_plant_effect_payload illustrate the effect of payload mass on the plant dynamics.
|
||||
% While the poles and zeros are shifting with payload mass, the alternating pattern of poles and zeros is maintained, ensuring that the phase remains bounded between 0 and 180 degrees, and thus good robustness properties.
|
||||
|
||||
|
||||
%% Effect of spindle's rotation on the IFF Plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$ deg/s')
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
% plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
||||
% 'DisplayName', '$f_{ni}/f_j$')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-4, 1e2]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-20, 200]);
|
||||
yticks([0:45:180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Effect of the payload's mass on the IFF Plant
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - 1kg')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - 25kg')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'DisplayName', '$f_{ni}/f_i$ - 50kg')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-4, 1e2]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||||
end
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
||||
end
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-20, 200]);
|
||||
yticks([0:45:180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
% Controller Design
|
||||
% <<ssec:nass_active_damping_control>>
|
||||
|
||||
% Previous analysis using the 3DoF rotating model showed that decentralized Integral Force Feedback (IFF) with pure integrators is unstable due to gyroscopic effects caused by spindle rotation.
|
||||
% This finding is also confirmed with the multi-body model of the NASS: the system is unstable when using pure integrators and without parallel stiffness.
|
||||
|
||||
% This instability can be mitigated by introducing sufficient stiffness in parallel with the force sensors.
|
||||
% However, as illustrated in Figure ref:fig:nass_iff_plant_kp, adding parallel stiffness increases the low frequency gain.
|
||||
% If using pure integrators, this would results in high loop gain at low frequencies, adversely affecting the damped plant dynamics, which is undesirable.
|
||||
% To resolve this issue, a second-order high-pass filter is introduced to limit the low frequency gain, as shown in Equation eqref:eq:nass_kiff.
|
||||
|
||||
% \begin{equation}\label{eq:nass_kiff}
|
||||
% \bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix}
|
||||
% K_{\text{IFF}}(s) & & 0 \\
|
||||
% & \ddots & \\
|
||||
% 0 & & K_{\text{IFF}}(s)
|
||||
% \end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s} \cdot \frac{\frac{s^2}{\omega_z^2}}{\frac{s^2}{\omega_z^2} + 2 \xi_z \frac{s}{\omega_z} + 1}
|
||||
% \end{equation}
|
||||
|
||||
% The cut-off frequency of the second-order high-pass filter is tuned to be below the frequency of the complex conjugate zero for the highest mass, which is at $5\,\text{Hz}$.
|
||||
% The overall gain is then increased to have large loop gain around resonances to be damped, as illustrated in Figure ref:fig:nass_iff_loop_gain.
|
||||
|
||||
|
||||
%% Verify that parallel stiffness permits to have a stable plant
|
||||
Kiff_pure_int = -200/s*eye(6);
|
||||
isstable(feedback(G_iff_m25_Rz, Kiff_pure_int, 1))
|
||||
isstable(feedback(G_iff_m25_Rz_no_kp, Kiff_pure_int, 1))
|
||||
|
||||
%% IFF Controller Design
|
||||
% Second order high pass filter
|
||||
wz = 2*pi*2;
|
||||
xiz = 0.7;
|
||||
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);
|
||||
|
||||
Kiff = -200 * ... % Gain
|
||||
1/(0.01*2*pi + s) * ... % LPF: provides integral action
|
||||
Ghpf * ... % 2nd order HPF (limit low frequency gain)
|
||||
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
|
||||
|
||||
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
|
||||
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
||||
|
||||
% The designed IFF controller is saved
|
||||
save('./mat/nass_K_iff.mat', 'Kiff');
|
||||
|
||||
%% Loop gain for the decentralized IFF
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'DisplayName', '1kg')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'DisplayName', '25kg')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'DisplayName', '50kg')
|
||||
for i = 2:6
|
||||
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-4, 1e2]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||||
end
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
||||
end
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-110, 200]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:nass_iff_loop_gain
|
||||
% #+caption: Loop gain for the decentralized IFF: $K_{\text{IFF}}(s) \cdot \frac{f_{mi}}{f_i}(s)$
|
||||
% #+RESULTS:
|
||||
% [[file:figs/nass_iff_loop_gain.png]]
|
||||
|
||||
% To verify stability, root loci for the three payload configurations are computed and shown in Figure ref:fig:nass_iff_root_locus.
|
||||
% The results demonstrate that the closed-loop poles remain within the left-half plane, indicating the robust stability properties of the applied decentralized IFF.
|
||||
|
||||
|
||||
%% Root Locus for the Decentralized IFF controller - 1kg Payload
|
||||
figure;
|
||||
gains = logspace(-2, 1, 200);
|
||||
|
||||
figure;
|
||||
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||||
nexttile();
|
||||
hold on;
|
||||
|
||||
plot(real(pole(G_iff_m1)), imag(pole(G_iff_m1)), 'x', 'color', colors(1,:), ...
|
||||
'DisplayName', '$g = 0$');
|
||||
plot(real(tzero(G_iff_m1)), imag(tzero(G_iff_m1)), 'o', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
|
||||
for g = gains
|
||||
clpoles = pole(feedback(G_iff_m1, g*Kiff, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% Optimal gain
|
||||
clpoles = pole(feedback(G_iff_m1, Kiff, +1));
|
||||
plot(real(clpoles), imag(clpoles), 'kx', ...
|
||||
'DisplayName', '$g_{opt}$');
|
||||
|
||||
xline(0);
|
||||
yline(0);
|
||||
hold off;
|
||||
axis equal;
|
||||
xlim([-900, 100]); ylim([-100, 900]);
|
||||
xticks([-900:100:0]);
|
||||
yticks([0:100:900]);
|
||||
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||||
xlabel('Real part'); ylabel('Imaginary part');
|
||||
|
||||
%% Root Locus for the Decentralized IFF controller - 25kg Payload
|
||||
gains = logspace(-2, 1, 200);
|
||||
|
||||
figure;
|
||||
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||||
nexttile();
|
||||
hold on;
|
||||
|
||||
plot(real(pole(G_iff_m25)), imag(pole(G_iff_m25)), 'x', 'color', colors(2,:), ...
|
||||
'DisplayName', '$g = 0$');
|
||||
plot(real(tzero(G_iff_m25)), imag(tzero(G_iff_m25)), 'o', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
|
||||
for g = gains
|
||||
clpoles = pole(feedback(G_iff_m25, g*Kiff, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% Optimal gain
|
||||
clpoles = pole(feedback(G_iff_m25, Kiff, +1));
|
||||
plot(real(clpoles), imag(clpoles), 'kx', ...
|
||||
'DisplayName', '$g_{opt}$');
|
||||
|
||||
xline(0);
|
||||
yline(0);
|
||||
hold off;
|
||||
axis equal;
|
||||
xlim([-900, 100]); ylim([-100, 900]);
|
||||
xticks([-900:100:0]);
|
||||
yticks([0:100:900]);
|
||||
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||||
xlabel('Real part'); ylabel('Imaginary part');
|
||||
|
||||
%% Root Locus for the Decentralized IFF controller - 50kg Payload
|
||||
gains = logspace(-2, 1, 200);
|
||||
|
||||
figure;
|
||||
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||||
nexttile();
|
||||
hold on;
|
||||
|
||||
plot(real(pole(G_iff_m50)), imag(pole(G_iff_m50)), 'x', 'color', colors(3,:), ...
|
||||
'DisplayName', '$g = 0$');
|
||||
plot(real(tzero(G_iff_m50)), imag(tzero(G_iff_m50)), 'o', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
|
||||
for g = gains
|
||||
clpoles = pole(feedback(G_iff_m50, g*Kiff, +1));
|
||||
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
|
||||
% Optimal gain
|
||||
clpoles = pole(feedback(G_iff_m50, Kiff, +1));
|
||||
plot(real(clpoles), imag(clpoles), 'kx', ...
|
||||
'DisplayName', '$g_{opt}$');
|
||||
|
||||
xline(0);
|
||||
yline(0);
|
||||
hold off;
|
||||
axis equal;
|
||||
xlim([-900, 100]); ylim([-100, 900]);
|
||||
xticks([-900:100:0]);
|
||||
yticks([0:100:900]);
|
||||
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||||
xlabel('Real part'); ylabel('Imaginary part');
|
||||
1105
matlab/nass_3_hac.m
Normal file
1105
matlab/nass_3_hac.m
Normal file
File diff suppressed because it is too large
Load Diff
@ -248,19 +248,30 @@ One big advantage of doing the control in the cartesian plane, is that we don't
|
||||
Maybe this should be done *here*.
|
||||
Here it can be reminded when doing the control in the cartesian frame.
|
||||
|
||||
** TODO [#B] Determine which .mat files are used and which are not
|
||||
** DONE [#B] Determine which .mat files are used and which are not
|
||||
CLOSED: [2025-02-17 Mon 23:04]
|
||||
|
||||
#+begin_src matlab :eval no :tangle no
|
||||
load("nass_model_conf_log.mat");
|
||||
load("nass_model_conf_simscape.mat");
|
||||
dist = load("nass_model_disturbances.mat");
|
||||
load("nass_model_references.mat");
|
||||
load("nass_model_controller.mat");
|
||||
load("nass_model_stages.mat");
|
||||
J_L_to_X = inv(nano_hexapod.geometry.J);
|
||||
#+end_src
|
||||
|
||||
- [ ] matlab/mat/conf_log.mat
|
||||
- [ ] matlab/mat/conf_simscape.mat
|
||||
- [ ] matlab/mat/conf_simulink.mat
|
||||
- [ ] matlab/mat/nano_hexapod.mat
|
||||
- [ ] matlab/mat/nass_disturbances.mat
|
||||
- [ ] matlab/mat/nass_model_conf_log.mat
|
||||
- [ ] matlab/mat/nass_model_conf_simscape.mat
|
||||
- [ ] matlab/mat/nass_model_controller.mat
|
||||
- [ ] matlab/mat/nass_model_disturbances.mat
|
||||
- [ ] matlab/mat/nass_model_references.mat
|
||||
- [ ] matlab/mat/nass_model_stages.mat
|
||||
- [X] matlab/mat/nass_model_conf_log.mat
|
||||
- [X] matlab/mat/nass_model_conf_simscape.mat
|
||||
- [X] matlab/mat/nass_model_controller.mat
|
||||
- [X] matlab/mat/nass_model_disturbances.mat
|
||||
- [X] matlab/mat/nass_model_references.mat
|
||||
- [X] matlab/mat/nass_model_stages.mat
|
||||
- [ ] matlab/mat/nass_references.mat
|
||||
- [ ] matlab/mat/nass_stages.mat
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user