73 lines
3.2 KiB
Matlab
73 lines
3.2 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);
|
|
|
|
% Range validity of the approximate inverse kinematics
|
|
|
|
% The accuracy of the Jacobian-based forward kinematics solution was estimated through a systematic error analysis.
|
|
% For a series of platform positions along the $x\text{-axis}$, the exact strut lengths are computed using the analytical inverse kinematics equation eqref:eq:nhexa_inverse_kinematics.
|
|
% These strut lengths are then used with the Jacobian to estimate the platform pose, from which the error between the estimated and true poses can be calculated.
|
|
|
|
% The estimation errors in the $x$, $y$, and $z$ directions are shown in Figure ref:fig:nhexa_forward_kinematics_approximate_errors.
|
|
% The results demonstrate that for displacements up to approximately $1\,\%$ of the hexapod's size (which corresponds to $100\,\mu m$ as the size of the Stewart platform is here $\approx 100\,mm$), the Jacobian approximation provides excellent accuracy.
|
|
|
|
% This finding has particular significance for the Nano-hexapod application.
|
|
% Since the maximum required stroke ($\approx 100\,\mu m$) is three orders of magnitude smaller than the stewart platform size ($\approx 100\,mm$), the Jacobian matrix can be considered constant throughout the workspace.
|
|
% It can be computed once at the rest position and used for both forward and inverse kinematics with high accuracy.
|
|
|
|
|
|
%% Estimate the errors associated with approximate forward kinematics using the Jacobian matrix
|
|
stewart = initializeSimplifiedNanoHexapod('H', 100e-3, 'MO_B', 0);
|
|
|
|
Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]
|
|
|
|
% Compute the strut exact length for each X-position
|
|
Ls_exact = zeros(6, length(Xrs));
|
|
for i = 1:length(Xrs)
|
|
[~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xrs(i); 0; 0]);
|
|
end
|
|
|
|
% From the strut length, compute the stewart pose using the Jacobian matrix
|
|
Xrs_approx = zeros(6, length(Xrs));
|
|
for i = 1:length(Xrs)
|
|
Xrs_approx(:, i) = inv(stewart.geometry.J)*L_exact(:, i);
|
|
end
|
|
|
|
%% Errors associated with the use of the Jacobian matrix to solve the forward kinematic problem
|
|
figure;
|
|
hold on;
|
|
plot(1e6*Xrs, 1e9*abs(Xrs - Xrs_approx(1, :)), 'DisplayName', '$\epsilon_x$');
|
|
plot(1e6*Xrs, 1e9*abs(Xrs_approx(2, :)), 'DisplayName', '$\epsilon_y$');
|
|
plot(1e6*Xrs, 1e9*abs(Xrs_approx(3, :)), 'DisplayName', '$\epsilon_z$');
|
|
plot(1e6*Xrs, 1e6*Xrs, 'k--', 'DisplayName', '$0.1\%$ error');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]); ylim([1, 1e4]);
|
|
xlabel('$D_x$ motion');
|
|
ylabel('Kinematic Errors');
|
|
xticks([1, 10, 100, 1000, 10000]);
|
|
yticks([1, 10, 100, 1000, 10000]);
|
|
xticklabels({'$1\mu m$', '$10\mu m$', '$100\mu m$', '$1mm$', '$10mm$'});
|
|
yticklabels({'$1nm$', '$10nm$', '$100nm$', '$1\mu m$', '$10\mu m$'});
|