1427 lines
70 KiB
Org Mode
1427 lines
70 KiB
Org Mode
#+TITLE: Decoupling Properties of the Cubic Architecture
|
|
:DRAWER:
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments no
|
|
#+PROPERTY: header-args:matlab+ :exports none
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :noweb yes
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
#+PROPERTY: header-args:matlab+ :tangle dehaeze26_cubic_architecture.m
|
|
:END:
|
|
|
|
* Matlab Init :noexport:ignore:
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes :results silent
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes :results silent
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes :results silent
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
* Introduction :ignore:
|
|
|
|
The Cubic configuration for the Stewart platform was first proposed by Dr. Gough in a comment to the original paper by Dr. Stewart [[cite:&stewart65_platf_with_six_degrees_freed]].
|
|
This configuration is characterized by active struts arranged in a mutually orthogonal configuration connecting the corners of a cube, as shown in Figure ref:fig:detail_kinematics_cubic_architecture_example.
|
|
|
|
Typically, the struts have similar length to the cube's edges, as illustrated in Figure ref:fig:detail_kinematics_cubic_architecture_example.
|
|
Practical implementations of such configurations can be observed in Figures ref:fig:detail_kinematics_jpl, ref:fig:detail_kinematics_uw_gsp and ref:fig:detail_kinematics_uqp.
|
|
It is also possible to implement designs with strut lengths smaller than the cube's edges (Figure ref:fig:detail_kinematics_cubic_architecture_example_small), as exemplified in Figure ref:fig:detail_kinematics_ulb_pz.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Example of a typical "cubic" architecture
|
|
MO_B = -50e-3; % Position {B} with respect to {M} [m]
|
|
|
|
H = 100e-3; % Height of the Stewart platform [m]
|
|
|
|
Hc = 100e-3; % Size of the useful part of the cube [m]
|
|
|
|
FOc = H + MO_B; % Center of the cube with respect to {F}
|
|
% here positionned at the frame {B}
|
|
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1);
|
|
stewart = initializeJointDynamics(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
|
|
displayArchitecture(stewart, 'labels', false, 'frames', false);
|
|
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false);
|
|
view([40, 5]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_architecture_example.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Example of a typical "cubic" architecture
|
|
MO_B = -20e-3; % Position {B} with respect to {M} [m]
|
|
|
|
H = 40e-3; % Height of the Stewart platform [m]
|
|
|
|
Hc = 100e-3; % Size of the useful part of the cube [m]
|
|
|
|
FOc = H + MO_B; % Center of the cube with respect to {F}
|
|
% here positionned at the frame {B}
|
|
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);
|
|
|
|
displayArchitecture(stewart, 'labels', false, 'frames', false);
|
|
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false);
|
|
view([40, 5]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_architecture_example_small.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_architecture_examples
|
|
#+caption: Typical Stewart platform cubic architectures in which struts' length is similar to the cube edges's length (\subref{fig:detail_kinematics_cubic_architecture_example}) or is taking just a portion of the edge (\subref{fig:detail_kinematics_cubic_architecture_example_small}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example}Classical Cubic architecture}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/detail_kinematics_cubic_architecture_example.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example_small}Alternative configuration}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/detail_kinematics_cubic_architecture_example_small.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
|
|
Several advantageous properties attributed to the cubic configuration have contributed to its widespread adoption [[cite:&geng94_six_degree_of_freed_activ;&preumont07_six_axis_singl_stage_activ;&jafari03_orthog_gough_stewar_platf_microm]]: simplified kinematics relationships and dynamical analysis [[cite:&geng94_six_degree_of_freed_activ]]; uniform stiffness in all directions [[cite:&hanieh03_activ_stewar]]; uniform mobility [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition, chapt.8.5.2]]; and minimization of the cross coupling between actuators and sensors in different struts [[cite:&preumont07_six_axis_singl_stage_activ]].
|
|
This minimization is attributed to the fact that the struts are orthogonal to each other, and is said to facilitate collocated sensor-actuator control system design, i.e., the implementation of decentralized control [[cite:&geng94_six_degree_of_freed_activ;&thayer02_six_axis_vibrat_isolat_system]].
|
|
|
|
These properties are examined in this section to assess their relevance for the nano-hexapod.
|
|
The mobility and stiffness properties of the cubic configuration are analyzed in Section ref:ssec:detail_kinematics_cubic_static.
|
|
Dynamical decoupling is investigated in Section ref:ssec:detail_kinematics_cubic_dynamic, while decentralized control, crucial for the NASS, is examined in Section ref:ssec:detail_kinematics_decentralized_control.
|
|
Given that the cubic architecture imposes strict geometric constraints, alternative designs are proposed in Section ref:ssec:detail_kinematics_cubic_design.
|
|
The ultimate objective is to determine the suitability of the cubic architecture for the nano-hexapod.
|
|
|
|
* Static Properties
|
|
<<ssec:detail_kinematics_cubic_static>>
|
|
*** Stiffness matrix for the Cubic architecture
|
|
|
|
Consider the cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_schematic_full.
|
|
The unit vectors corresponding to the edges of the cube are described by equation eqref:eq:detail_kinematics_cubic_s.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cubic_s}
|
|
\hat{\bm{s}}_1 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
|
|
\hat{\bm{s}}_2 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
|
|
\hat{\bm{s}}_3 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
|
|
\hat{\bm{s}}_4 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
|
|
\hat{\bm{s}}_5 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
|
|
\hat{\bm{s}}_6 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+name: fig:detail_kinematics_cubic_schematic_cases
|
|
#+caption: Cubic architecture. Struts are represented in blue. The cube's center by a black dot. The Struts can match the cube's edges (\subref{fig:detail_kinematics_cubic_schematic_full}) or just take a portion of the edge (\subref{fig:detail_kinematics_cubic_schematic})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic_full}Full cube}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.9
|
|
[[file:figs/detail_kinematics_cubic_schematic_full.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic}Cube's portion}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.9
|
|
[[file:figs/detail_kinematics_cubic_schematic.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
Coordinates of the cube's vertices relevant for the top joints, expressed with respect to the cube's center, are shown in equation eqref:eq:detail_kinematics_cubic_vertices.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cubic_vertices}
|
|
\tilde{\bm{b}}_1 = \tilde{\bm{b}}_2 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{-\sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad
|
|
\tilde{\bm{b}}_3 = \tilde{\bm{b}}_4 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{ \sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad
|
|
\tilde{\bm{b}}_5 = \tilde{\bm{b}}_6 = H_c \begin{bmatrix} \frac{-2}{\sqrt{2}} \\ 0 \\ \frac{1}{2} \end{bmatrix}
|
|
\end{equation}
|
|
|
|
In the case where top joints are positioned at the cube's vertices, a diagonal stiffness matrix is obtained as shown in equation eqref:eq:detail_kinematics_cubic_stiffness.
|
|
Translation stiffness is twice the stiffness of the struts, and rotational stiffness is proportional to the square of the cube's size $H_c$.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cubic_stiffness}
|
|
\bm{K}_{\{B\} = \{C\}} = k \begin{bmatrix}
|
|
2 & 0 & 0 & 0 & 0 & 0 \\
|
|
0 & 2 & 0 & 0 & 0 & 0 \\
|
|
0 & 0 & 2 & 0 & 0 & 0 \\
|
|
0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 & 0 \\
|
|
0 & 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 \\
|
|
0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
However, typically, the top joints are not placed at the cube's vertices but at positions along the cube's edges (Figure ref:fig:detail_kinematics_cubic_schematic).
|
|
In that case, the location of the top joints can be expressed by equation eqref:eq:detail_kinematics_cubic_edges, yet the computed stiffness matrix remains identical to Equation eqref:eq:detail_kinematics_cubic_stiffness.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cubic_edges}
|
|
\bm{b}_i = \tilde{\bm{b}}_i + \alpha \hat{\bm{s}}_i
|
|
\end{equation}
|
|
|
|
|
|
The stiffness matrix is therefore diagonal when the considered $\{B\}$ frame is located at the center of the cube (shown by frame $\{C\}$).
|
|
This means that static forces (resp torques) applied at the cube's center will induce pure translations (resp rotations around the cube's center).
|
|
This specific location where the stiffness matrix is diagonal is referred to as the "Center of Stiffness" (analogous to the "Center of Mass" where the mass matrix is diagonal).
|
|
|
|
#+begin_src matlab
|
|
%% Analytical formula for Stiffness matrix of the Cubic Stewart platform
|
|
% Define symbolic variables
|
|
syms k Hc alpha H
|
|
|
|
assume(k > 0); % k is positive real
|
|
assume(Hc, 'real'); % Hc is real
|
|
assume(H, 'real'); % H is real
|
|
assume(alpha, 'real'); % alpha is real
|
|
|
|
% Define si matrix (edges of the cubes)
|
|
si = 1/sqrt(3)*[
|
|
[ sqrt(2), 0, 1]; ...
|
|
[-sqrt(2)/2, -sqrt(3/2), 1]; ...
|
|
[-sqrt(2)/2, sqrt(3/2), 1]; ...
|
|
[ sqrt(2), 0, 1]; ...
|
|
[-sqrt(2)/2, -sqrt(3/2), 1]; ...
|
|
[-sqrt(2)/2, sqrt(3/2), 1] ...
|
|
];
|
|
|
|
% Define ci matrix (vertices of the cubes)
|
|
ci = Hc * [
|
|
[1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
|
|
[1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
|
|
[1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ...
|
|
[1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ...
|
|
[-sqrt(2), 0, 0.5]; ...
|
|
[-sqrt(2), 0, 0.5] ...
|
|
];
|
|
|
|
% Apply vertical shift to ci
|
|
ci = ci + H * [0, 0, 1];
|
|
|
|
% Calculate bi vectors (Stewart platform top joints)
|
|
bi = ci + alpha * si;
|
|
|
|
% Initialize stiffness matrix
|
|
K = sym(zeros(6,6));
|
|
|
|
% Calculate elements of the stiffness matrix
|
|
for i = 1:6
|
|
% Extract vectors for each leg
|
|
s_i = si(i,:)';
|
|
b_i = bi(i,:)';
|
|
|
|
% Calculate cross product vector
|
|
cross_bs = cross(b_i, s_i);
|
|
|
|
% Build matrix blocks
|
|
K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs';
|
|
K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i';
|
|
K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i';
|
|
K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs';
|
|
end
|
|
|
|
% Scale by stiffness coefficient
|
|
K = k * K;
|
|
|
|
% Simplify the expressions
|
|
K = simplify(K);
|
|
|
|
% Display the analytical stiffness matrix
|
|
disp('Analytical Stiffness Matrix:');
|
|
pretty(K);
|
|
#+end_src
|
|
|
|
*** Effect of having frame $\{B\}$ off-centered
|
|
|
|
When the reference frames $\{A\}$ and $\{B\}$ are shifted from the cube's center, off-diagonal elements emerge in the stiffness matrix.
|
|
|
|
Considering a vertical shift as shown in Figure ref:fig:detail_kinematics_cubic_schematic, the stiffness matrix transforms into that shown in Equation eqref:eq:detail_kinematics_cubic_stiffness_off_centered.
|
|
Off-diagonal elements increase proportionally with the height difference between the cube's center and the considered $\{B\}$ frame.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cubic_stiffness_off_centered}
|
|
\bm{K}_{\{B\} \neq \{C\}} = k \begin{bmatrix}
|
|
2 & 0 & 0 & 0 & -2 H & 0 \\
|
|
0 & 2 & 0 & 2 H & 0 & 0 \\
|
|
0 & 0 & 2 & 0 & 0 & 0 \\
|
|
0 & 2 H & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 & 0 \\
|
|
-2 H & 0 & 0 & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 \\
|
|
0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
This stiffness matrix structure is characteristic of Stewart platforms exhibiting symmetry, and is not an exclusive property of cubic architectures.
|
|
Therefore, the stiffness characteristics of the cubic architecture are only distinctive when considering a reference frame located at the cube's center.
|
|
This poses a practical limitation, as in most applications, the relevant frame (where motion is of interest and forces are applied) is located above the top platform.
|
|
|
|
It should be noted that for the stiffness matrix to be diagonal, the cube's center doesn't need to coincide with the geometric center of the Stewart platform.
|
|
This observation leads to the interesting alternative architectures presented in Section ref:ssec:detail_kinematics_cubic_design.
|
|
|
|
*** Uniform Mobility
|
|
|
|
The translational mobility of the Stewart platform with constant orientation was analyzed.
|
|
Considering limited actuator stroke (elongation of each strut), the maximum achievable positions in XYZ space were estimated.
|
|
The resulting mobility in X, Y, and Z directions for the cubic architecture is illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_translations.
|
|
|
|
The translational workspace analysis reveals that for the cubic architecture, the achievable positions form a cube whose axes align with the struts, with the cube's edge length corresponding to the strut axial stroke.
|
|
These findings suggest that the mobility pattern is more subtle than sometimes described in the literature [[cite:&mcinroy00_desig_contr_flexur_joint_hexap]], exhibiting uniformity primarily along directions aligned with the cube's edges rather than uniform spherical distribution in all XYZ directions.
|
|
This configuration still offers more consistent mobility characteristics compared to alternative architectures illustrated in Figure ref:fig:detail_kinematics_mobility_trans.
|
|
|
|
The rotational mobility, illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_rotations, exhibits greater achievable angular stroke in the $R_x$ and $R_y$ directions compared to the $R_z$ direction.
|
|
Furthermore, an inverse relationship exists between the cube's dimension and rotational mobility, with larger cube sizes corresponding to more limited angular displacement capabilities.
|
|
|
|
#+begin_src matlab
|
|
%% Cubic configuration
|
|
H = 100e-3; % Height of the Stewart platform [m]
|
|
Hc = 100e-3; % Size of the useful part of the cube [m]
|
|
FOc = 50e-3; % Center of the cube at the Stewart platform center
|
|
MO_B = -50e-3; % Position {B} with respect to {M} [m]
|
|
MHb = 0;
|
|
|
|
stewart_cubic = initializeStewartPlatform();
|
|
stewart_cubic = initializeFramesPositions(stewart_cubic, 'H', H, 'MO_B', MO_B);
|
|
stewart_cubic = generateCubicConfiguration(stewart_cubic, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb);
|
|
stewart_cubic = computeJointsPose(stewart_cubic);
|
|
stewart_cubic = initializeStrutDynamics(stewart_cubic, 'k', 1);
|
|
stewart_cubic = computeJacobian(stewart_cubic);
|
|
stewart_cubic = initializeCylindricalPlatforms(stewart_cubic, 'Fpr', 150e-3, 'Mpr', 150e-3);
|
|
|
|
% Let's now define the actuator stroke.
|
|
L_max = 50e-6; % [m]
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Mobility of a Stewart platform with Cubic architecture - Translations
|
|
thetas = linspace(0, pi, 100);
|
|
phis = linspace(0, 2*pi, 200);
|
|
rs = zeros(length(thetas), length(phis));
|
|
|
|
for i = 1:length(thetas)
|
|
for j = 1:length(phis)
|
|
Tx = sin(thetas(i))*cos(phis(j));
|
|
Ty = sin(thetas(i))*sin(phis(j));
|
|
Tz = cos(thetas(i));
|
|
|
|
dL = stewart_cubic.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
|
|
|
|
rs(i, j) = L_max/max(abs(dL));
|
|
% rs(i, j) = max(abs([dL(dL<0)*L_min; dL(dL>=0)*L_max]));
|
|
end
|
|
end
|
|
|
|
[phi_grid, theta_grid] = meshgrid(phis, thetas);
|
|
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
|
|
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
|
|
Z = 1e6 * rs .* cos(theta_grid);
|
|
|
|
figure;
|
|
hold on;
|
|
surf(X, Y, Z, 'FaceColor', 'white', 'EdgeColor', colors(1,:));
|
|
quiver3(0, 0, 0, 150, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
|
quiver3(0, 0, 0, 0, 150, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
|
quiver3(0, 0, 0, 0, 0, 150, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
|
text(150, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' );
|
|
text(0, 150, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom');
|
|
text(0, 0, 150, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' );
|
|
hold off;
|
|
axis equal;
|
|
grid off;
|
|
axis off;
|
|
view(105, 15);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_mobility_translations.pdf', 'width', 'normal', 'height', 'full', 'simplify', true);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Mobility of a Stewart platform with Cubic architecture - Rotations
|
|
thetas = linspace(0, pi, 100);
|
|
phis = linspace(0, 2*pi, 200);
|
|
rs_cubic = zeros(length(thetas), length(phis));
|
|
|
|
for i = 1:length(thetas)
|
|
for j = 1:length(phis)
|
|
Rx = sin(thetas(i))*cos(phis(j));
|
|
Ry = sin(thetas(i))*sin(phis(j));
|
|
Rz = cos(thetas(i));
|
|
|
|
dL = stewart_cubic.kinematics.J*[0; 0; 0; Rx; Ry; Rz;];
|
|
rs_cubic(i, j) = L_max/max(abs(dL));
|
|
end
|
|
end
|
|
|
|
[phi_grid, theta_grid] = meshgrid(phis, thetas);
|
|
|
|
X_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* cos(phi_grid);
|
|
Y_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* sin(phi_grid);
|
|
Z_cubic = 1e6 * rs_cubic .* cos(theta_grid);
|
|
|
|
figure;
|
|
hold on;
|
|
surf(X_cubic, Y_cubic, Z_cubic, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:));
|
|
quiver3(0, 0, 0, 1500, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
|
quiver3(0, 0, 0, 0, 1500, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
|
quiver3(0, 0, 0, 0, 0, 1500, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
|
text(1500, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' );
|
|
text(0, 1500, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom');
|
|
text(0, 0, 1500, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' );
|
|
hold off;
|
|
axis equal;
|
|
grid off;
|
|
axis off;
|
|
view(105, 15);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_mobility_rotations.pdf', 'width', 'normal', 'height', 'full', 'simplify', true);
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_mobility
|
|
#+caption: Mobility of a Stewart platform with Cubic architecture. Both for translations (\subref{fig:detail_kinematics_cubic_mobility_translations}) and rotations (\subref{fig:detail_kinematics_cubic_mobility_rotations})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_translations}Mobility in translation}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/detail_kinematics_cubic_mobility_translations.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_rotations}Mobility in rotation}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/detail_kinematics_cubic_mobility_rotations.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
* Dynamical Decoupling
|
|
<<ssec:detail_kinematics_cubic_dynamic>>
|
|
*** Introduction :ignore:
|
|
|
|
This section examines the dynamics of the cubic architecture in the Cartesian frame which corresponds to the transfer function from forces and torques $\bm{\mathcal{F}}$ to translations and rotations $\bm{\mathcal{X}}$ of the top platform.
|
|
When relative motion sensors are integrated in each strut (measuring $\bm{\mathcal{L}}$), the pose $\bm{\mathcal{X}}$ is computed using the Jacobian matrix as shown in Figure ref:fig:detail_kinematics_centralized_control.
|
|
|
|
#+name: fig:detail_kinematics_centralized_control
|
|
#+caption: Typical control architecture in the cartesian frame
|
|
[[file:figs/detail_kinematics_centralized_control.png]]
|
|
|
|
*** Low frequency and High frequency coupling
|
|
|
|
As derived during the conceptual design phase, the dynamics from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ is described by Equation eqref:eq:detail_kinematics_transfer_function_cart.
|
|
At low frequency, the behavior of the platform depends on the stiffness matrix eqref:eq:detail_kinematics_transfer_function_cart_low_freq.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_transfer_function_cart_low_freq}
|
|
\frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to 0]{} \bm{K}^{-1}
|
|
\end{equation}
|
|
|
|
In Section ref:ssec:detail_kinematics_cubic_static, it was demonstrated that for the cubic configuration, the stiffness matrix is diagonal if frame $\{B\}$ is positioned at the cube's center.
|
|
In this case, the "Cartesian" plant is decoupled at low frequency.
|
|
At high frequency, the behavior is governed by the mass matrix (evaluated at frame $\{B\}$) eqref:eq:detail_kinematics_transfer_function_high_freq.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_transfer_function_high_freq}
|
|
\frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to \infty]{} - \omega^2 \bm{M}^{-1}
|
|
\end{equation}
|
|
|
|
To achieve a diagonal mass matrix, the center of mass of the mobile components must coincide with the $\{B\}$ frame, and the principal axes of inertia must align with the axes of the $\{B\}$ frame.
|
|
|
|
#+name: fig:detail_kinematics_cubic_payload
|
|
#+caption: Cubic stewart platform with top cylindrical payload
|
|
#+attr_latex: :width 0.6\linewidth
|
|
[[file:figs/detail_kinematics_cubic_payload.png]]
|
|
|
|
To verify these properties, a cubic Stewart platform with a cylindrical payload was analyzed (Figure ref:fig:detail_kinematics_cubic_payload).
|
|
Transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ were computed for two specific locations of the $\{B\}$ frames.
|
|
When the $\{B\}$ frame was positioned at the center of mass, coupling at low frequency was observed due to the non-diagonal stiffness matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com).
|
|
Conversely, when positioned at the center of stiffness, coupling occurred at high frequency due to the non-diagonal mass matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_cok).
|
|
|
|
#+begin_src matlab
|
|
%% Input/Output definition of the Simscape model
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External metrology [m,rad]
|
|
|
|
% Prepare simulation
|
|
controller = initializeController('type', 'open-loop');
|
|
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3);
|
|
|
|
%% Cubic Stewart platform with payload above the top platform - B frame at the CoM
|
|
H = 200e-3; % height of the Stewart platform [m]
|
|
MO_B = 50e-3; % Position {B} with respect to {M} [m]
|
|
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Mpm', 1e-6, ... % Massless platform
|
|
'Fpm', 1e-6, ... % Massless platform
|
|
'Mph', 20e-3, ... % Thin platform
|
|
'Fph', 20e-3, ... % Thin platform
|
|
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
|
|
'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', 1e-6, ... % Massless strut
|
|
'Msm', 1e-6, ... % Massless strut
|
|
'Fsh', stewart.geometry.l(1)/2, ...
|
|
'Msh', stewart.geometry.l(1)/2 ...
|
|
);
|
|
|
|
% Run the linearization
|
|
G_CoM = linearize(mdl, io)*inv(stewart.kinematics.J).';
|
|
G_CoM.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
G_CoM.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
|
|
%% Same geometry but B Frame at cube's center (CoK)
|
|
MO_B = -100e-3; % Position {B} with respect to {M} [m]
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Mpm', 1e-6, ... % Massless platform
|
|
'Fpm', 1e-6, ... % Massless platform
|
|
'Mph', 20e-3, ... % Thin platform
|
|
'Fph', 20e-3, ... % Thin platform
|
|
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
|
|
'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', 1e-6, ... % Massless strut
|
|
'Msm', 1e-6, ... % Massless strut
|
|
'Fsh', stewart.geometry.l(1)/2, ...
|
|
'Msh', stewart.geometry.l(1)/2 ...
|
|
);
|
|
|
|
% Run the linearization
|
|
G_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
|
|
G_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
G_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of mass of the payload
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
% for i = 1:5
|
|
% for j = i+1:6
|
|
% plot(freqs, abs(squeeze(freqresp(G_CoM(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
% 'HandleVisibility', 'off');
|
|
% end
|
|
% end
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$D_x/F_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$D_y/F_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$D_z/F_z$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
|
|
'DisplayName', '$R_x/M_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
|
|
'DisplayName', '$R_y/M_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
|
|
'DisplayName', '$R_z/M_z$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$R_x/F_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$R_y/F_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_x/M_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_y/M_x$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]);
|
|
ylim([1e-10, 2e-3])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_cart_coupling_com.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of the cube
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
% for i = 1:5
|
|
% for j = i+1:6
|
|
% plot(freqs, abs(squeeze(freqresp(G_CoK(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
% 'HandleVisibility', 'off');
|
|
% end
|
|
% end
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$D_x/F_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$D_y/F_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$D_z/F_z$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
|
|
'DisplayName', '$R_x/M_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
|
|
'DisplayName', '$R_y/M_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
|
|
'DisplayName', '$R_z/M_z$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$R_x/F_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$R_y/F_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_x/M_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_y/M_x$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]);
|
|
ylim([1e-10, 2e-3])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_cart_coupling_cok.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_cart_coupling
|
|
#+caption: Transfer functions for a Cubic Stewart platform expressed in the Cartesian frame. Two locations of the $\{B\}$ frame are considered: at the center of mass of the moving body (\subref{fig:detail_kinematics_cubic_cart_coupling_com}) and at the cube's center (\subref{fig:detail_kinematics_cubic_cart_coupling_cok}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com}$\{B\}$ at the center of mass}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_cubic_cart_coupling_com.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_cok}$\{B\}$ at the cube's center}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_cubic_cart_coupling_cok.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Payload's CoM at the cube's center
|
|
|
|
An effective strategy for improving dynamical performances involves aligning the cube's center (center of stiffness) with the center of mass of the moving components [[cite:&li01_simul_fault_vibrat_isolat_point]].
|
|
This can be achieved by positioning the payload below the top platform, such that the center of mass of the moving body coincides with the cube's center (Figure ref:fig:detail_kinematics_cubic_centered_payload).
|
|
This approach was physically implemented in several studies [[cite:&mcinroy99_dynam;&jafari03_orthog_gough_stewar_platf_microm]], as shown in Figure ref:fig:detail_kinematics_uw_gsp.
|
|
The resulting dynamics are indeed well-decoupled (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com_cok), taking advantage from diagonal stiffness and mass matrices.
|
|
The primary limitation of this approach is that, for many applications including the nano-hexapod, the payload must be positioned above the top platform.
|
|
If a design similar to Figure ref:fig:detail_kinematics_cubic_centered_payload were employed for the nano-hexapod, the X-ray beam would intersect with the struts during spindle rotation.
|
|
|
|
#+begin_src matlab
|
|
%% Cubic Stewart platform with payload above the top platform
|
|
H = 200e-3;
|
|
MO_B = -100e-3; % Position {B} with respect to {M} [m]
|
|
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Mpm', 1e-6, ... % Massless platform
|
|
'Fpm', 1e-6, ... % Massless platform
|
|
'Mph', 20e-3, ... % Thin platform
|
|
'Fph', 20e-3, ... % Thin platform
|
|
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
|
|
'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', 1e-6, ... % Massless strut
|
|
'Msm', 1e-6, ... % Massless strut
|
|
'Fsh', stewart.geometry.l(1)/2, ...
|
|
'Msh', stewart.geometry.l(1)/2 ...
|
|
);
|
|
|
|
% Sample at the Center of the cube
|
|
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3);
|
|
|
|
% Run the linearization
|
|
G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.');
|
|
G_CoM_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$D_x/F_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$D_y/F_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$D_z/F_z$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ...
|
|
'DisplayName', '$R_x/M_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ...
|
|
'DisplayName', '$R_y/M_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ...
|
|
'DisplayName', '$R_z/M_z$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$R_x/F_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$R_y/F_x$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_x/M_y$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$D_y/M_x$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]);
|
|
ylim([1e-10, 2e-3])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_cart_coupling_com_cok.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_com_cok
|
|
#+caption: Cubic Stewart platform with payload at the cube's center (\subref{fig:detail_kinematics_cubic_centered_payload}). Obtained cartesian plant is fully decoupled (\subref{fig:detail_kinematics_cubic_cart_coupling_com_cok})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_centered_payload}Payload at the cube's center}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_cubic_centered_payload.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com_cok}Fully decoupled cartesian plant}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_cubic_cart_coupling_com_cok.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Conclusion
|
|
|
|
The analysis of dynamical properties of the cubic architecture yields several important conclusions.
|
|
Static decoupling, characterized by a diagonal stiffness matrix, is achieved when reference frames $\{A\}$ and $\{B\}$ are positioned at the cube's center.
|
|
Note that this property can also be obtained with non-cubic architectures that exhibit symmetrical strut arrangements.
|
|
Dynamic decoupling requires both static decoupling and coincidence of the mobile platform's center of mass with reference frame $\{B\}$.
|
|
While this configuration offers powerful control advantages, it requires positioning the payload at the cube's center, which is highly restrictive and often impractical.
|
|
|
|
* Decentralized Control
|
|
<<ssec:detail_kinematics_decentralized_control>>
|
|
*** Introduction :ignore:
|
|
|
|
The orthogonal arrangement of struts in the cubic architecture suggests a potential minimization of inter-strut coupling, which could theoretically create favorable conditions for decentralized control.
|
|
Two sensor types integrated in the struts are considered: displacement sensors and force sensors.
|
|
The control architecture is illustrated in Figure ref:fig:detail_kinematics_decentralized_control, where $\bm{K}_{\mathcal{L}}$ represents a diagonal transfer function matrix.
|
|
|
|
#+name: fig:detail_kinematics_decentralized_control
|
|
#+caption: Decentralized control in the frame of the struts.
|
|
[[file:figs/detail_kinematics_decentralized_control.png]]
|
|
|
|
The obtained plant dynamics in the frame of the struts are compared for two Stewart platforms.
|
|
The first employs a cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_payload.
|
|
The second uses a non-cubic Stewart platform shown in Figure ref:fig:detail_kinematics_non_cubic_payload, featuring identical payload and strut dynamics but with struts oriented more vertically to differentiate it from the cubic architecture.
|
|
|
|
#+name: fig:detail_kinematics_non_cubic_payload
|
|
#+caption: Stewart platform with non-cubic architecture
|
|
#+attr_latex: :width 0.6\linewidth
|
|
[[file:figs/detail_kinematics_non_cubic_payload.png]]
|
|
|
|
#+begin_src matlab
|
|
%% Input/Output definition of the Simscape model
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Displacement sensors [m]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensor [N]
|
|
|
|
% Prepare simulation : Payload above the top platform
|
|
controller = initializeController('type', 'open-loop');
|
|
sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3);
|
|
|
|
%% Cubic Stewart platform
|
|
H = 200e-3; % height of the Stewart platform [m]
|
|
MO_B = 50e-3; % Position {B} with respect to {M} [m]
|
|
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Mpm', 1e-6, ... % Massless platform
|
|
'Fpm', 1e-6, ... % Massless platform
|
|
'Mph', 20e-3, ... % Thin platform
|
|
'Fph', 20e-3, ... % Thin platform
|
|
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
|
|
'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', 1e-6, ... % Massless strut
|
|
'Msm', 1e-6, ... % Massless strut
|
|
'Fsh', stewart.geometry.l(1)/2, ...
|
|
'Msh', stewart.geometry.l(1)/2 ...
|
|
);
|
|
|
|
% Run the linearization
|
|
G_cubic = linearize(mdl, io);
|
|
G_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
|
'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
%% Non-Cubic Stewart platform
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
|
stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3);
|
|
stewart = generateGeneralConfiguration(stewart, 'FH', 25e-3, 'FR', 250e-3, 'MH', 25e-3, 'MR', 250e-3, ...
|
|
'FTh', [-22, 22, 120-22, 120+22, 240-22, 240+22]*(pi/180), ...
|
|
'MTh', [-60+22, 60-22, 60+22, 180-22, 180+22, -60-22]*(pi/180));
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof');
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Mpm', 1e-6, ... % Massless platform
|
|
'Fpm', 1e-6, ... % Massless platform
|
|
'Mph', 20e-3, ... % Thin platform
|
|
'Fph', 20e-3, ... % Thin platform
|
|
'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ...
|
|
'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)));
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', 1e-6, ... % Massless strut
|
|
'Msm', 1e-6, ... % Massless strut
|
|
'Fsh', stewart.geometry.l(1)/2, ...
|
|
'Msh', stewart.geometry.l(1)/2 ...
|
|
);
|
|
|
|
% Run the linearization
|
|
G_non_cubic = linearize(mdl, io);
|
|
G_non_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_non_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
|
'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
#+end_src
|
|
|
|
*** Relative Displacement Sensors
|
|
|
|
The transfer functions from actuator force in each strut to the relative motion of the struts are presented in Figure ref:fig:detail_kinematics_decentralized_dL.
|
|
As anticipated from the equations of motion from $\bm{f}$ to $\bm{\mathcal{L}}$ eqref:eq:detail_kinematics_transfer_function_struts, the $6 \times 6$ plant is decoupled at low frequency.
|
|
At high frequency, coupling is observed as the mass matrix projected in the strut frame is not diagonal.
|
|
|
|
No significant advantage is evident for the cubic architecture (Figure ref:fig:detail_kinematics_cubic_decentralized_dL) compared to the non-cubic architecture (Figure ref:fig:detail_kinematics_non_cubic_decentralized_dL).
|
|
The resonance frequencies differ between the two cases because the more vertical strut orientation in the non-cubic architecture alters the stiffness properties of the Stewart platform, consequently shifting the frequencies of various modes.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
|
|
'DisplayName', '$l_i/f_i$');
|
|
plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'DisplayName', '$l_i/f_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]);
|
|
ylim([1e-10, 1e-4])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_non_cubic_decentralized_dL.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
|
|
'DisplayName', '$l_i/f_i$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'DisplayName', '$l_i/f_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]);
|
|
ylim([1e-10, 1e-4])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_decentralized_dL.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_decentralized_dL
|
|
#+caption: Bode plot of the transfer functions from actuator force to relative displacement sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_dL}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_dL})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_dL}Non cubic architecture}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_non_cubic_decentralized_dL.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_dL}Cubic architecture}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_cubic_decentralized_dL.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Force Sensors
|
|
|
|
Similarly, the transfer functions from actuator force to force sensors in each strut were analyzed for both cubic and non-cubic Stewart platforms.
|
|
The results are presented in Figure ref:fig:detail_kinematics_decentralized_fn.
|
|
The system demonstrates good decoupling at high frequency in both cases, with no clear advantage for the cubic architecture.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ...
|
|
'DisplayName', '$f_{m,i}/f_i$');
|
|
plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'DisplayName', '$f_{m,i}/f_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]); ylim([1e-4, 1e2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_non_cubic_decentralized_fn.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture
|
|
freqs = logspace(0, 4, 1000);
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
ax1 = nexttile();
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ...
|
|
'DisplayName', '$f_{m,i}/f_i$');
|
|
plot(freqs, abs(squeeze(freqresp(G_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'DisplayName', '$f_{m,i}/f_j$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]); ylim([1e-4, 1e2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_decentralized_fn.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_decentralized_fn
|
|
#+caption: Bode plot of the transfer functions from actuator force to force sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_fn}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_fn})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_fn}Non cubic architecture}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_non_cubic_decentralized_fn.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_fn}Cubic architecture}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/detail_kinematics_cubic_decentralized_fn.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Conclusion
|
|
|
|
The presented results do not demonstrate the pronounced decoupling advantages often associated with cubic architectures in the literature.
|
|
Both the cubic and non-cubic configurations exhibited similar coupling characteristics, suggesting that the benefits of orthogonal strut arrangement for decentralized control is less obvious than often reported in the literature.
|
|
|
|
* Cubic architecture with Cube's center above the top platform
|
|
<<ssec:detail_kinematics_cubic_design>>
|
|
*** Introduction :ignore:
|
|
|
|
As demonstrated in Section ref:ssec:detail_kinematics_cubic_dynamic, the cubic architecture can exhibit advantageous dynamical properties when the center of mass of the moving body coincides with the cube's center, resulting in diagonal mass and stiffness matrices.
|
|
As shown in Section ref:ssec:detail_kinematics_cubic_static, the stiffness matrix is diagonal when the considered $\{B\}$ frame is located at the cube's center.
|
|
However, the $\{B\}$ frame is typically positioned above the top platform where forces are applied and displacements are measured.
|
|
|
|
This section proposes modifications to the cubic architecture to enable positioning the payload above the top platform while still leveraging the advantageous dynamical properties of the cubic configuration.
|
|
|
|
Three key parameters define the geometry of the cubic Stewart platform: $H$, the height of the Stewart platform (distance from fixed base to mobile platform); $H_c$, the height of the cube, as shown in Figure ref:fig:detail_kinematics_cubic_schematic_full; and $H_{CoM}$, the height of the center of mass relative to the mobile platform (coincident with the cube's center).
|
|
|
|
Depending on the cube's size $H_c$ in relation to $H$ and $H_{CoM}$, different designs emerge.
|
|
In the following examples, $H = 100\,mm$ and $H_{CoM} = 20\,mm$.
|
|
|
|
#+begin_src matlab
|
|
%% Cubic configurations with center of the cube above the top platform
|
|
H = 100e-3; % height of the Stewart platform [m]
|
|
MO_B = 20e-3; % Position {B} with respect to {M} [m]
|
|
FOc = H + MO_B; % Center of the cube with respect to {F}
|
|
#+end_src
|
|
|
|
*** Small cube
|
|
|
|
When the cube size $H_c$ is smaller than twice the height of the CoM $H_{CoM}$ eqref:eq:detail_kinematics_cube_small, the resulting design is shown in Figure ref:fig:detail_kinematics_cubic_above_small.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cube_small}
|
|
H_c < 2 H_{CoM}
|
|
\end{equation}
|
|
|
|
# TODO - Add link to Figure ref:fig:nhexa_stewart_piezo_furutani (page pageref:fig:nhexa_stewart_piezo_furutani)
|
|
This configuration is similar to that described in [[cite:&furutani04_nanom_cuttin_machin_using_stewar]], although they do not explicitly identify it as a cubic configuration.
|
|
Adjacent struts are parallel to each other, differing from the typical architecture where parallel struts are positioned opposite to each other.
|
|
|
|
This approach yields a compact architecture, but the small cube size may result in insufficient rotational stiffness.
|
|
|
|
#+begin_src matlab
|
|
%% Small cube
|
|
Hc = 2*MO_B; % Size of the useful part of the cube [m]
|
|
|
|
stewart_small = initializeStewartPlatform();
|
|
stewart_small = initializeFramesPositions(stewart_small, 'H', H, 'MO_B', MO_B);
|
|
stewart_small = generateCubicConfiguration(stewart_small, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
|
|
stewart_small = computeJointsPose(stewart_small);
|
|
stewart_small = initializeStrutDynamics(stewart_small, 'k', 1);
|
|
stewart_small = computeJacobian(stewart_small);
|
|
stewart_small = initializeCylindricalPlatforms(stewart_small, 'Fpr', 1.1*max(vecnorm(stewart_small.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_small.platform_M.Mb)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% ISO View
|
|
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
|
|
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_small_iso.pdf', 'width', 'normal', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Side view
|
|
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
|
|
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
view([90,0])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_small_side.pdf', 'width', 'half', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Top view
|
|
displayArchitecture(stewart_small, 'labels', false, 'frames', false);
|
|
plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
view([0,90])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_small_top.pdf', 'width', 'half', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_above_small
|
|
#+caption: Cubic architecture with cube's center above the top platform. A cube height of 40mm is used.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_iso}Isometric view}
|
|
#+attr_latex: :options {0.36\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_small_iso.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_side}Side view}
|
|
#+attr_latex: :options {0.30\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_small_side.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_top}Top view}
|
|
#+attr_latex: :options {0.30\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_small_top.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Medium sized cube
|
|
|
|
Increasing the cube's size such that eqref:eq:detail_kinematics_cube_medium is verified produces an architecture with intersecting struts (Figure ref:fig:detail_kinematics_cubic_above_medium).
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cube_medium}
|
|
2 H_{CoM} < H_c < 2 (H_{CoM} + H)
|
|
\end{equation}
|
|
|
|
This configuration resembles the design proposed in [[cite:&yang19_dynam_model_decoup_contr_flexib]] (Figure ref:fig:detail_kinematics_yang19), although their design is not strictly cubic.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Example of a cubic architecture with cube's center above the top platform - Medium cube size
|
|
Hc = H + 2*MO_B; % Size of the useful part of the cube [m]
|
|
|
|
stewart_medium = initializeStewartPlatform();
|
|
stewart_medium = initializeFramesPositions(stewart_medium, 'H', H, 'MO_B', MO_B);
|
|
stewart_medium = generateCubicConfiguration(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
|
|
stewart_medium = computeJointsPose(stewart_medium);
|
|
stewart_medium = initializeStrutDynamics(stewart_medium, 'k', 1);
|
|
stewart_medium = computeJacobian(stewart_medium);
|
|
stewart_medium = initializeCylindricalPlatforms(stewart_medium, 'Fpr', 1.1*max(vecnorm(stewart_medium.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_medium.platform_M.Mb)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% ISO View
|
|
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
|
|
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_medium_iso.pdf', 'width', 'normal', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Side view
|
|
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
|
|
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
view([90,0])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_medium_side.pdf', 'width', 'half', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Top view
|
|
displayArchitecture(stewart_medium, 'labels', false, 'frames', false);
|
|
plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
view([0,90])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_medium_top.pdf', 'width', 'half', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_above_medium
|
|
#+caption: Cubic architecture with cube's center above the top platform. A cube height of 140mm is used.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_iso}Isometric view}
|
|
#+attr_latex: :options {0.36\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_medium_iso.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_side}Side view}
|
|
#+attr_latex: :options {0.30\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_medium_side.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_top}Top view}
|
|
#+attr_latex: :options {0.30\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_medium_top.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Large cube
|
|
|
|
When the cube's height exceeds twice the sum of the platform height and CoM height eqref:eq:detail_kinematics_cube_large, the architecture shown in Figure ref:fig:detail_kinematics_cubic_above_large is obtained.
|
|
|
|
\begin{equation}\label{eq:detail_kinematics_cube_large}
|
|
2 (H_{CoM} + H) < H_c
|
|
\end{equation}
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Example of a cubic architecture with cube's center above the top platform - Large cube size
|
|
Hc = 2*(H + MO_B); % Size of the useful part of the cube [m]
|
|
|
|
stewart_large = initializeStewartPlatform();
|
|
stewart_large = initializeFramesPositions(stewart_large, 'H', H, 'MO_B', MO_B);
|
|
stewart_large = generateCubicConfiguration(stewart_large, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
|
|
stewart_large = computeJointsPose(stewart_large);
|
|
stewart_large = initializeStrutDynamics(stewart_large, 'k', 1);
|
|
stewart_large = computeJacobian(stewart_large);
|
|
stewart_large = initializeCylindricalPlatforms(stewart_large, 'Fpr', 1.1*max(vecnorm(stewart_large.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_large.platform_M.Mb)));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% ISO View
|
|
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
|
|
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_large_iso.pdf', 'width', 'normal', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Side view
|
|
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
|
|
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
view([90,0])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_large_side.pdf', 'width', 'half', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Top view
|
|
displayArchitecture(stewart_large, 'labels', false, 'frames', false);
|
|
plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true);
|
|
scatter3(0, 0, FOc, 200, 'kh');
|
|
view([0,90])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/detail_kinematics_cubic_above_large_top.pdf', 'width', 'half', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+name: fig:detail_kinematics_cubic_above_large
|
|
#+caption: Cubic architecture with cube's center above the top platform. A cube height of 240mm is used.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_iso}Isometric view}
|
|
#+attr_latex: :options {0.36\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_large_iso.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_side}Side view}
|
|
#+attr_latex: :options {0.30\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_large_side.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_top}Top view}
|
|
#+attr_latex: :options {0.30\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/detail_kinematics_cubic_above_large_top.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
*** Platform size
|
|
|
|
#+begin_src matlab
|
|
%% Get the analytical formula for the location of the top and bottom joints
|
|
% Define symbolic variables
|
|
syms k Hc Hcom alpha H
|
|
|
|
assume(k > 0); % k is positive real
|
|
assume(Hcom > 0); % k is positive real
|
|
assume(Hc > 0); % Hc is real
|
|
assume(H > 0); % H is real
|
|
assume(alpha, 'real'); % alpha is real
|
|
|
|
% Define si matrix (edges of the cubes)
|
|
si = 1/sqrt(3)*[
|
|
[ sqrt(2), 0, 1]; ...
|
|
[-sqrt(2)/2, -sqrt(3/2), 1]; ...
|
|
[-sqrt(2)/2, sqrt(3/2), 1]; ...
|
|
[ sqrt(2), 0, 1]; ...
|
|
[-sqrt(2)/2, -sqrt(3/2), 1]; ...
|
|
[-sqrt(2)/2, sqrt(3/2), 1] ...
|
|
];
|
|
|
|
% Define ci matrix (vertices of the cubes)
|
|
ci = Hc * [
|
|
[1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
|
|
[1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ...
|
|
[1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ...
|
|
[1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ...
|
|
[-sqrt(2), 0, 0.5]; ...
|
|
[-sqrt(2), 0, 0.5] ...
|
|
];
|
|
|
|
% Apply vertical shift to ci
|
|
ci = ci + (H + Hcom) * [0, 0, 1];
|
|
|
|
% Calculate bi vectors (Stewart platform top joints)
|
|
bi = ci + alpha * si;
|
|
|
|
|
|
% Extract the z-component value from the first row of ci
|
|
% (all rows have the same z-component)
|
|
ci_z = ci(1, 3);
|
|
|
|
% The z-component of si is 1 for all rows
|
|
si_z = si(1, 3);
|
|
|
|
alpha_for_0 = solve(ci_z + alpha * si_z == 0, alpha);
|
|
alpha_for_H = solve(ci_z + alpha * si_z == H, alpha);
|
|
|
|
% Verify the results
|
|
% Substitute alpha values and check the resulting bi_z values
|
|
bi_z_0 = ci + alpha_for_0 * si;
|
|
disp('Radius for fixed base:');
|
|
simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2))
|
|
|
|
bi_z_H = ci + alpha_for_H * si;
|
|
disp('Radius for mobile platform:');
|
|
simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2))
|
|
#+end_src
|
|
|
|
For the proposed configuration, the top joints $\bm{b}_i$ (resp. the bottom joints $\bm{a}_i$) and are positioned on a circle with radius $R_{b_i}$ (resp. $R_{a_i}$) described by Equation eqref:eq:detail_kinematics_cube_joints.
|
|
|
|
\begin{subequations}\label{eq:detail_kinematics_cube_joints}
|
|
\begin{align}
|
|
R_{b_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 H_{CoM}^2} \label{eq:detail_kinematics_cube_top_joints} \\
|
|
R_{a_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 (H_{CoM} + H)^2} \label{eq:detail_kinematics_cube_bot_joints}
|
|
\end{align}
|
|
\end{subequations}
|
|
|
|
Since the rotational stiffness for the cubic architecture scales with the square of the cube's height eqref:eq:detail_kinematics_cubic_stiffness, the cube's size can be determined based on rotational stiffness requirements.
|
|
Subsequently, using Equation eqref:eq:detail_kinematics_cube_joints, the dimensions of the top and bottom platforms can be calculated.
|
|
|
|
* Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
The analysis of the cubic architecture for Stewart platforms yielded several important findings.
|
|
While the cubic configuration provides uniform stiffness in the XYZ directions, it stiffness property becomes particularly advantageous when forces and torques are applied at the cube's center.
|
|
Under these conditions, the stiffness matrix becomes diagonal, resulting in a decoupled Cartesian plant at low frequencies.
|
|
|
|
Regarding mobility, the translational capabilities of the cubic configuration exhibit uniformity along the directions of the orthogonal struts, rather than complete uniformity in the Cartesian space.
|
|
This understanding refines the characterization of cubic architecture mobility commonly presented in literature.
|
|
|
|
The analysis of decentralized control in the frame of the struts revealed more nuanced results than expected.
|
|
While cubic architectures are frequently associated with reduced coupling between actuators and sensors, this study showed that these benefits may be more subtle or context-dependent than commonly described.
|
|
Under the conditions analyzed, the coupling characteristics of cubic and non-cubic configurations, in the frame of the struts, appeared similar.
|
|
|
|
Fully decoupled dynamics in the Cartesian frame can be achieved when the center of mass of the moving body coincides with the cube's center.
|
|
However, this arrangement presents practical challenges, as the cube's center is traditionally located between the top and bottom platforms, making payload placement problematic for many applications.
|
|
|
|
To address this limitation, modified cubic architectures have been proposed with the cube's center positioned above the top platform.
|
|
Three distinct configurations have been identified, each with different geometric arrangements but sharing the common characteristic that the cube's center is positioned above the top platform.
|
|
This structural modification enables the alignment of the moving body's center of mass with the center of stiffness, resulting in beneficial decoupling properties in the Cartesian frame.
|
|
|
|
|
|
* Helping Functions :noexport:
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle no
|
|
:END:
|
|
|
|
** Initialize Path
|
|
#+NAME: m-init-path-tangle
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./src/'); % Path for functions
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
% Simulink Model name
|
|
mdl = 'nano_hexapod_model';
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
#+END_SRC
|