1. Introduction
The first successfully applied parallel manipulator (PM) structure was originally developed by Gough and later modified by Stewart [Reference Gough1, Reference Stewart2]. Over the past two decades, the research direction and application of PMs has shifted toward control, coordination, and algorithms, rather than emphasizing the development of new structures. This places additional demands on the study of the PM dynamics.
To achieve higher performance requirements, we propose a new 6-RR-RP-RR configuration, where P, R, and RP denote prismatic, revolute joints, and an actuated ball screw assembly, respectively. Compared with the Stewart platform, the dynamics of PMs with RR–joints are complicated by the additional torques. Nevertheless, a study by Gloess and Lula [Reference Gloess and Lula3] on the impact of the RR–joint on the stiffness of a PM found that it provides twice the joint stiffness of a conventional universal joint. Additionally, the axial rotation range was extended to
$\pm 90^{\circ }$
in both joint shifts, thereby expanding the workspace of the PM [Reference Großmann and Kauschinger4]. More importantly, in the application process, we have realized that the offset RR–joint, compared to the conventional universal joint, can divide the joint into several parts, significantly reducing the manufacturing complexity of the joint. Moreover, during the installation process, it provides more installation space, making it more suitable for PM designed for mass production purposes. In conclusion, it is meaningful to consider the dynamics of PMs with RR–joints.
In recent years, there has been a rapid increase in theoretical research on PMs with RR–joints, particularly the computation of their kinematics. Analysis of the workspace of a 3-RR-P-RR PM [Reference Hu and Lu5] showed that it has a larger reachable workspace than similar structures using only one RR–joint or none at all. Algorithms based on joint constraints [Reference Dalvand and Shirinzadeh6, Reference Dalvand, Shirinzadeh and Nahavandi7] have attempted to establish the displacement kinematics of a 6-RR-C-RR PM with RR–joints (where C denotes a cylindrical joint whose translational degree of freedom (DOF) is actuated). Additionally, the displacement kinematics of a 6-P-RR-R-RR PM have been studied using both the joint-constraint algorithm and the Denavit–Hartenberg (D–H) parameters [Reference Han, Han, Xu, Zhu, Yu and Wu8, Reference Han, Zhang, Zhang, Han, Li and Xu9], with numerical solutions obtained. The displacement kinematics of a 6-RR-RP-RR PM (see Fig. 1) was analyzed by Yu et al. [Reference Yu, Xu, Wu, Yu, He and Wang10], while Zhang et al. [Reference Zhang, Han, Zhang, Xu, Xiong, Han and Li11] derived the velocity and acceleration solution of the PM by introducing the differential form of the kinematic model using the Jacobian matrix. To date, all research on PMs with RR–joints has focused on kinematics, rather than dynamics. The dynamic model of the 6-RR-RP-RR PM will be derived in this paper.
The CAD model of the 6-RR-RP-RR PM.
Several approaches have been developed for studying the dynamics of PMs, such as the principle of virtual work [Reference Staicu12–Reference Zhao and Gao14], Kane’s method [Reference Asadi and Sadati15–Reference Wu, Xiong and Kong17], Hamilton’s principle [Reference Zhang, Mills, Cleghorn, Jin and Zhao18], and screw theory [Reference Zhao, Geng, Chen, Li and Yang19–Reference Gallardo, Rico, Frisoli, Checcacci and Bergamasco21]. Other common approaches include Lagrangian methods [Reference Geng, Haynes, Lee and Carroll22–Reference Zou, Zhang and Huang24], Newton–Euler methods [Reference Dasgupta and Mruthyunjaya25, Reference He, Zheng, Gao and Zhang26], and hybrid techniques [Reference Abo-Shanab27–Reference Bingul and Karahan29]. The Lagrangian method describes a system’s motion from the energy perspective [Reference Cheng, Yiu and Li30]. Therefore, there is no need to consider the internal force or changes in the reference frame system [Reference Pang and Shahinpoor31]. In contrast, the Newton–Euler method is more efficient but requires the internal forces of links to be considered [Reference Silver32]. The motion of links in space has six degrees-of-freedom (DOFs). The Newton equation and the Euler equation provide the constraint equations for translational and rotational movements, respectively.
As an efficient and convenient approach, the Newton–Euler method has been widely used in recent years. For instance, Chen and Wang [Reference Chen and Wang33] used this method to establish a dynamic model for a redundantly actuated cable-driven PM, while Arian et al. [Reference Arian, Danaei, Abdi and Nahavandi34] employed the Newton–Euler method to model the dynamics of a 3-DOF Gantry–Tau manipulator. The legs of a PM can be considered as a series mechanism, allowing for the development of an efficient iterative dynamic model through the iteration of both outward motion and inward forces. Importantly, the effect of the mobile platform at the end of the legs must be taken into consideration. In this way, He et al. [Reference He, Zheng, Gao and Zhang35] applied the Newton–Euler method to obtain a dynamic model for a 7-DOF hybrid serial–parallel manipulator.
By applying the Newton–Euler approach, this article presents a dynamic algorithm for a 6-RR-RP-RR PM. First, we develop an iterative algorithm for determining the relationship between joint motion and link motion. Next, we establish a dynamic model using the Newton–Euler equations and a linear equation system. We then analyze the actuated forces, before validating the accuracy of the mathematical model through simulations.
The remainder of this paper is organized as follows. Section 2 describes the manipulator and the definition of the joints’ reference frame. Section 3 describes the inverse kinematic model of the PM joints and then proposes an iterative algorithm for computing the link motion in the base frame. Section 4 establishes the dynamic model of the PM using the Newton–Euler method and solves the equation system with a sparse coefficient matrix by generalized minimal residual method (GMRES). Section 5 solves the dynamic model and compares it with the simulation model. Finally, Section 6 presents the conclusions to this study.
2. Manipulator description
The 6-RR-RP-RR PM studied in this paper consists of a mobile platform (MP), a base platform (BP), and six actuated legs which consist of a RP–joint and two RR–joints, as shown in Fig. 1.
Configuration, geometric parameters, and frame definition: (a) simplified form of the RR–joint and the offset value; (b) frame definition for MP and BP; (c)
$y$
-axis and
$z$
-axis of the frames
$\mathcal{P}$
and
$\mathcal{B}$
, and geometric parameters in the axial direction.
Fig. 2 shows the configuration, geometric parameters, and the frames of the PM. Fig. 2a shows the used RR–joints and their simplified form.
$U_p$
and
$U_b$
are the offset values of the RR–joints on the MP and BP sides, respectively. Fig. 2b defines the frames of the MP and BP, where the reference frame
$O_P-X_PY_PZ_P$
(the
$\mathcal{P}$
frame) is centered on point
$O_P$
in the MP and the base frame
$O_B-X_BY_BZ_B$
(the
$\mathcal{B}$
frame) is centered on
$O_B$
in the BP. The points at which the legs connect with the MP and BP are defined as
$O_{Pj} (j=1\sim 6)$
and
$O_{Bj}(j=1\sim 6)$
, respectively. Additionally, the frames of the connecting points are defined as
$\mathcal{P}_j$
and
$\mathcal{B}_j$
and are transformed from the frames
$\mathcal{P}$
and
$\mathcal{B}$
through a specified transformation [Reference Zhang, Han, Zhang, Xu, Xiong, Han and Li11]. Fig. 2c shows another view of the frames
$\mathcal{P}$
and
$\mathcal{B}$
, as well as the geometric parameters along the
$z$
-axis.
Schematic of a single leg motion and distribution of joint frames. (The black frame in the top right corner represents the directional legend of the joint frames.).
A single leg can be considered as a serial kinematic chain, and its reference frames can be determined using the D–H parameter method. As depicted in Fig. 3, the following frames and parameters can be defined:
$O_{ij}-X_{ij}Y_{ij}Z_{ij}$
(denoted as frame
$\mathcal{F} _{ij}$
, where
$i=1\sim 6$
and
$j=1\sim 6$
) represents the reference frame of the
$i$
th joint of the
$j$
th leg. Note that
$\mathcal{F} _{6j}$
is equivalent to
$\mathcal{P}_j$
. The generalized motion parameters of the joints are denoted by:
\begin{equation*} \mathbf {q}_{ij}=\left [ \begin {matrix} \theta _{1j}& \theta _{2j}& \theta _{3j}& d_{4j}& \theta _{5j}& \theta _{6j}\\[5pt] \end {matrix} \right ] ^{\textrm {T}}, \end{equation*}
$\theta _{ij}$
represent the rotational angle, and
$d_{ij}$
represents the distance of movement, which lie along the
$z$
-axis of their respective joint reference frames, with the subscript
$ij$
referring to the
$i$
th joint of the
$j$
th leg:
\begin{equation} \mathbf{T}_i=\mathbf{Rot}\left ( X,\alpha _i \right ) \cdot \mathbf{Tran}\left ( X,a_i \right ) \cdot \mathbf{Rot}\left ( Z,\theta _i \right ) \cdot \mathbf{Tran}\left ( Z,d_i \right ) \end{equation}
where
$\mathbf{T}_i$
is a homogeneous transformation matrix.
$\mathbf{Rot}\left ( X,\alpha _i \right )$
and
$\mathbf{Rot}\left ( Z,\theta _i \right )$
represent rotation about the
$x$
-axis and
$z$
-axis, respectively.
$\mathbf{Tran}\left ( X,a_i \right )$
and
$\mathbf{Tran}\left ( Z,d_i \right )$
represent translation along the
$x$
-axis and
$z$
-axis by a specified distance.
3. Kinematics
The inverse kinematic problem aims to uncover the connection between the position of the MP and the motion of each joint. Further, to establish the differential motion, including velocity and acceleration, the relationship between the MP and each leg is precisely defined through the Jacobian equations.
3.1. Inverse kinematic model
-
1. Displacement model: Due to the introduction of offset universal joints, a single leg is now regarded as a serial kinematic chain, resulting in the existence of multiple solutions for its inverse kinematic problem. Consequently, obtaining an explicit solution for the leg length is not straightforward through the geometric methods typically employed to solve the inverse kinematic problem of the Gough–Stewart Platform. Therefore, this paper solves the inverse displacement-kinematic problem of the PM using a numerical method [Reference Han, Han, Xu, Zhu, Yu and Wu8]. Specifically, by obtaining a set of nonlinear equations with six unknowns through two equivalent homogeneous transformation matrices and solving the equations using the Newton–Raphson method, we obtain
(2)with
\begin{equation} \left [ \mathbf{q}_{ij} \right ] _{\left ( k+1 \right )}=\left [ \mathbf{q}_{ij} \right ] _{\left ( k \right )}-\left [ \frac{\partial \boldsymbol{\Phi }}{\partial \mathbf{q}_{ij}} \right ] _{\left ( k \right )}^{-1}\cdot \left [ \mathbf{q}_{ij} \right ] _{\left ( k \right )} \end{equation}
$k$
represents the number of iterations. The nonlinear system consisting of six equations selected from the equivalent transformation is represented by
$\boldsymbol{\Phi }$
. And
$[\mathbf{q}_{ij}]_{(0)}$
is the joint parameters of the current leg at the initial position. Detailed information on the above equation can be found in Appendix.
-
2. Velocity and acceleration models: The mapping matrix can be used to describe the velocity mapping relation between the MP and the joints [Reference Zhang, Han, Zhang, Xu, Xiong, Han and Li11]:
(3)and
\begin{equation} \dot{\mathbf{q}}_{ij}=\mathbf{J}_{j}^{-1}\cdot \dot{\mathbf{q}}_{MP},\quad \mathbf{J}_{j}\in \mathbb{R}^{6\times 6} \end{equation}
$\mathbf{J}_j$
can be decomposed into two parts:(4)where
\begin{equation} \mathbf{J}_j=\,^{\dot{\mathbf{q}}_t}\mathbf{J}_{\dot{\mathbf{q}}_{MP}}^{-1}\cdot \,^{\dot{\mathbf{q}}_h}\mathbf{J}_{\dot{\mathbf{q}}_t}^{-1}\cdot \,^{\dot{\mathbf{q}}_h}\mathbf{J}_{\dot{\mathbf{q}}_{ij}}=\,^{\dot{\mathbf{q}}_h}\mathbf{J}_{\dot{\mathbf{q}}_{MP}}^{-1}\cdot \,^{\dot{\mathbf{q}}_h}\mathbf{J}_{\dot{\mathbf{q}}_{ij}} \end{equation}
relates to the motion of the MP.
\begin{equation*} ^{\dot {\mathbf {q}}_h}\mathbf {J}_{\dot {\mathbf {q}}_{MP}}=\,^{\dot {\mathbf {q}}_h}\mathbf {J}_{\dot {\mathbf {q}}_t}\cdot ^{\dot {\mathbf {q}}_{t}}\mathbf {J}_{\dot {\mathbf {q}}_{MP}} \end{equation*}
$^{\dot{\mathbf{q}}_h}\mathbf{J}_{\dot{\mathbf{q}}_t}$
is obtained by treating MP as a rigid body and analyzing the velocity transformation on the rigid body.
$^{\dot{\mathbf{q}}_{t}}\mathbf{J}_{\dot{\mathbf{q}}_{MP}}$
is obtained by transforming Euler angular velocity to Cartesian angular velocity. And
$^{\dot{\mathbf{q}}_h}\mathbf{J}_{\dot{\mathbf{q}}_{ij}}$
is the geometric Jacobian matrix which relates to the motion of the joints. The term
$\dot{\mathbf{q}}_{ij}=\left [ \begin{matrix} \dot{\theta }_{1j}& \dot{\theta }_{2j}& \dot{\theta }_{3j}& \dot{d}_{4j}& \dot{\theta }_{5j}& \dot{\theta }_{6j}\\[5pt] \end{matrix} \right ] ^{\textrm{T}}$
is the differential motion of joints,
$\dot{\mathbf{q}}_{MP}=\left [ \begin{matrix} \dot{p}_x& \dot{p}_y& \dot{p}_z& \dot{\varphi }& \dot{\theta }& \dot{\psi }\\[5pt] \end{matrix} \right ] ^{\textrm{T}}$
represents the first derivative of MP motion with respect to Euler angles by the sequence of RPY (Roll-Pitch-Yaw),
$\dot{\mathbf{q}}_{t}=\left [ \begin{matrix} \dot{p}_x& \dot{p}_y& \dot{p}_z& \omega _x& \omega _y& \omega _z\\[5pt] \end{matrix} \right ] ^{\textrm{T}}$
refers to the differential motion of the MP with respect to
$\mathcal{B}$
, and
$\dot{\mathbf{q}}_h$
represents the differential motion of the point
$O_{Pj}$
in
$\mathcal{B}$
. Taking the time derivative of both sides of Eq. (3), the generalized acceleration of the leg joints is(5)with
\begin{equation} \ddot{\mathbf{q}}_{ij}=\dot{\mathbf{J}}_{j}^{-1}\cdot \dot{\mathbf{q}}_{M\textrm{P}}+\mathbf{J}_{j}^{-1}\cdot \ddot{\mathbf{q}}_{M\textrm{P}} \end{equation}
$\ddot{\mathbf{q}}_{MP}$
is the generalized acceleration of the MP concerning the Euler angle and
$\ddot{\mathbf{q}}_{ij}$
is the joint acceleration.
3.2. Motion parameters of the MP
The MP can be considered as a parallel link, and the input velocity of
$O_P$
can be used to determine the motion parameters of the platform. Eq. (4) establishes the mapping relation between the generalized velocity of the MP
$\dot{\mathbf{p}}_t$
in the frame
$\mathcal{B}$
and its velocity
$\dot{\mathbf{p}}_{MP}$
concerning the Euler angle. Thus, we obtain the generalized velocity of the MP as:
\begin{equation} \dot{\mathbf{q}}_t=\,^{\dot{\mathbf{q}}_t}\mathbf{J}_{\dot{\mathbf{q}}_{MP}}\cdot \dot{\mathbf{q}}_{MP}=\left [ \begin{matrix} \dot{p}_{x}& \dot{p}_{y}& \dot{p}_{z}& \omega _{x}& \omega _{y}& \omega _{z}\\[5pt] \end{matrix} \right ] ^{\textrm{T}} \end{equation}
Taking the time derivative of both sides of Eq. (6), the generalized acceleration of the MP is derived as:
\begin{equation} \begin{split} \ddot{\mathbf{q}}_t=\,^{\dot{\mathbf{q}}_t}\dot{\mathbf{J}}_{\dot{\mathbf{q}}_{MP}}\cdot \dot{\mathbf{q}}_{MP}+\,^{\dot{\mathbf{q}}_t}\mathbf{J}_{\dot{\mathbf{q}}_{MP}}\cdot \ddot{\mathbf{q}}_{MP}=\left [ \begin{matrix} \ddot{p}_{x}& \ddot{p}_{y}& \ddot{p}_{z}& \dot{\omega }_{x}& \dot{\omega }_{y}& \dot{\omega }_{z}\\[5pt] \end{matrix} \right ] ^{\textrm{T}} \end{split} \end{equation}
with the angular velocity of the MP is
$\boldsymbol{\omega }_{MP}=\left [ \begin{matrix} \omega _{x}& \omega _{y}& \omega _{z}\\[5pt] \end{matrix} \right ] ^{\textrm{T}}$
, the angular acceleration is
$\dot{\boldsymbol{\omega }}_{MP}=\left [ \begin{matrix} \dot{\omega }_{x}& \dot{\omega }_{y}& \dot{\omega }_{z}\\[5pt] \end{matrix} \right ] ^{\textrm{T}}$
, and the acceleration is
$\ddot{\mathbf{p}}_{MP}=\left [ \begin{matrix} \ddot{p}_{x}& \ddot{p}_{y}& \ddot{p}_{z}\\[5pt] \end{matrix} \right ] ^{\textrm{T}}$
.
In summary, the acceleration of the mass center of the MP under the frame
$\mathcal{B}$
is given by:
\begin{equation} \begin{split} \ddot{\mathbf{p}}_{CMP}=\ddot{\mathbf{p}}_{MP}+\dot{\boldsymbol{\omega }}_{MP}\times \mathbf{r}_{OP,CMP}+\boldsymbol{\omega }_{MP}\times \left ( \boldsymbol{\omega }_{MP}\times \mathbf{r}_{OP,CMP} \right ) \end{split} \end{equation}
with
$\mathbf{r}_{OP,CMP}$
is the vector from
$O_P$
to the mass center of the MP under the frame
$\mathcal{B}$
.
3.3. Iterative algorithm for velocity and acceleration of links
The motion parameters of the joints can be obtained from the inverse kinematic model. However, these parameters are all related to the
$z$
-axis of the joint frame
$\mathcal{F}_{ij}$
. To use the velocity and acceleration parameters as inputs for the dynamic model, they need to be transformed into the motion of the joint and link coordinate systems with respect to the base frame. To address this issue, an iterative algorithm is proposed for converting the joint motion.
The connection of adjacent joints is defined as a link, and the dynamic model requires the motion of these links as its input. Thus, an iterative algorithm (different from the previous Newton–Raphson iterative algorithm for the displacement kinematics) is adopted to obtain the motion parameters of each link under the frame
$\mathcal{B}$
.
Any link
$L_{ij}$
can be represented as follows:
Characterization of link
$L_{ij}$
for the Newton–Euler formulation.
From Fig. 4, the parameters characterizing the motion of the link can be defined (see Table I).
The relationship between the motion of link
$L_{ij}$
and its joints is established through the following velocity transformation relation of the rigid body:
\begin{align} \boldsymbol{\omega }_{ij}=\left\{\begin{array}{l@{\quad}l} \boldsymbol{\omega }_{( i-1) j} & (\textrm{P-joint})\\[5pt] \boldsymbol{\omega }_{(i-1)j}+\dot{\theta }_{ij}\mathbf{Z}_{ij}& (\textrm{R-joint}) \end{array} \right.\end{align}
and
\begin{equation} \dot{\mathbf{p}}_{ij}=\left\{\begin{array}{l@{\quad}l} \dot{\mathbf{p}}_{\left ( i-1 \right ) j}+\dot{d}_{ij}\mathbf{Z}_{ij}+\boldsymbol{\omega }_{\left ( i-1 \right ) j}\times \mathbf{r}_{\left ( i-1,i \right ) j}& \left ( \textrm{P-joint} \right )\\[5pt] \dot{\mathbf{p}}_{\left ( i-1 \right ) j}+\boldsymbol{\omega }_{\left ( i-1 \right ) j}\times \mathbf{r}_{\left ( i-1,i \right ) j}& \left ( \textrm{R-joint} \right ) \end{array}\right. \end{equation}
By differentiating Eqs. (9) and (10), we obtain
\begin{equation} \dot{\boldsymbol{\omega }}_{ij}=\left\{\begin{array}{l@{\quad}l} \dot{\boldsymbol{\omega }}_{\left ( i-1 \right ) j}& \left ( \textrm{P-joint} \right )\\[5pt] \dot{\boldsymbol{\omega }}_{\left ( i-1 \right ) j}+\ddot{\theta }_{ij}\mathbf{Z}_{ij}+\dot{\theta }_{ij}\left ( \boldsymbol{\omega }_{ij}\times \mathbf{Z}_{ij} \right )& \left ( \textrm{R-joint} \right ) \end{array} \right.\end{equation}
and
\begin{equation} \ddot{\mathbf{p}}_{ij}=\left\{\begin{array}{l@{\quad}l} \ddot{\mathbf{p}}_{\left ( i-1 \right ) j}+\ddot{d}_{ij}\mathbf{Z}_{ij}+2\dot{d}_{ij}\left ( \boldsymbol{\omega }_{(i-1)j}\times \mathbf{Z}_{ij} \right ) & \\[5pt] \qquad +\dot{\boldsymbol{\omega }}_{\left ( i-1 \right )j}\times \mathbf{r}_{\left ( i-1,i \right ) j}+\boldsymbol{\omega }_{\left ( i-1 \right )j}\times \left ( \boldsymbol{\omega }_{\left ( i-1 \right )j}\times \mathbf{r}_{\left ( i-1,i \right ) j} \right )& \left ( \textrm{P-joint} \right )\\[5pt] \ddot{\mathbf{p}}_{\left ( i-1 \right ) j}+\dot{\boldsymbol{\omega }}_{\left ( i-1 \right ) j}\times \mathbf{r}_{\left ( i-1,i \right ) j}+\boldsymbol{\omega }_{\left ( i-1 \right ) j}\times \left ( \boldsymbol{\omega }_{\left ( i-1 \right ) j}\times \mathbf{r}_{\left ( i-1,i \right ) j} \right )& \left ( \textrm{R-joint} \right ) \end{array} \right. \end{equation}
According to Eqs. (9), (11), and (12), and the geometry of the links, the acceleration of the mass center of link
$L_{ij}$
under the frame
$\mathcal{B}$
can be determined as:
\begin{equation} \ddot{\mathbf{p}}_{Cij}=\ddot{\mathbf{p}}_{ij}+\dot{\boldsymbol{\omega }}_{ij}\times \mathbf{r}_{\left ( i,Ci \right )j}+\boldsymbol{\omega }_{ij}\times \left ( \boldsymbol{\omega }_{ij}\times \mathbf{r}_{\left ( i,Ci \right )j} \right ) \end{equation}
As shown in Fig. 5, applying Eqs. (9), (11), and (13), the motion parameters for the links of the legs can be expressed in the frame
$\mathcal{B}$
. Here,
$\boldsymbol{\omega }_{\mathcal{B} j}$
,
$\dot{\boldsymbol{\omega }}_{\mathcal{B} j}$
, and
$\ddot{\mathbf{p}}_{\mathcal{B} j}$
are the initial iterative parameters, determined by the motion of the BP. When the BP is stationary, these parameters are typically set to zeros.
Variables of link
$L_{ij}$
.
Computational structure of the iterative algorithm.
4. Dynamics
With the use of RR–joints, both ends of the leg are affected by torques, the direction, and magnitude of which are unknown. Thus, the dynamic equation of the leg cannot be obtained explicitly. However, all forces and torques in the Newton–Euler method are linear and can be solved by establishing a system of linear equations.
Before establishing the Newton–Euler equation for the
$j$
th leg, it is crucial to determine the forces and torques acting on this leg. These are shown in Fig. 6.
Forces and torques on the links of the
$j$
th leg.
Fig. 6 shows that each link is subjected to two force vectors in unknown directions (each force vector can be decomposed into three unknown forces along the frame axes of the frame
$\mathcal{B}$
), And four torques, two under frame
$O_{ij}$
and others under frame
$O_{(i+1)j}$
(in specific directions along their x and y axes). Links
$L_{2j}$
and
$L_{3j}$
are combined into a single link, which is defined as
$L_{sj}$
. Joints 1, 2, 5, and 6 are all R–joints. These R–joints have only one DOF, but five constraints. Hence, in the absence of consideration for reactive forces, an R-joint is subject to forces in three directions and torques in two directions. Joint 4 is an RP–joint, and its rotational and feed motions are coupled. The output torque of the motor can be converted from the supporting force in the direction of the feed. Thus, the supporting force in the direction of the feed
$f_{4j\textrm{z}}$
(along the
$z$
-axis of
$\mathcal{F}_{4j}$
) can be regarded as the actuated force. Therefore, joint 4 is subject to binding forces in two directions (
$x$
-axis and
$y$
-axis of
$\mathcal{F}_{4j}$
), the actuated force along the feed direction, and constrained torques in two directions. In summary, each link is subjected to three unknown forces and two unknown torques, with a total of five unknown variables.
For each link, three Newton equations of constrained motion and three Euler equations of constrained rotation in space can be independently established, giving a total of six equations. Generally, each leg can be divided into four individual links, resulting in a total of 25 unknown variables related through 24 equivalent relationships.
Each leg has 25 unknown variables and 24 equations, giving 150 unknown variables and 144 equations for all six legs. For the MP, six expressions are obtained by the Newton–Euler equation. Therefore, we can establish a linear system with 150 equations for the whole PM.
4.1. Newton–Euler equations for a single leg
The
$j$
th leg
$\left ( j=1\sim 6 \right )$
can be divided into four links, denoted by
$L_{ij}\left ( i=1,s,4,5 \right )$
. The five joint origins are denoted by
$O_{ij}\left ( i=1,2,4,5,6 \right )$
. Each joint has five constraints, three unknown forces, and two unknown torques.
First, for link
$L_{1j}$
, the Newton–Euler equations are
\begin{equation} \begin{cases} \mathbf{f}_{1j}-\mathbf{f}_{2j}+m_{1j}\mathbf{g}=m_{1j}\ddot{\mathbf{p}}_{C1j}\\[5pt] \boldsymbol{\tau _{1j}-\boldsymbol{\tau }_{2j}}+\tilde{\mathbf{r}}_{B,1j}\cdot \mathbf{f}_{1j}-\tilde{\mathbf{r}}_{B,2j}\cdot \mathbf{f}_{2j}+m_{1j}\tilde{\mathbf{r}}_{B,C1j}\mathbf{g}\,-m_{1j}\tilde{\mathbf{r}}_{B,C1j}\ddot{\mathbf{p}}_{C1j}=\dfrac{d}{dt}\left ( \mathbf{I}_{1j}\boldsymbol{\omega }_{1j} \right )\\[5pt] \end{cases} \end{equation}
$\tilde{\mathbf{r}}_{B,1j}$
is the skew-symmetric matrix of
$\mathbf{r}_{B,1j}$
:
\begin{equation*} \mathbf {r}_{B,1j}=\left [ \begin {matrix} r_{1jx}& r_{1jy}& r_{1jz}\\[5pt] \end {matrix} \right ], \end{equation*}
and
\begin{equation*} \tilde {\mathbf {r}}_{B,1j}=\left [ \begin {matrix} 0& -r_{1jz}& r_{1jy}\\[5pt] r_{1jz}& 0& -r_{1jx}\\[5pt] -r_{1jy}& r_{1jx}& 0\\[5pt] \end {matrix} \right ]. \end{equation*}
All vector expressions with a tilde in the following text are similar.
$\mathbf{I}_{1j}$
is the inertia tensor of
$L_{1j}$
under the frame
$\mathcal{B}$
. According to the coordinate transformation rules for the inertia tensor,
\begin{equation*} \mathbf {I}_{1j}=\mathbf {R}_{O_{C1j}}\cdot ^{O_{C1j}}\mathbf {I}_{1j}\cdot \mathbf {R}_{O_{C1j}}^{\textrm {T}} \end{equation*}
where
$^{O_{1j}}\mathbf{I}_{1j}$
is the inertia tensor of
$L_{1j}$
concerning the reference frame of the center of mass of link
$1j$
, which is a constant matrix.
$\mathbf{R}_{O_{C1j}}$
is the transformation matrix from the mass center reference frame to the frame
$\mathcal{B}$
.
Similarly, the Newton–Euler equations of
$L_{4j}$
and
$L_{5j}$
are
\begin{equation} \begin{cases} \mathbf{f}_{4j}-\mathbf{f}_{5j}+m_{4j}\mathbf{g}=m_{4j}\ddot{\mathbf{p}}_{C4j}\\[5pt] \boldsymbol{\tau }_{4j}-\boldsymbol{\tau }_{5j}+\tilde{\mathbf{r}}_{B,4j}\cdot \mathbf{f}_{4j}-\tilde{\mathbf{r}}_{B,5j}\cdot \mathbf{f}_{5j}+m_{4j}\tilde{\mathbf{r}}_{B,C4j}\mathbf{g}-m_{4j}\tilde{\mathbf{r}}_{B,C4j}\ddot{\mathbf{p}}_{C4j}=\dfrac{d}{dt}\left ( \mathbf{I}_{4j}\boldsymbol{\omega }_{4j} \right )\\[5pt] \end{cases} \end{equation}
and
\begin{equation} \begin{cases} \mathbf{f}_{5j}-\mathbf{f}_{6j}+m_{5j}\mathbf{g}=m_{5j}\ddot{\mathbf{p}}_{C5j}\\[5pt] \boldsymbol{\tau }_{5j}-\boldsymbol{\tau }_{6j}+\tilde{\mathbf{r}}_{B,5j}\cdot \mathbf{f}_{5j}-\tilde{\mathbf{r}}_{B,6j}\cdot \mathbf{f}_{6j}+m_{5j}\tilde{\mathbf{r}}_{B,C5j}\mathbf{g}-m_{5j}\tilde{\mathbf{r}}_{B,C5j}\ddot{\mathbf{p}}_{C5j}=\dfrac{d}{dt}\left ( \mathbf{I}_{5j}\boldsymbol{\omega }_{5j} \right )\\[5pt] \end{cases} \end{equation}
For
$L_{sj}$
, we have
\begin{equation} \begin{cases} \mathbf{f}_{2j}-\mathbf{f}_{4j}+\sum _{i=2}^3{m_{ij}\mathbf{g}}=\sum _{i=2}^3{m_{ij}\ddot{\mathbf{p}}_{Cij}}\\[5pt] \boldsymbol{\tau }_{2j}-\boldsymbol{\tau }_{4j}+\tilde{\mathbf{r}}_{B,2j}\cdot \mathbf{f}_{2j}-\tilde{\mathbf{r}}_{B,4j}\cdot \mathbf{f}_{4j}\\[5pt] \qquad \qquad \qquad \qquad +\sum _{i=2}^3{(m_{ij}\tilde{\mathbf{r}}_{B,Cij}\mathbf{g}}-m_{ij}\tilde{\mathbf{r}}_{B,Cij}\ddot{\mathbf{p}}_{Cij})=\sum _{i=2}^3{\frac{d}{dt}\left ( \mathbf{I}_{ij}\boldsymbol{\omega }_{ij} \right )}\\[5pt] \end{cases} \end{equation}
Rearranging Eqs. (14), (15), (16), and (17) gives
\begin{equation} \begin{cases} \quad \mathbf{f}_{1j}-\mathbf{f}_{2j} =m_{1j}\left ( \ddot{\mathbf{p}}_{C1j}-\mathbf{g} \right )\\[5pt] \quad \mathbf{f}_{2j}-\mathbf{f}_{4j} =\sum _{i=2}^3{m_{ij}\left ( \ddot{\mathbf{p}}_{Cij}-\mathbf{g} \right )}\\[5pt] \quad \mathbf{f}_{4j}-\mathbf{f}_{5j} =m_{4j}\left ( \ddot{\mathbf{p}}_{C4j}-\mathbf{g} \right )\\[5pt] \quad \mathbf{f}_{5j}-\mathbf{f}_{6j} =m_{5j}\left ( \ddot{\mathbf{p}}_{C5j}-\mathbf{g} \right )\\[5pt] \quad \tilde{\mathbf{r}}_{B,1j}\cdot \mathbf{f}_{1j}-\tilde{\mathbf{r}}_{B,2j}\cdot \mathbf{f}_{2j}+\boldsymbol{\tau _{1j}-\boldsymbol{\tau }_{2j}}=\frac{d}{dt}\left ( \mathbf{I}_{1j}\boldsymbol{\omega }_{1j} \right )+m_{1j}\tilde{\mathbf{r}}_{B,C1j} \left ( \ddot{\mathbf{p}}_{C1j}-\mathbf{g} \right )\\[5pt] \quad \tilde{\mathbf{r}}_{B,2j}\cdot \mathbf{f}_{2j}-\tilde{\mathbf{r}}_{B,4j}\cdot \mathbf{f}_{4j}+\boldsymbol{\tau _{2j}-\boldsymbol{\tau }_{4j}} =\sum _{i=2}^3{\left [ \frac{d}{dt}\left ( \mathbf{I}_{ij}\boldsymbol{\omega }_{ij} \right )+m_{ij}\tilde{\mathbf{r}}_{B,Cij}\left ( \ddot{\mathbf{p}}_{Cij}-\mathbf{g} \right ) \right ]}\\[5pt] \quad \tilde{\mathbf{r}}_{B,4j}\cdot \mathbf{f}_{4j}-\tilde{\mathbf{r}}_{B,5j}\cdot \mathbf{f}_{5j}+\boldsymbol{\tau _{4j}-\boldsymbol{\tau }_{5j}}=\frac{d}{dt}\left ( \mathbf{I}_{4j}\boldsymbol{\omega }_{4j} \right )+m_{4j}\tilde{\mathbf{r}}_{B,C4j}\left ( \ddot{\mathbf{p}}_{C4j}-\mathbf{g} \right )\\[5pt] \quad \tilde{\mathbf{r}}_{B,5j}\cdot \mathbf{f}_{5j}-\tilde{\mathbf{r}}_{B,6j}\cdot \mathbf{f}_{6j}+\boldsymbol{\tau _{5j}-\boldsymbol{\tau }_{6j}}=\frac{d}{dt}\left ( \mathbf{I}_{5j}\boldsymbol{\omega }_{5j} \right )+m_{5j}\tilde{\mathbf{r}}_{B,C5j} \left ( \ddot{\mathbf{p}}_{C5j}-\mathbf{g} \right )\\[5pt] \end{cases} \end{equation}
In Eq. (18), we arrange all unknown variables on the left and all known parts on the right.
4.2. Newton–Euler equations for the MP
The MP is a link connecting the ends of the six legs. Any external loads acting on the MP are simplified as a force
$\mathbf{F}_{e}$
and a torque
$\mathbf{M}_{e}$
under the frame
$\mathcal{B}$
. Moreover, the MP is constrained by six R–joints at the leg–MP connections
$O_{Pj}$
, each containing three forces and two torques. Thus, the dynamic equations are written as:
\begin{equation} \begin{cases} \sum _{j=1}^6{\mathbf{f}_{6j}-\mathbf{F}_{e}+m_{MP}\mathbf{g}}=m_{MP}\ddot{\mathbf{p}}_{CMP}\\[5pt] \sum _{j=1}^6{\left ( \tilde{\mathbf{r}}_{B,6j}\cdot \mathbf{f}_{6j}+\boldsymbol{\tau }_{6j} \right ) -\tilde{\mathbf{r}}_{B,P}\cdot \mathbf{F}_e -\mathbf{M}_e} \\[5pt] \qquad \qquad \qquad \qquad + m_{MP}\tilde{\mathbf{r}}_{B,CMP}\mathbf{g}-m_{MP}\tilde{\mathbf{r}}_{B,CMP}\ddot{\mathbf{p}}_{CMP}=\frac{d}{dt}\left ( \mathbf{I}_{MP}\boldsymbol{\omega }_{MP} \right ) \end{cases} \end{equation}
where
\begin{equation*} \mathbf {I}_{MP}=\mathbf {R}_{COP}\cdot ^{COP}\mathbf {I}_{MP}\cdot \mathbf {R}_{COP}^{\textrm {T}} \end{equation*}
is the inertia tensor of the MP under the frame
$\mathcal{B}$
; the terms
$\mathbf{R}_{COP}$
and
$^{COP}\mathbf{I}_{MP}$
are similar to those in previous equations.
Rearranging Eq. (19), we obtain the following:
\begin{equation} \begin{cases} \sum _{j=1}^6{\mathbf{f}_{6j}}=m_{MP}\left ( \ddot{\mathbf{p}}_{CMP}-\mathbf{g} \right ) +\mathbf{F}_e\\[5pt] \sum _{j=1}^6{\left ( \tilde{\mathbf{r}}_{B,6j}\cdot \mathbf{f}_{6j}+\boldsymbol{\tau }_{6j} \right )}=\frac{d}{dt}\left ( \mathbf{I}_{MP}\boldsymbol{\omega }_{MP} \right )+\tilde{\mathbf{r}}_{B,P}\cdot \mathbf{F}_e+\mathbf{M}_e+m_{MP}\tilde{\mathbf{r}}_{B,CMP}\left ( \ddot{\mathbf{p}}_{CMP}-\mathbf{g} \right )\\[5pt] \end{cases} \end{equation}
4.3. Solution for the actuated force
Combining Eqs. (18) and (20) yields the Newton–Euler equations of the whole PM:
\begin{equation} \left [ \begin{array}{l} \mathbf{F}_1-\mathbf{F}_2\\[5pt] \mathbf{F}_2-\mathbf{F}_4\\[5pt] \mathbf{F}_4-\mathbf{F}_5\\[5pt] \mathbf{F}_5-\mathbf{F}_6\\[5pt] \sum _{j=1}^6{\mathbf{f}_{6j}}\\[5pt] \tilde{\mathbf{r}}_{B,1}\cdot \mathbf{F}_1-\tilde{\mathbf{r}}_{B,2}\cdot \mathbf{F}_2+\boldsymbol{\mu }_1-\boldsymbol{\mu }_2\\[5pt] \tilde{\mathbf{r}}_{B,2}\cdot \mathbf{F}_2-\tilde{\mathbf{r}}_{B,4}\cdot \mathbf{F}_4+\boldsymbol{\mu }_2-\boldsymbol{\mu }_4\\[5pt] \tilde{\mathbf{r}}_{B,4}\cdot \mathbf{F}_4-\tilde{\mathbf{r}}_{B,5}\cdot \mathbf{F}_5+\boldsymbol{\mu }_4-\boldsymbol{\mu }_5\\[5pt] \tilde{\mathbf{r}}_{B,5}\cdot \mathbf{F}_5-\tilde{\mathbf{r}}_{B,6}\cdot \mathbf{F}_6+\boldsymbol{\mu }_5-\boldsymbol{\mu }_6\\[5pt] \sum _{j=1}^6{\left ( \tilde{\mathbf{r}}_{B,6j}\cdot \mathbf{f}_{6j}+\boldsymbol{\tau }_{6j} \right )}\\[5pt] \end{array} \right ] = \left [ \begin{array}{l} \mathbf{G}_1\\[5pt] \mathbf{G}_2+\mathbf{G}_3\\[5pt] \mathbf{G}_4\\[5pt] \mathbf{G}_5\\[5pt] \mathbf{G}_{MP}+\mathbf{F}_e\\[5pt] \mathbf{N}_1+\boldsymbol{\mu }_{C1}\\[5pt] \mathbf{N}_2+\mathbf{N}_3+\boldsymbol{\mu }_{C2}+\boldsymbol{\mu }_{C3}\\[5pt] \mathbf{N}_4+\boldsymbol{\mu }_{C4}\\[5pt] \mathbf{N}_5+\boldsymbol{\mu }_{C5}\\[5pt] \mathbf{N}_{MP}+\boldsymbol{\mu }_{CMP}+\tilde{\mathbf{r}}_{B,P}\cdot \mathbf{F}_e+\mathbf{M}_e\\[5pt] \end{array} \right ] \end{equation}
The elements of Eq. (21) are as follows:
On the left-hand side:
\begin{equation*} \mathbf {F}_i=\left [ \begin {matrix} \mathbf {f}_{i1}^{\text {T}}& \mathbf {f}_{i2}^{\text {T}}& \mathbf {f}_{i3}^{\text {T}}& \mathbf {f}_{i4}^{\text {T}}& \mathbf {f}_{i5}^{\text {T}}& \mathbf {f}_{i6}^{\text {T}}\\[5pt] \end {matrix} \right ] ^{\textrm {T}}_{18\times 1} \end{equation*}
\begin{equation*} \boldsymbol {\mu }_i=\left [ \begin {matrix} \boldsymbol {\tau }_{i1}^{\text {T}}& \boldsymbol {\tau }_{i2}^{\text {T}}& \boldsymbol {\tau }_{i3}^{\text {T}}& \boldsymbol {\tau }_{i4}^{\text {T}}& \boldsymbol {\tau }_{i5}^{\text {T}}& \boldsymbol {\tau }_{i6}^{\text {T}}\\[5pt] \end {matrix} \right ] ^{\textrm {T}}_{18\times 1} \end{equation*}
\begin{equation*} \tilde {\mathbf {r}}_{B,i}=\left [ \begin {matrix} \tilde {\mathbf {r}}_{B,i1}^{\textrm {T}}& \tilde {\mathbf {r}}_{B,i2}^{\textrm {T}}& \tilde {\mathbf {r}}_{B,i3}^{\textrm {T}}& \tilde {\mathbf {r}}_{B,i4}^{\textrm {T}}& \tilde {\mathbf {r}}_{B,i5}^{\textrm {T}}& \tilde {\mathbf {r}}_{B,i6}^{\textrm {T}}\\[5pt] \end {matrix} \right ] ^{\textrm {T}}_{18\times 3} \end{equation*}
On the right-hand side,
$\mathbf{G}_i$
is the inertia forces vector composed of the inertia forces of links
$L_{ij}\,(j=1\sim 6)$
and
$\mathbf{G}_{MP}$
is the inertia force of the MP:
\begin{equation*} \mathbf {G}_i=\left [ \begin {array}{c} m_{i1}\left ( \ddot {\mathbf {p}}_{Ci1}-\mathbf {g} \right )\\[5pt] m_{i2}\left ( \ddot {\mathbf {p}}_{Ci2}-\mathbf {g} \right )\\[5pt] \vdots \\[5pt] m_{i6}\left ( \ddot {\mathbf {p}}_{Ci6}-\mathbf {g} \right )\\[5pt] \end {array} \right ]_{18\times 1} \end{equation*}
\begin{equation*} \mathbf {G}_{MP}=[m_{MP}\left ( \ddot {\mathbf {p}}_{CMP}-\mathbf {g} \right ) ]_{3\times 1} \end{equation*}
Then,
$\boldsymbol{\mu }_{Ci}$
represents the moment vector due to gravity of links
$L_{ij}\,(j=1\sim 6)$
.
$\boldsymbol{\mu }_{\textrm{CMP}}$
is similar.
\begin{equation*} \boldsymbol {\mu }_{Ci}=\left [ \begin {array}{c} m_{i1}\tilde {\mathbf {r}}_{B,Ci1}\left ( \ddot {\mathbf {p}}_{Ci1}-\mathbf {g} \right )\\[5pt] m_{i2}\tilde {\mathbf {r}}_{B,Ci2}\left ( \ddot {\mathbf {p}}_{Ci2}-\mathbf {g} \right )\\[5pt] \vdots \\[5pt] m_{i6}\tilde {\mathbf {r}}_{B,Ci6}\left ( \ddot {\mathbf {p}}_{Ci6}-\mathbf {g} \right )\\[5pt] \end {array} \right ] _{18\times 1} \end{equation*}
\begin{equation*} \boldsymbol {\mu }_{CMP}=[ m_{MP}\tilde {\mathbf {r}}_{B,CMP}\left ( \ddot {\mathbf {p}}_{CMP}-\mathbf {g} \right ) ] _{3\times 1} \end{equation*}
Under the theorem of the moment of momentum,
$\mathbf{N}_i$
represents the differential angular momentum vector of links
$L_{ij}\,(j=1\sim 6)$
and
$\mathbf{N}_{MP}$
is the differential angular momentum of the MP:
\begin{equation*} \mathbf {N}_i=\left [ \begin {array}{c} \dfrac {d}{dt}\left ( \mathbf {I}_{i1}\boldsymbol {\omega }_{i1} \right )\\[8pt] \dfrac {d}{dt}\left ( \mathbf {I}_{i2}\boldsymbol {\omega }_{i2} \right )\\[8pt] \vdots \\[5pt] \dfrac {d}{dt}\left ( \mathbf {I}_{i6}\boldsymbol {\omega }_{i6} \right )\\[5pt] \end {array} \right ] _{18\times 1} \end{equation*}
\begin{equation*} \mathbf {N}_{MP}=\left [ \frac {d}{dt}\left ( \mathbf {I}_{MP}\boldsymbol {\omega }_{MP} \right )\right ] _{3\times 1} \end{equation*}
Rearranging Eq. (21) and rewriting it in matrix form, we obtain the following:
\begin{equation} \mathbf{A}=\left [ \begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} \mathbf{I}_{18}& -\mathbf{I}_{18}& & & & & & & & \\[5pt] & \;\;\;\;\mathbf{I}_{18}& -\mathbf{I}_{18}& & & & & & & \\[5pt] & & \;\;\;\;\mathbf{I}_{18}& -\mathbf{I}_{18}& & & & & & \\[5pt] & & & \;\;\;\;\mathbf{I}_{18}& -\mathbf{I}_{18}& & & & & \\[5pt] & & & & \;\;\;\;\mathbf{I}_{3,18}& & & & & \\[5pt] \;\;\;\;\tilde{\mathbf{r}}_{\textrm{B},1 }& -\tilde{\mathbf{r}}_{\textrm{B},2 }& & & & \;\;\;\;\mathbf{e}_1& -\mathbf{e}_2& & & \\[5pt] & \;\;\;\;\tilde{\mathbf{r}}_{\textrm{B},2 }& -\tilde{\mathbf{r}}_{\textrm{B},4 }& & & & \;\;\;\;\mathbf{e}_2& -\mathbf{e}_4& & \\[5pt] & & \;\;\;\;\tilde{\mathbf{r}}_{\textrm{B},4 }& -\tilde{\mathbf{r}}_{\textrm{B},5 }& & & & \;\;\;\;\mathbf{e}_4& -\mathbf{e}_5& \\[5pt] & & & \;\;\;\;\tilde{\mathbf{r}}_{\textrm{B},5 }& -\tilde{\mathbf{r}}_{\textrm{B},6 }& & & & \;\;\;\;\mathbf{e}_5& -\mathbf{e}_6\\[5pt] & & & & \;\;\;\;\;\;\;\mathbf{Sr}_{\textrm{B},6}& & & & & \;\;\;\;\;\;\;\mathbf{Se}_{6}\\[5pt] \end{array} \right ] _{150\times 150} \end{equation}
where
$\mathbf{I}_{18}$
denotes the identity matrix of order 18 and
\begin{equation*} \mathbf {I}_{3,18}=\left [ \begin {matrix} \,\mathbf {I}_3& \cdots & \mathbf {I}_3\,\\[5pt] \end {matrix} \right ] _{3\times 18} \end{equation*}
The orientations of the torques on the links are
\begin{equation*} \mathbf {e}_i=\left [ \begin {array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} \mathbf {X}_{i1}& \mathbf {Y}_{i1}& & & & & & \\[5pt] & & \mathbf {X}_{i2}& \mathbf {Y}_{i2}& & & & \\[5pt] & & & & \ddots & \ddots & & \\[5pt] & & & & & & \mathbf {X}_{i6}& \mathbf {Y}_{i6} \end {array} \right ] _{18\times 12} \end{equation*}
and
\begin{equation*} \mathbf {Sr}_{\textrm {B},6}\,\,=\,\,\left [ \begin {matrix} \tilde {\mathbf {r}}_{\textrm {B},61}& \tilde {\mathbf {r}}_{\textrm {B},62}& \dots & \tilde {\mathbf {r}}_{\textrm {B},65}& \tilde {\mathbf {r}}_{\textrm {B},66} \end {matrix} \right ] _{3\times 18}\\[5pt] \end{equation*}
\begin{equation*} \mathbf {Se}_{6}=\left [ \begin {matrix} \mathbf {X}_{61}& \mathbf {Y}_{61}& \dots & \mathbf {X}_{66}& \mathbf {Y}_{66}\\[5pt] \end {matrix} \right ] _{3\times 12} \end{equation*}
Here,
\begin{equation} \mathbf{x}=\left [ \begin{matrix} \mathbf{F}^{\textrm{T}}_1& \mathbf{F}^{\textrm{T}}_2& \mathbf{F}^{\textrm{T}}_4& \mathbf{F}^{\textrm{T}}_5& \mathbf{F}^{\textrm{T}}_6& \boldsymbol{\tau }_{1}^{\textrm{T}}& \boldsymbol{\tau }_{2}^{\textrm{T}}& \boldsymbol{\tau }_{4}^{\textrm{T}}& \boldsymbol{\tau }_{5}^{\textrm{T}}& \boldsymbol{\tau }_{6}^{\textrm{T}}\\[5pt] \end{matrix} \right ] ^{\textrm{T}}_{150\times 1} \end{equation}
where
$\boldsymbol{\tau }_{i}=\left [ \begin{matrix} \tau _{i1x}& \tau _{i1y}& \tau _{i2x}& \tau _{i2y}& \cdots & \tau _{i6x}& \tau _{i6y}\\[5pt] \end{matrix} \right ] ^{\textrm{T}}_{12\times 1}$
. The vector
$\mathbf{x}$
contains
$150$
elements, where elements 37–54 contain the required actuated forces.
All terms of
$\mathbf{b}$
are known values on the right side of Eq. (21). Therefore, Eq. (21) can be rewritten as:
\begin{equation} \mathbf{A}\cdot \mathbf{x}=\mathbf{b} \end{equation}
As long as the PM is not in a singular configuration or has actuated redundancy, matrix A is a full-rank matrix,
$|\mathbf{A}|\ne 0$
, and the system of equations has a unique solution. Furthermore, approximately 95.68% of the elements in the matrix are zeros, indicating that it is a sparse matrix. To improve solving speed, it can be converted into a sparse matrix during computations, and the GMRES [Reference Saad and Schultz36] can be used.
Thus, we choose
$\mathbf{x}_0 \in \mathbb{R}^{150\times 1}$
as an initial vector and set
\begin{equation*} \mathbf {r}_0 = \mathbf {b}-\mathbf {A}\mathbf {x}_0, \quad \mathbf {v}_1 = \frac {\mathbf {r}_0}{\|\mathbf {r}_0\|},\quad \mathbf {\beta } =||\mathbf {r}_0|| \end{equation*}
And then, use the Arnoldi process, for
$n=1,2,\cdots,k,\cdots$
, we do
\begin{equation*} \begin {aligned} {h}_{m,n}&=(\mathbf {A}\mathbf {v}_{n})^{\text {T}}\mathbf {v}_{m},\quad m=1,2,\cdots,n\\[5pt] \hat {\mathbf {v}}_{n+1}&{=\mathbf {A}\mathbf {v}_{n}-\sum ^{n}_{m=1}{h}_{m,n}\mathbf {v}_m}\\[5pt] {{h}_{n+1,n}}&{=||\hat {\mathbf {v}}_{n+1}||}\\[5pt] {\mathbf {v}_{n+1}}&{=\hat {\mathbf {v}}_{n+1}/{h}_{n+1,n}} \end {aligned} \end{equation*}
until
${h}_{n+1,n}\lt \epsilon$
.
$\epsilon$
is the upper limit of the error based on the specific situation.
And we can get the orthonormal basis
$\mathbf{V}_k = [\mathbf{v_1},\dots,\mathbf{v}_k]_{{150\times k}}$
and the matrix elements
$h_{m,n}$
form the matrix
$\mathbf{H}_{k}{\in \mathbb{R}^{(k+1)\times k}}$
.
Solve the linear least squares as:
\begin{equation} \text{min}||\beta \mathbf{e}-{\mathbf{H}}_{k}\mathbf{y}_k||\,, \end{equation}
and
$\mathbf{e}=[1,0,\cdots,0]_{(k+1)\times 1}$
. Note the result as
$\mathbf{y}_k$
.
Finally, the numerical solution
\begin{equation} \mathbf{x} = \mathbf{x}_0 + \mathbf{V}_k\mathbf{y}_k\,. \end{equation}
The set of actuated forces is denoted as
$\mathbf{F}_{4j\textrm{z}}$
:
\begin{equation}{\mathbf{F}_{4j\textrm{z}}\,{=\left [ \begin{matrix} f_{41z} &\dots &f_{46z}\end{matrix} \right ] ^{\textrm{T}}}}\,, \end{equation}
where
\begin{equation*} {{f_{4j\textrm {z}}=\left [ \begin {matrix} \mathbf {x}\left ( 34+3j \right )& \, \mathbf {x}\left ( 35+3j \right )& \, \mathbf {x}\left ( 36+3j \right )\\[5pt] \end {matrix} \right ] \cdot \mathbf {Z}_{4j}}}\\[5pt] \end{equation*}
5. Simulations
To validate the correctness of the dynamics of the 6-RR-RP-RR PM, we compare the results of the mathematical dynamic model with those of a simulation model. The model described in this paper consists of both inverse kinematics and inverse dynamics. The simulation model, which is established using the automatic dynamic analysis of multibody systems software, provides both kinematic and dynamic solutions through various probes.
First, the structure parameters of the PM must be defined. From Fig. 2, these parameters are as listed in Table II. And their dynamic parameters are as listed in Table III.
Structure parameters of PM for numerical simulations.
Dynamic parameters of PM for numerical simulations.
To verify the accuracy of the algorithm and facilitate computation, the specific parameters of the actual PM are simplified to ideal parameters in the simulation.
${\left [I_{xy},I_{xz},I_{yz}\right ]}^{\text{T}} = [0,0,0]^{\text{T}}$
The MP has 6-DOFs in space and consists of translation and rotation motions along the three directions of the frame
$\mathcal{B}$
. Let the MP move along the spiral line, with arbitrary angle changes in the motion process. The parametric equations of the trajectory under the frame
$\mathcal{B}$
are
\begin{equation} \begin{cases} p_x=0.005\sin \left ( t \right )& (m)\\[5pt] p_y=0.005\cos \left ( t \right )& (m)\\[5pt] p_z=0.002\,t& (m)\\[5pt] \varphi \,\,={\frac{\pi }{180}}\sin \left ( t \right ) & (rad)\\[5pt] \theta \,\,={\frac{2\pi }{180}} \sin \left ( t \right ) & (rad)\\[5pt] \psi \,\,={\frac{5\pi }{180}} \sin \left ( t \right ) & (rad)\\[5pt] \end{cases} \end{equation}
The displacement trajectory comparison is illustrated in Fig. 7. From the figure, it is evident that the mathematical results closely align with the simulations.
Kinematic comparison (MM – mathematical model, SM – simulation model).
From such a complex trajectory, we can learn how the algorithm performs in a real-world condition rather than in a well-designed environment. Furthermore, we conducted simulations under three different operating conditions to validate the correctness of the algorithm, and these conditions are presented in Table IV.
The spiral trajectory equation has illustrated as Eq. (28), and the acceleration of gravity is as follows:
\begin{equation*} \textbf {g} = [0,0,-9.8]^{\text {T}}\,(m/s^2). \end{equation*}
And the external action contains a force vector
$\mathbf{F}_{e}^{\mathcal{P}}$
and a torque vector
$\textbf{M}_e^{\mathcal{P}}$
, which executed on the frame
$\mathcal{P}$
, as:
\begin{equation*} \textbf {F}_e^{\mathcal {P}} = [10\sin (t),10\cos (t),-10\sin (t)]^{\text {T}}\,(N),\quad \textbf {M}_e^{\mathcal {P}} = [\sin (t),2\cos (t),3\sin (t)]^{\text {T}}\,(N\cdot m), \end{equation*}
\begin{equation*} \textbf {F}_e = \mathbf {T}_{\mathcal {P}}\cdot \textbf {F}_e^{\mathcal {P}},\qquad \textbf {M}_e = \mathbf {T}_{\mathcal {P}}\cdot \textbf {M}_e^{\mathcal {P}}. \end{equation*}
In Condition 1, we consider the influence of inertia forces resulting from motion on the actuated force, while disregarding gravity and external actions. As shown in Fig. 8, the graph simultaneously includes the actuated force curves of both the mathematical model (MM) and the simulation model (SM). The right-hand coordinate system in the graph represents the average relative error between the two models. This means obtaining the relative error between the two models for each individual leg and then averaging these relative errors across the six legs. The error exhibits a random zigzag pattern, unrelated to inertia forces and motion conditions, with magnitudes at the order of
$10^{-4}$
. The spike observed at
$0.8s$
is considered arising as the actuated force approaches zero. Therefore, the algorithm remains reliable when considering only inertia forces.
Three different operating conditions.
Condition 1, a comparison of the models considering only inertia forces.
In Condition 2, both inertia forces and gravity are considered simultaneously. As shown in Fig. 9, the actuated forces noticeably increase. And since gravity acceleration is a constant value unaffected by simulation software computation errors, the error between the simulation results and the mathematical model significantly decreases. Furthermore, both models remain consistent, indicating that the algorithm remains reliable.
Condition 2, a comparison of the models considering both inertia forces and gravity.
Finally, in Condition 3, as shown in Fig. 10, inertia forces, gravity, and external actions are taken into account. As a result, actuated forces increase further, but the magnitude of the error has not changed significantly. Hence, the primary factor contributing to the error is presumably the acceleration error of the simulation.
Condition 3, a comparison of the models considering inertia forces, gravity, and external actions.
In summary, we can conclude that the algorithm can be considered reliable, and the error is unrelated to motion.
6. Conclusion
This paper has described the inverse dynamic analysis of a 6-RR-RP-RR PM with offset universal joints. The introduction of RR–joints means that the dynamic model must be represented as an implicit system of equations. Based on the existing inverse kinematic model, this study established an iterative algorithm using rigid body velocity transformations, resulting in the velocity and acceleration input parameters of all links in the base frame. The Newton–Euler method was then used to analyze the inverse dynamics of the PM, and dynamic equations for all links were established in the form of a equation system containing 150 unknowns and 150 equations. By solving the equation system, a vector
$\mathbf{X}$
containing all joint forces and torques, including the desired actuated force
$\mathbf{F}_{4jz}$
, was obtained. The correctness of the mathematical model was verified through numerical simulations using the automatic dynamic analysis of multibody systems software. The algorithm closely aligns with the simulation and is deemed reliable. The derived equations can be used to determine the forces acting on all joints of the PM. They are not only useful for calculating the actuated force and force control, but also for structural optimization of PMs. In future research, the effect of friction will be studied by introducing dissipative forces into each Euler dynamic equation, and the PM will be validated through experiments.
Author contributions
Hasiaoqier Han concevied the study. Qingwen Wu and Huze Huang designed the study. Dawei Li and Huze Huang performed simulation analyses. Hasiaoqier Han and Zhenbang Xu derived the part of kinematics, and Huze Huang derived the part of dynamics. Huze Huang wrote the article.
Financial support
This work was supported by the National Natural Science Foundation of China [Grant Nos. 52005478, 62235018] and the Youth Innovation Promotion Association, Chinese Academy of Sciences [Grant No. 2022215].
Competing interests
All authors disclosed no relevant relationships.
Ethical approval
None.
Appendix Details of the inverse kinematic
The reference frame transformation relation for the
$j$
th leg is as follows:
\begin{equation} \mathbf{T}_{leg}=\mathbf{T}_1\cdot \mathbf{T}_2\cdot \mathbf{T}_3\cdot \mathbf{T}_4\cdot \mathbf{T}_5\cdot \mathbf{T}_6 \end{equation}
Because the frames
$\mathcal{P}_j$
and
$\mathcal{B}_j$
are known in frames
$\mathcal{P}$
and
$\mathcal{B}$
, when the position of the MP is determined, the equivalent reference frame transformation relation for the
$j$
th leg is also obtained. The homogeneous transformation matrix of its frame system
$\mathbf{T}_{leg}^{\prime }$
is given by:
\begin{equation} \mathbf{T}_{leg}^{\prime }=\mathbf{T}_{\mathcal{B}_j}^{^{-1}}\cdot \mathbf{T}_{\mathcal{P}}\cdot ^{\mathcal{P}}\mathbf{T}_{\mathcal{P}_j} \end{equation}
where
$\mathbf{T}_{\mathcal{B}_j}$
represents the transformation matrix of
$\mathcal{B}_j$
concerning the frame
$\mathcal{B}$
,
$\mathbf{T}_{\mathcal{P}}$
represents the transformation matrix of the frame
$\mathcal{P}$
concerning the frame
$\mathcal{B}$
, and
$^{\mathcal{P}}\mathbf{T}_{\mathcal{P}_j}$
represents the transformation matrix of the frames
$\mathcal{P}_j$
concerning the frame
$\mathcal{P}$
.
As demonstrated by the equivalence relation, Eq. (29) is equivalent to Eq. (30):
\begin{equation} \mathbf{T}_{leg}-\mathbf{T}_{leg}^{\prime }=\mathbf{Q}=0 \end{equation}
At this point, we have the equivalent relation matrix for one leg. Equation (31) contains all six unknowns of
$\mathbf{q}_{ij}$
. Each element can be considered as a separate equation, so the six matrix elements can be selected to form the following nonlinear system of equations:
\begin{equation} \boldsymbol{\Phi }_j=\begin{cases} \Phi _1\left ( \mathbf{q}_{ij}^{\textrm{T}} \right ) =\mathbf{Q}\left ( 1,2 \right ) =0\\[5pt] \Phi _2\left ( \mathbf{q}_{ij}^{\textrm{T}} \right ) =\mathbf{Q}\left ( 1,3 \right ) =0\\[5pt] \Phi _3\left ( \mathbf{q}_{ij}^{\textrm{T}} \right ) =\mathbf{Q}\left ( 1,4 \right ) =0\\[5pt] \Phi _4\left ( \mathbf{q}_{ij}^{\textrm{T}} \right ) =\mathbf{Q}\left ( 2,1 \right ) =0\\[5pt] \Phi _5\left ( \mathbf{q}_{ij}^{\textrm{T}} \right ) =\mathbf{Q}\left ( 2,2 \right ) =0\\[5pt] \Phi _6\left ( \mathbf{q}_{ij}^{\textrm{T}} \right ) =\mathbf{Q}\left ( 3,1 \right ) =0\\[5pt] \end{cases} \end{equation}
Each of these equations is nonlinear and contains several variables. Thus, the solution can only be obtained indirectly. Therefore, the Newton–Raphson iteration method is applied to solve this nonlinear system for the
$j$
th leg:
\begin{equation} \left [ \mathbf{q}_{ij} \right ] _{\left ( k+1 \right )}=\left [ \mathbf{q}_{ij} \right ] _{\left ( k \right )}-\left [ \frac{\partial \boldsymbol{\Phi }_j}{\partial \mathbf{q}_{ij}} \right ] _{\left ( k \right )}^{-1}\cdot \left [ \mathbf{q}_{ij} \right ] _{\left ( k \right )} \end{equation}
