294 lines
13 KiB
Matlab
294 lines
13 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
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Data directory
|
|
data_dir = './mat/';
|
|
|
|
% Simulink Model name
|
|
mdl = 'nano_hexapod_model';
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
% Validation of the multi-body model
|
|
% <<ssec:nhexa_model_validation>>
|
|
|
|
% The developed multi-body model of the Stewart platform is represented schematically in Figure ref:fig:nhexa_stewart_model_input_outputs, highlighting the key inputs and outputs: actuator forces $\bm{f}$, force sensor measurements $\bm{f}_n$, and relative displacement measurements $\bm{\mathcal{L}}$.
|
|
% The frames $\{F\}$ and $\{M\}$ serve as interfaces for integration with other elements in the multi-body system.
|
|
% A three-dimensional visualization of the model is presented in Figure ref:fig:nhexa_simscape_screenshot.
|
|
|
|
% #+attr_latex: :options [b]{0.6\linewidth}
|
|
% #+begin_minipage
|
|
% #+name: fig:nhexa_stewart_model_input_outputs
|
|
% #+caption: Nano-Hexapod plant with inputs and outputs. Frames $\{F\}$ and $\{M\}$ can be connected to other elements in the multi-body models.
|
|
% #+attr_latex: :scale 1 :float nil
|
|
% [[file:figs/nhexa_stewart_model_input_outputs.png]]
|
|
% #+end_minipage
|
|
% \hfill
|
|
% #+attr_latex: :options [b]{0.35\linewidth}
|
|
% #+begin_minipage
|
|
% #+name: fig:nhexa_simscape_screenshot
|
|
% #+caption: 3D representation of the multi-body model
|
|
% #+attr_latex: :width 0.90\linewidth :float nil
|
|
% [[file:figs/nhexa_simscape_screenshot.jpg]]
|
|
% #+end_minipage
|
|
|
|
% The validation of the multi-body model is performed using the simplest Stewart platform configuration, enabling direct comparison with the analytical transfer functions derived in Section ref:ssec:nhexa_stewart_platform_dynamics.
|
|
% This configuration consists of massless universal joints at the base, massless spherical joints at the top platform, and massless struts with stiffness $k_a = 1\,\text{N}/\mu\text{m}$ and damping $c_a = 10\,\text{N}/({\text{m}/\text{s}})$.
|
|
% The geometric parameters remain as specified in Table ref:tab:nhexa_actuator_parameters.
|
|
|
|
% While the moving platform itself is considered massless, a $10\,\text{kg}$ cylindrical payload is mounted on top with a radius of $r = 110\,mm$ and a height $h = 300\,mm$.
|
|
|
|
% For the analytical model, the stiffness, damping and mass matrices are defined in eqref:eq:nhexa_analytical_matrices.
|
|
|
|
% \begin{subequations}\label{eq:nhexa_analytical_matrices}
|
|
% \begin{align}
|
|
% \bm{\mathcal{K}} &= \text{diag}(k_a,\ k_a,\ k_a,\ k_a,\ k_a,\ k_a) \\
|
|
% \bm{\mathcal{C}} &= \text{diag}(c_a,\ c_a,\ c_a,\ c_a,\ c_a,\ c_a) \\
|
|
% \bm{M} &= \text{diag}\left(m,\ m,\ m,\ \frac{1}{12}m(3r^2 + h^2),\ \frac{1}{12}m(3r^2 + h^2),\ \frac{1}{2}mr^2\right)
|
|
% \end{align}
|
|
% \end{subequations}
|
|
|
|
% The transfer functions from actuator forces to strut displacements are computed using these matrices according to equation eqref:eq:nhexa_transfer_function_struts.
|
|
% These analytical transfer functions are then compared with those extracted from the multi-body model.
|
|
% The multi-body model yields a state-space representation with 12 states, corresponding to the six degrees of freedom of the moving platform.
|
|
|
|
% Figure ref:fig:nhexa_comp_multi_body_analytical presents a comparison between the analytical and multi-body transfer functions, specifically showing the response from the first actuator force to all six strut displacements.
|
|
% The close agreement between both approaches across the frequency spectrum validates the multi-body model's accuracy in capturing the system's dynamic behavior.
|
|
|
|
|
|
%% Plant using Analytical Equations
|
|
% Stewart platform definition
|
|
k = 1e6; % Actuator stiffness [N/m]
|
|
c = 1e1; % Actuator damping [N/(m/s)]
|
|
|
|
stewart = initializeSimplifiedNanoHexapod(...
|
|
'Mpm', 1e-3, ...
|
|
'actuator_type', '1dof', ...
|
|
'actuator_k', k, ...
|
|
'actuator_kp', 0, ...
|
|
'actuator_c', c ...
|
|
);
|
|
|
|
% Payload: Cylinder
|
|
h = 300e-3; % Height of the cylinder [m]
|
|
r = 110e-3; % Radius of the cylinder [m]
|
|
m = 10; % Mass of the payload [kg]
|
|
initializeSample('type', 'cylindrical', 'm', m, 'H', h, 'R', r);
|
|
|
|
% Mass Matrix
|
|
M = zeros(6,6);
|
|
M(1,1) = m;
|
|
M(2,2) = m;
|
|
M(3,3) = m;
|
|
M(4,4) = 1/12*m*(3*r^2 + h^2);
|
|
M(5,5) = 1/12*m*(3*r^2 + h^2);
|
|
M(6,6) = 1/2*m*r^2;
|
|
|
|
% Stiffness and Damping matrices
|
|
K = k*eye(6);
|
|
C = c*eye(6);
|
|
|
|
% Compute plant in the frame of the struts
|
|
G_analytical = inv(ss(inv(stewart.geometry.J')*M*inv(stewart.geometry.J)*s^2 + C*s + K));
|
|
|
|
% Compare with Simscape model
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
|
|
% 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, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Encoders [m]
|
|
|
|
G_simscape = linearize(mdl, io);
|
|
G_simscape.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_simscape.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
|
|
|
|
%% Comparison of the analytical transfer functions and the multi-body model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_simscape(i,1), freqs, 'Hz'))), 'color', [colors(i,:), 0.5], ...
|
|
'DisplayName', sprintf('$l_%i/f_1$ - Multi-Body', i))
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_analytical(i,1), freqs, 'Hz'))), '--', 'color', [colors(i,:)], ...
|
|
'DisplayName', sprintf('$l_%i/f_1$ - Analytical', i))
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 6, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_simscape(i,1), freqs, 'Hz'))), 'color', [colors(i,:),0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_analytical(i,1), freqs, 'Hz'))), '--', 'color', colors(i,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
% Nano Hexapod Dynamics
|
|
% <<ssec:nhexa_model_dynamics>>
|
|
|
|
% Following the validation of the multi-body model, a detailed analysis of the nano-hexapod dynamics has been performed.
|
|
% The model parameters are set according to the specifications outlined in Section ref:ssec:nhexa_model_def, with a payload mass of $10\,kg$.
|
|
% Transfer functions from actuator forces $\bm{f}$ to both strut displacements $\bm{\mathcal{L}}$ and force measurements $\bm{f}_n$ are derived from the multi-body model.
|
|
|
|
% The transfer functions relating actuator forces to strut displacements are presented in Figure ref:fig:nhexa_multi_body_plant_dL.
|
|
% Due to the system's symmetrical design and identical strut configurations, all diagonal terms (transfer functions from force $f_i$ to displacement $l_i$ of the same strut) exhibit identical behavior.
|
|
% While the system possesses six degrees of freedom, only four distinct resonance frequencies are observed in the frequency response.
|
|
% This reduction from six to four observable modes is attributed to the system's symmetry, where two pairs of resonances occur at identical frequencies.
|
|
|
|
% The system's behavior can be characterized in three frequency regions.
|
|
% At low frequencies, well below the first resonance, the plant demonstrates good decoupling between actuators, with the response dominated by the strut stiffness: $\bm{G}(j\omega) \xrightarrow[\omega \to 0]{} \bm{\mathcal{K}}^{-1}$.
|
|
% In the mid-frequency range, the system exhibits coupled dynamics through its resonant modes, reflecting the complex interactions between the platform's degrees of freedom.
|
|
% At high frequencies, above the highest resonance, the response is governed by the payload's inertia mapped to the strut coordinates: $\bm{G}(j\omega) \xrightarrow[\omega \to \infty]{} \bm{J} \bm{M}^{-T} \bm{J}^T \frac{-1}{\omega^2}$
|
|
|
|
% The force sensor transfer functions, shown in Figure ref:fig:nhexa_multi_body_plant_fm, display characteristics typical of collocated actuator-sensor pairs.
|
|
% Each actuator's transfer function to its associated force sensor exhibits alternating complex conjugate poles and zeros.
|
|
% The inclusion of parallel stiffness introduces an additional complex conjugate zero at low frequency, a feature previously observed in the three-degree-of-freedom rotating model.
|
|
|
|
|
|
%% Multi-Body model of the Nano-Hexapod
|
|
% Initialize 1DoF
|
|
initializeSimplifiedNanoHexapod('flex_type_F', '2dof', 'flex_type_M', '3dof', 'actuator_type', '1dof');
|
|
initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
|
|
% 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, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Encoders [m]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N]
|
|
|
|
% With no payload
|
|
G = linearize(mdl, io);
|
|
G.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
|
'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
%% Multi-Body model of the Nano-Hexapod without parallel stiffness
|
|
% Initialize 1DoF
|
|
initializeSimplifiedNanoHexapod('flex_type_F', '2dof', 'flex_type_M', '3dof', 'actuator_type', '1dof', 'actuator_kp', 0);
|
|
|
|
% With no payload
|
|
G_no_kp = linearize(mdl, io);
|
|
G_no_kp.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_no_kp.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
|
'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
%% Transfer function from actuator force inputs to displacement of each strut
|
|
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(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$l_i/f_i$')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$l_i/f_j$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(i,i), freqs, 'Hz'))), 'color', [colors(1,:),0.5]);
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
%% Transfer function from actuator force inputs to force sensor in each strut
|
|
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(6+i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(7,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$')
|
|
plot(freqs, abs(squeeze(freqresp(G_no_kp(7,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ (no $k_p$)')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G(6+i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_no_kp(6+i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(7,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', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(6+i,i), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp(6+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([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|