Dynamic Modeling Method of Multibody System of 6-DOF Robot Based on Screw Theory

Abstract

An accurate dynamic model is a prerequisite for realizing precise control of industrial robots. The dynamics research of multi-degree of freedom (DOF) robots is relatively unexplored and needs to be solved urgently. In this paper, a dynamic modeling method of multibody system of 6-DOF robot is proposed based on the screw theory. The established dynamic model has a more concise and unified mathematical form, and the modular matrix expression is convenient for the control of the robot. In order to ensure that the screw method is suitable for motion in a wide range of angles, quaternions are used as generalized angular coordinates, and the model established thereby eliminates singularities and improves computational efficiency. The correctness and accuracy of the screw method is verified by the simulation example, and the modeling theory and method can provide a theoretical basis for the precise control of the robot.

1. Introduction

The shortcoming of the low absolute positioning accuracy of the serial robot makes it incapable of precision machining. The fundamental way to solve this problem is to realize the precise control of the robot based on dynamics, that is to achieve precise force control and position control at the same time so as to meet the requirements of intelligence and precision. Therefore, accurate dynamic model is of great significance to the application of serial robots in high-end manufacturing.

The research of the robot dynamics based on traditional methods has been widely carried out. The traditional dynamic methods are Newton–Euler method and Lagrange method. The Newton–Euler method is a vector mechanics method and a recursive method. The motion parameters of the latter rigid body are obtained recursively from the former rigid body. The increase in the number of rigid bodies will make the derivation process cumbersome and complicated, and the parameters coupling makes the dynamic problem a nonlinear problem that is difficult to solve, so it is suitable for problems with few degrees of freedom (DOFs), but not for problems with multiple DOFs. The Lagrange method is an analytical mechanics method. The Lagrange quantity in the form of energy will become cumbersome and complicated with the increase of the number of rigid bodies, and the phenomenon of parameters coupling will intensify, which significantly increases the number of dynamic equations and the difficulty of solving them. Therefore, it is suitable for problems with few DOFs, but not for problems with multiple DOFs.

Modern mathematical method based on screw theory, Lie groups and Lie algebras, have been used in the research of robot dynamics. It is a vector mechanics method, and the dynamic equations expressed have the advantages of simplicity and unity. The existing screw method, like Newton–Euler method, is also a recursive method. Although it is possible to describe and solve dynamic problems more concisely and uniformly, when dealing with multi-DOF problems, parameters coupling also makes the calculation complicated and difficult to solve.

Dynamics of multibody systems is a branch of classical mechanics. With the development of computing technology, it has gradually become an independent discipline which studies systems composed of a large number of rigid bodies. Dynamics of multibody systems describe the interconnections between rigid bodies by graph theory. Firstly, the dynamic model of a single rigid body is established by vector mechanics, analytical mechanics, or Kane method. Then, the dynamic equations of each rigid body are combined to obtain the model of the system by graph theory, and the constraint equations are established to eliminate the redundant variables in the model and make the free variables equal to the number of DOF. The description of graph theory makes the dynamic model of the robot obtained by combining the dynamic models of each rigid body. Since the dynamic models of each rigid body are independent of each other, the coupling of the dynamic and kinematic parameters between the rigid bodies is eliminated. Decoupling is a necessary condition for solving a multi-DOF dynamic model, and the resulting dynamic model has two advantages. (1) Decoupling enables the multi-DOF dynamic model to have an analytical solution, which is convenient for the solution and control of the model. (2) Decoupling enables the dynamic equations of each rigid body to be combined and expressed as a modular matrix equation. The matrix equation, as the dynamic model of the robot, has a simple and compact form and is easy to control. It should be noted that when traditional dynamic methods and existing screw methods deal with multi-DOF dynamic problems, due to the difficulty of decoupling dynamic and kinematic parameters, the dynamic equations of each rigid body can only exist independently and cannot be unified into a modular matrix equation, which brings difficulties to the solution and control. Therefore, dynamics of multibody systems is a powerful tool for studying multi-DOF mechanisms and is suitable for analyzing complex systems composed of a large number of components in the fields of robots, vehicles, and aerospace.

Aiming at the problem of accurate dynamic modeling of multi-DOF serial robots, 6-DOF robot is selected as the research object in this paper. A dynamic modeling method of multibody system based on screw theory is proposed for the first time. This method has the advantages of simple and compact expression of screw theory and being good at solving multi-DOF dynamic problems of dynamics of multibody systems. Compared with the previous screw method, this screw method has the following characteristics. (1) The kinematic and dynamic parameters can be decoupled, and the dynamic model can be expressed in the form of a modular matrix, which is convenient for control. This method can express the dynamic model of the 6-DOF robot with only one matrix equation, so it has a more concise and unified mathematical expression. (2) By establishing and introducing constraint equations, the ideal constraining force and moment can be evaluated and analyzed. (3) Quaternions are used as generalized coordinates in this method, which eliminates singularities, improves computational efficiency, and makes the robot suitable for large-angle range motion. It should be noted that the traditional dynamics method and the existing screw method are suitable for solving problems with few DOFs. While the dynamics of multibody systems are suitable for solving problems with multiple DOFs, the applications of the two are different. The number of dynamic equations of the problem with few DOFs is few, the expression is relatively simple, and the calculation difficulty is relatively small. The calculation and solution of the problem do not have to rely on computers. The multiple-DOF problem will inevitably lead to more dynamic equations and more complex expressions. Therefore, the calculation is very difficult, and the calculation and solution must be completed with the help of computers. In fact, it is the development of computer and computing technology that promotes the development of dynamics of multibody systems. Therefore, in terms of computational efficiency, it is obvious that the problem with few DOFs is higher than the problem with multiple DOFs. However, for the multi-DOF problem, the computational efficiency can be improved by selecting the quaternion as the generalized angular coordinate. This is because when calculating the coordinate transformation matrix, the sine and cosine functions of the angle need to be calculated 29 times when the Euler angle or the Cardano angle is used as the angle coordinate, and the sine and cosine functions do not need to be calculated when the quaternion is used as the angle coordinate. The coordinate transformation matrix needs to be calculated many times in the dynamic solution process, so the quaternion can greatly reduce the calculation steps and improve the calculation efficiency. The dynamic model established by this method is correct and accurate, which is verified by a numerical simulation example. The dynamic theory and method in this paper can provide a theoretical basis for precise control and structural reliability analysis.

2. Related Work

2.1. Traditional Dynamics Method

The dynamics research based on the traditional Newton–Euler method and Lagrange method has been widely carried out and achieved corresponding results. Khalil et al. [1] studied the forward and inverse dynamics of the Gough–Stewart robot. The dynamic model of the robot is established according to the dynamic model of the legs. The dynamic model established by the Newton–Euler method has an intuitive physical explanation. In order to reduce the computational cost, the basic inertial parameters of the robot are determined through analysis. Djuric et al. [2] studied the dynamics of a reconfigurable Puma-Fanuc robot, established a dynamics model with the Newton–Euler recursion method, and each element of the inertial matrix, Coriolis moment matrix, centrifugal moment matrix, and gravitational moment vector was automatically generated using a newly developed automatic separation method. Ibrahim et al. [3] studied a hybrid robot with parallel modules connected in series and established the forward and inverse dynamics models of the robot by the Newton–Euler recursion method. The proposed algorithm can be programmed with symbolic and inertial parameters. Koopaee et al. [4] developed a two-dimensional modular snake-like robot, established the dynamic model of the robot by the Euler–Lagrange equation, and proposed an adaptive controller based on torque feedback in the gait parameter space and optimized the control gain. The effectiveness of the model and controller is verified by simulation and experiment. Kalyoncu et al. [5] studied a flexible robotic manipulator with rotating prismatic joints and established a dynamic model of the manipulator based on the Lagrange equation. Under the action of external driving force and axial force, the end of the flexible manipulator moves along a path composed of multiple straight lines. Li et al. [6] studied a 2-DOF planar parallel robot with flexible links for high-speed pick-up and placement operations. The dynamic model of the robot is established by the Euler–Lagrange method, and the dynamic characteristics of the robot are analyzed based on the model, which improves the dynamic accuracy of the end effector at high speed. Traditional dynamic methods are mostly used in the research of less-DOF robots, but they are difficult to be used in the research of multi-DOF robots. The Newton–Euler method is a recursive method. The motion parameters of the latter rigid body are obtained based on the motion parameters of the former rigid body. Therefore, the more DOFs, the more lengthy the dynamic equations and the larger the number of equations, which makes the dynamic model difficult to solve. The Lagrange method is a dynamic method based on the Lagrange quantity and derived by combining the variational method with the principle of least action. The Lagrange quantity in the form of energy becomes lengthy as the number of rigid bodies increases, which makes the Lagrange equations lengthy, and the more generalized coordinates due to the increased number of rigid bodies also increase the number of Lagrange equations. The solution of the dynamic model thus becomes very difficult. The lengthy dynamic expressions and more dynamic equations make the dynamic model difficult to solve for two reasons. (1) The coupling of dynamic parameters (mass and principal moment of inertia) and kinematic parameters make it difficult to obtain analytical solutions of the dynamic equations. (2) The ideal constraining force and moment that cannot be measured for each hinge increase the unknown quantities in the equation, which brings difficulties to the solution and control of the dynamic model. The essence of ideal constraining force and moment is the constraint condition that makes the robot move according to objective laws, and the robot without constraint conditions cannot be controlled, so the ideal constraining force and moment need to be solved.

2.2. Screw Theory

The screw theory originated in the 19th century, and with the evolution of algebra to geometric algebra, it is interrelated with the newly born Lie groups and Lie algebras, which together constitute the modern mathematical foundation of robotics. The screw theory is a vector mechanics method, so the physical meaning is clear, and it studies the translation and rotation of a straight line in space. The algebraic connotation of screw theory is embodied in screw algebra, which is a vector algebra used to describe screw, and is also a subalgebra of Lie algebra. Screw algebra, Lie group, and Lie algebra are used to describe the motion of straight lines in space and their related algebraic operations. The description has geometrical intuition and algebraic abstraction. Screw is a ray passing through the origin in Lie algebra, a geometric quantity, and an element of projective Lie algebra. A twist is a screw with velocity amplitude, which describes the motion of rigid body and is an element of Lie algebra. A wrench is a screw with force amplitude and is an element of dual Lie algebra [7,8]. The screw theory unifies velocity and angular velocity into twist, and unifies force and moment into wrench, which can succinctly and uniformly describe the motion of spatial mechanisms and solve problems. At present, the modern mathematical methods of screw theory, Lie groups and Lie algebras, have been applied by some roboticists [9,10,11,12,13,14,15]. Gallardo et al. [16] proposed a method for dynamic analysis of parallel robots, which is based on screw theory and virtual work principle, and verified the generality of the method by applying it to a 2-DOF parallel spherical mechanism and a Gough–Stewart platform. Zhao et al. [17] studied the modeling method based on the screw theory, which can simplify the kinematics and dynamics of the manipulator, and proposed a dynamic coupling index based on the Hessian matrix. By applying the method to analyze the folding parallel robot, the correctness of the modeling method and coupling index is verified. Gallardo et al. [18] conducted kinematics and dynamics research on the 2(3-RPS) manipulator based on the screw theory and virtual work principle. The screw method reduces the calculation time, and the expression is simple and compact, which can be used to solve the velocity and acceleration analysis of the space mechanism. Gallardo et al. [19] studied the 4-PRURh and 4-PRURv parallel robots, established the kinematics and dynamics models of the robot with screw theory, deduced the DOF of the parallel manipulator, and obtained the input and output equations of the velocity and acceleration of the robot in a compact form. Gallardo et al. [20] proposed a kinematics and dynamics analysis method for solving modular spatially redundant manipulators. The manipulator is made up of optional three-legged parallel mechanisms in series. The kinematics and dynamics of the basic module are studied by the screw theory, and the expression of the basic module is extended to a spatially redundant manipulator. Ding et al. [21] established a dynamic model of a robot with spatially flexible links based on screw theory and proposed a Ding–Holzer method by making some reasonable assumptions about the system. The method takes into account the spatial force and deformation of the moving flexible arm and can be used to solve the dynamic modeling problem of the flexible arm with six-dimensional force and deformation at the tip. Compared with the traditional dynamic method, the existing screw method can make the expression of the dynamic model concise and compact, however, when applied to a multi-DOF robot, the coupling of dynamic and kinematic parameters and the solution of ideal constraints cannot be solved.

3. Dynamic Modeling of Multibody System Based on Screw Theory

3.1. General Equation of Dynamics

The general equation of dynamics is the equation of dynamics expressed in generalized angular coordinates, that is, the angular coordinates are not specified as Euler angles, Cardano angles, or quaternions.

The dynamic equation derived from the screw theory has a more concise and unified expression, which is convenient for the dynamic modeling and control of the robot. The dynamics of multibody system is a powerful tool for solving multi-DOF problems. It adopts the mathematical description of graph theory to decouple the dynamic and kinematic parameters of each rigid body and solves the dynamic model with the help of a computer. In this paper, a modeling method of 6-DOF robot is proposed by combining the screw theory with the dynamics method of multibody system. This method is applicable to the dynamic modeling and solution of 6-DOF robot. The dynamic model can be expressed by only one matrix equation, and the expression is more concise and unified.

The 6-DOF robot has 6 rotational DOFs and consists of 6 rigid bodies $B_i (i=1,2,\cdots ,6)$ and 6 hinges $O_1,O_2,\cdots ,O_6$, where $B_6$ is the end effector, as shown in Figure 1. In order to facilitate analysis and calculation, each rigid body is simplified as a homogeneous cylinder with mass $m_i$ and central inertia tensor ${\mathit{J}}_i$, each rigid body reference system $(O_1,{\underset{¯}{\mathit{e}}}^{(1)}),(O_2,{\underset{¯}{\mathit{e}}}^{(2)}),\cdots ,(O_6,{\underset{¯}{\mathit{e}}}^{(6)})$ is established at the position of the centroid $O_{c1},O_{c2},\cdots ,O_{c6}$, and the world reference system $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$ is established at the centroid of the bottom surface of the base $B_1$. The directions of the base vectors ${\mathit{e}}_1^{(i)},{\mathit{e}}_2^{(i)},{\mathit{e}}_3^{(i)}$ are shown in Figure 1b.

The multibody system dynamic equation of the robot consists of the dynamic equations of each rigid body. Therefore, the dynamic equation of the rigid body should be established first. The dynamic equation of $B_i$ is derived according to the screw theory (the underlined $\underset{¯}{}$ represents the matrix form).

$${\underset{¯}{\psi }}_i^{}{\underset{¯}{\dot{\widehat{v}}}}_i^{}+{\underset{¯}{\tilde{\widehat{v}}}}_i^{}{\underset{¯}{\psi }}_i^{}{\underset{¯}{\widehat{v}}}_i^{}={\underset{¯}{\widehat{F}}}_i^{}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(1)

Here, ${\underset{¯}{\psi }}_i^{}$, ${\underset{¯}{\widehat{v}}}_i^{}$, ${\underset{¯}{\dot{\widehat{v}}}}_i^{}$, and ${\underset{¯}{\widehat{F}}}_i^{}$ are the generalized inertia matrix, twist, acceleration screw, and wrench of $B_i$ respectively, and ${\underset{¯}{\tilde{\widehat{v}}}}_i^{}$ is a 6-order square matrix.

$${\underset{¯}{\psi }}_i^{}=\left[\begin{array}{cc}{m}_i{\underset{\_}{E}}_{3\times 3}& \\ & {\underset{¯}{J}}_i^{(i)}\end{array}\right]$$

(2)

$${\underset{¯}{\widehat{v}}}_i^{}={({\underset{¯}{v}}_i^{\mathrm{T}} {\underset{¯}{\omega }}_i^{(i)\mathrm{T}})}^{\mathrm{T}}={({\dot{x}}_i^{}\hspace{0.33em}{\dot{y}}_i^{}\hspace{0.33em}{\dot{z}}_i^{}\hspace{0.33em}{\omega }_{i1}^{(i)}\hspace{0.33em}{\omega }_{i2}^{(i)}\hspace{0.33em}{\omega }_{i3}^{(i)})}^{\mathrm{T}}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(3)

$${\underset{¯}{\dot{\widehat{v}}}}_i^{}={({\underset{¯}{\dot{v}}}_i^{\mathrm{T}} {\underset{¯}{\dot{\omega }}}_i^{(i)\mathrm{T}})}^{\mathrm{T}}={({\ddot{x}}_i^{}\hspace{0.33em}{\ddot{y}}_i^{}\hspace{0.33em}{\ddot{z}}_i^{}\hspace{0.33em}{\dot{\omega }}_{i1}^{(i)}\hspace{0.33em}{\dot{\omega }}_{i2}^{(i)}\hspace{0.33em}{\dot{\omega }}_{i3}^{(i)})}^{\mathrm{T}}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(4)

$${\underset{¯}{\tilde{\widehat{v}}}}_i^{}=\left[\begin{array}{cc}{\underset{¯}{\tilde{\omega }}}_i^{(i)}& {\underset{¯}{0}}_{3\times 3}\\ {\underset{¯}{\tilde{v}}}_i^{}& {\underset{¯}{\tilde{\omega }}}_i^{(i)}\end{array}\right]\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(5)

$${\underset{¯}{\widehat{F}}}_i^{}=\left[\begin{array}{c}{\underset{¯}{F}}_i^g-{\displaystyle \sum _{k=1}^{n_e}{S}_{ik}^e{\underset{¯}{F}}_k^e}\\ -{\displaystyle \sum _{k=1}^{n_e}{\underset{¯}{C}}_{ik}^{e(i)}\times {\underset{¯}{F}}_k^{e(i)}}-{\displaystyle \sum _{k=1}^{n_e}{S}_{ik}^{}{\underset{¯}{M}}_k^{a(i)}}\end{array}\right]\hspace{0.33em}(i=1,2,\cdots ,6)$$

(6)

${\dot{x}}_i^{}\hspace{0.33em}{\dot{y}}_i^{}\hspace{0.33em}{\dot{z}}_i^{}$ and ${\ddot{x}}_i^{}\hspace{0.33em}{\ddot{y}}_i^{}\hspace{0.33em}{\ddot{z}}_i^{}$ are the components of $B_i$’s velocity ${\mathit{v}}_i^{}$ and acceleration ${\dot{\mathit{v}}}_i^{}$ in the ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)},{\mathit{e}}_3^{(0)}$ direction, respectively; ${\omega }_{i1}^{(i)}\hspace{0.33em}{\omega }_{i2}^{(i)}\hspace{0.33em}{\omega }_{i3}^{(i)}$ and ${\dot{\omega }}_{i1}^{(i)}\hspace{0.33em}{\dot{\omega }}_{i2}^{(i)}\hspace{0.33em}{\dot{\omega }}_{i3}^{(i)}$ are the components of $B_i$‘s angular velocity ${\mathit{\omega }}_i^{}$ and angular acceleration ${\dot{\mathit{\omega }}}_i^{}$ in the ${\mathit{e}}_1^{(i)},{\mathit{e}}_2^{(i)},{\mathit{e}}_3^{(i)}$ direction, respectively (Superscript ${}_{}^{(i)}$ represents the expression relative to $(O_i,{\underset{¯}{\mathit{e}}}^{(i)})$, and no superscript represents the expression relative to $(O_i,{\underset{¯}{\mathit{e}}}^{(0)})$); ${\mathit{F}}_i^g$ is the gravity force of $B_i$; ${\mathit{F}}_i^e$, and ${\mathit{M}}_i^a$ are the force element and active moment that $B_{i-1}$ acts on $B_i$, respectively, and there are $n_e$ force elements and moments associated with $B_i$; the body-hinge vector ${\mathit{C}}_{ik}^{}$ is the vector between the hinge point and the centroid, and the directions are from the inside to the outside, namely: ${\mathit{C}}_{ik}^{}=\frac{1}{2}{l}_i{\mathit{e}}_1^{(i)} (i=1,2,\cdots ,6; k=i,i+1)$; the 3-order antisymmetric matrices ${\underset{¯}{\tilde{\omega }}}_i^{(i)}$ and ${\underset{¯}{\tilde{v}}}_i^{}$ and the incidence element $S_{ij}$ are defined as

$${\underset{¯}{\tilde{\omega }}}_i^{(i)}=\left[\begin{array}{ccc}0& -{\omega }_{i3}^{(i)}& {\omega }_{i2}^{(i)}\\ {\omega }_{i3}^{(i)}& 0& -{\omega }_{i1}^{(i)}\\ -{\omega }_{i2}^{(i)}& {\omega }_{i1}^{(i)}& 0\end{array}\right], {\underset{¯}{\tilde{v}}}_i^{}=\left[\begin{array}{ccc}0& -{\dot{z}}_i^{}& {\dot{y}}_i^{}\\ {\dot{z}}_i^{}& 0& -{\dot{x}}_i^{}\\ -{\dot{y}}_i^{}& {\dot{x}}_i^{}& 0\end{array}\right]$$

(7)

$$S_{ij}=\left\{\begin{array}{cc}1& O_j \mathrm{has} \mathrm{incidence} \mathrm{with} B_i \mathrm{and} \mathrm{starts} \mathrm{with} B_i\\ -1& O_j \mathrm{has} \mathrm{incidence} \mathrm{with} B_i \mathrm{and} \mathrm{ends} \mathrm{with} B_i\\ 0& O_j \mathrm{has} \mathrm{no} \mathrm{incidence} \mathrm{with} B_i\end{array}\right. \left(i,j=1,2,\cdots ,n\right)$$

(8)

In graph theory, the relationship between $O_j$ and $B_i$ is called incidence, $O_j$ starts with $B_i$ means that $B_i$ is the starting point of $O_j$, and $O_j$ ends with $B_i$ means that $B_i$ is the end point of $O_j$.

3.2. Dynamic Equation Based on Quaternion

Since the integral of angular velocity has no physical meaning, it cannot represent the angle. In practical applications, Euler angles, Cardano angles, Rodrigues parameters, or quaternions are selected as generalized angular coordinates. The first three all have singularities and are suitable for small-angle range motion. The quaternion has no singularity, is suitable for motion in a large angle range, and does not need to calculate the sine and cosine of the angle, which reduces the calculation steps and improves the calculation efficiency. For multi-DOF robots, quaternion coordinates are a better choice.

The angle in the dynamic Equation (1) of $B_i$ is expressed by the quaternion coordinate ${\underset{¯}{\Lambda }}_i={({\lambda }_{i0}\hspace{0.33em}{\lambda }_{i1}\hspace{0.33em}{\lambda }_{i2}\hspace{0.33em}{\lambda }_{i3})}^{\mathrm{T}}$, and the quaternion and the position coordinate ${\underset{¯}{r}}_i={(x_i\hspace{0.33em}{y}_i\hspace{0.33em}{z}_i)}^{\mathrm{T}}$ form the generalized coordinate ${\underset{¯}{q}}_i={({\underset{¯}{r}}_i^{\mathrm{T}}\hspace{0.33em}{\underset{¯}{\Lambda }}_i^{\mathrm{T}})}^{\mathrm{T}}$. The generalized velocity ${\underset{¯}{\dot{q}}}_i={({\underset{¯}{\dot{r}}}_i^{\mathrm{T}}\hspace{0.33em}{\underset{¯}{\dot{\Lambda }}}_i^{\mathrm{T}})}^{\mathrm{T}}$ and the generalized acceleration ${\underset{¯}{\ddot{q}}}_i={({\underset{¯}{\ddot{r}}}_i^{\mathrm{T}}\hspace{0.33em}{\underset{¯}{\ddot{\Lambda }}}_i^{\mathrm{T}})}^{\mathrm{T}}$ are derived from the first derivative ${\underset{¯}{\dot{\Lambda }}}_i={({\dot{\lambda }}_{i0}\hspace{0.33em}{\dot{\lambda }}_{i1}\hspace{0.33em}{\dot{\lambda }}_{i2}\hspace{0.33em}{\dot{\lambda }}_{i3})}^{\mathrm{T}}$ and the second derivative ${\underset{¯}{\ddot{\Lambda }}}_i={({\ddot{\lambda }}_{i0}\hspace{0.33em}{\ddot{\lambda }}_{i1}\hspace{0.33em}{\ddot{\lambda }}_{i2}\hspace{0.33em}{\ddot{\lambda }}_{i3})}^{\mathrm{T}}$ of ${\underset{¯}{\Lambda }}_i$.

Twist ${\underset{¯}{\widehat{v}}}_i^{}$ and acceleration screw ${\underset{¯}{\dot{\widehat{v}}}}_i^{}$ have the following relationships with generalized velocity ${\underset{¯}{\dot{q}}}_i$ and generalized acceleration ${\underset{¯}{\ddot{q}}}_i$,

$${\underset{¯}{\widehat{v}}}_i^{}={\underset{¯}{J}}_i^v(q){\underset{¯}{\dot{q}}}_i^{}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(9)

$${\underset{¯}{\dot{\widehat{v}}}}_i^{}={\underset{¯}{J}}_i^v(q){\underset{¯}{\ddot{q}}}_i^{}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(10)

Here, $J_i^v(q)$ is the Jacobian matrix

$${\underset{¯}{J}}_i^v(q)=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& -2{\lambda }_{i1}& 2{\lambda }_{i0}& 2{\lambda }_{i3}& -2{\lambda }_{i2}\\ 0& 0& 0& -2{\lambda }_{i2}& -2{\lambda }_{i3}& 2{\lambda }_{i0}& 2{\lambda }_{i1}\\ 0& 0& 0& -2{\lambda }_{i3}& 2{\lambda }_{i2}& -2{\lambda }_{i1}& 2{\lambda }_{i0}\end{array}\right]$$

(11)

Therefore, the dynamic Equation (1) is expressed as

$${\underset{¯}{\psi }}_i^{}{\underset{¯}{J}}_i^v(q){\underset{¯}{\ddot{q}}}_i^{}+{\underset{¯}{\tilde{\widehat{v}}}}_i^{}(q){\underset{¯}{\psi }}_i^{}{\underset{¯}{J}}_i^v(q){\underset{¯}{\dot{q}}}_i^{}={\underset{¯}{\widehat{F}}}_i^{}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(12)

Here, ${\underset{¯}{\tilde{\widehat{v}}}}_i^{}(q)$ is the generalized coordinate representation of ${\underset{¯}{\tilde{\widehat{v}}}}_i^{}$,

$${\underset{¯}{\tilde{\widehat{v}}}}_i^{}(q)=\left[\begin{array}{cc}2{\underset{¯}{R}}_i^{\ast }{\underset{¯}{\dot{R}}}_i^{\ast \mathrm{T}}& {\underset{¯}{0}}_{3\times 3}\\ {\underset{¯}{\tilde{v}}}_i& 2{\underset{¯}{R}}_i^{\ast }{\underset{¯}{\dot{R}}}_i^{\ast \mathrm{T}}\end{array}\right]\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(13)

${\underset{¯}{R}}_i^{\ast }, {\underset{¯}{\dot{R}}}_i^{\ast \mathrm{T}}$ are defined as

$${\underset{¯}{R}}_i^{\ast }=\left[\begin{array}{cccc}-{\lambda }_{i1}& {\lambda }_{i0}& {\lambda }_{i3}& -{\lambda }_{i2}\\ -{\lambda }_{i2}& -{\lambda }_{i3}& {\lambda }_{i0}& {\lambda }_{i1}\\ -{\lambda }_{i3}& {\lambda }_{i2}& -{\lambda }_{i1}& {\lambda }_{i0}\end{array}\right], {\underset{¯}{\dot{R}}}_i^{\ast \mathrm{T}}=\left[\begin{array}{ccc}-{\dot{\lambda }}_{i1}& -{\dot{\lambda }}_{i2}& -{\dot{\lambda }}_{i3}\\ {\dot{\lambda }}_{i0}& -{\dot{\lambda }}_{i3}& {\dot{\lambda }}_{i2}\\ {\dot{\lambda }}_{i3}& {\dot{\lambda }}_{i0}& -{\dot{\lambda }}_{i1}\\ -{\dot{\lambda }}_{i2}& {\dot{\lambda }}_{i1}& {\dot{\lambda }}_{i0}\end{array}\right]\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(14)

The unconstrained rigid body has 3 rotational DOFs, while the quaternion has four parameters, so the quaternions are not independent parameters and have the following constraints,

$${\underset{¯}{\Lambda }}_i^{\mathrm{T}}{\underset{¯}{\Lambda }}_i^{}-1=0\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(15)

In order to be combined with Equation (12), that is, to add constraint, Equation (15) needs to be derived twice with respect to time $t$, and rewritten into the generalized coordinate form, where the following constraint is obtained,

$${}^q\underset{¯}{\Lambda }{}_i^{\mathrm{T}}{\underset{¯}{\ddot{q}}}_i^{}+{}^q\underset{¯}{\dot{\Lambda }}{}_i^{\mathrm{T}}{\underset{¯}{\dot{q}}}_i^{}=0\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(16)

Here,

$${}^q\underset{¯}{\Lambda }{}_i^{\mathrm{T}}=\left[\begin{array}{cc}{\underset{¯}{0}}_{1\times 3}^{}& {\underset{¯}{\Lambda }}_i^{\mathrm{T}}\end{array}\right], {}^q\underset{¯}{\dot{\Lambda }}{}_i^{\mathrm{T}}=\left[\begin{array}{cc}{\underset{¯}{0}}_{1\times 3}^{}& {\underset{¯}{\dot{\Lambda }}}_i^{\mathrm{T}}\end{array}\right]\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(17)

Combining Equation (12) with Equation (17), that is, adding the quaternion self-constraint, the dynamic equation expressed by the generalized coordinate of $B_i$ is obtained,

$$\left[\begin{array}{c}{\underset{¯}{\psi }}_i^{}{\underset{¯}{J}}_i^v(q)\\ {}^q\underset{¯}{\Lambda }{}_i^{\mathrm{T}}\end{array}\right]{\underset{¯}{\ddot{q}}}_i^{}+\left[\begin{array}{c}\underset{¯}{\tilde{\widehat{v}}}{ }_i^{}(q){\underset{¯}{\psi }}_i^{}{\underset{¯}{J}}_i^v(q)\\ {}^q\underset{¯}{\dot{\Lambda }}{}_i^{\mathrm{T}}\end{array}\right]{\underset{¯}{\dot{q}}}_i^{}=\left[\begin{array}{c}{\underset{¯}{\widehat{F}}}_i^{}\\ 0\end{array}\right]\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(18)

Equation (18) can be abbreviated as

$$\underset{¯}{\psi }{ }_i^{}(q) {\underset{¯}{\ddot{q}}}_i^{}={\underset{¯}{\kappa }}_i^{}(q)\hspace{0.33em}\hspace{0.33em}(i=1,2,\cdots ,6)$$

(19)

Here,

$${\underset{¯}{\psi }}_i^{} (q)=\left[\begin{array}{c}{\underset{¯}{\psi }}_i^{}{\underset{¯}{J}}_i^v(q)\\ {}^q\underset{¯}{\Lambda }{}_i^{\mathrm{T}}\end{array}\right], {\underset{¯}{\kappa }}_i^{} (q)=\left[\begin{array}{c}{\underset{¯}{\widehat{F}}}_i^{}\\ 0\end{array}\right]-\left[\begin{array}{c}\underset{¯}{\tilde{\widehat{v}}}{ }_i^{}(q){\underset{¯}{\psi }}_i^{}{\underset{¯}{J}}_i^v(q)\\ {}^q\underset{¯}{\dot{\Lambda }}{}_i^{\mathrm{T}}\end{array}\right]{\underset{¯}{\dot{q}}}_i^{}$$

(20)

The dynamic equations Equation (19) of the 6 rigid bodies are combined to obtain the dynamic equation of the robot.

$$\underset{¯}{\psi }(q)\underset{¯}{\ddot{q}}=\underset{¯}{\kappa }(q)$$

(21)

Here,

$$\underset{¯}{\psi }(q)=diag({\underset{¯}{\psi }}_1^{}(q)\hspace{0.33em}{\underset{¯}{\psi }}_2^{}(q) \cdots {\underset{¯}{\psi }}_6^{}(q)),\underset{¯}{\ddot{q}}={({\underset{¯}{\ddot{q}}}_1^{\mathrm{T}}\hspace{0.33em}{\underset{¯}{\ddot{q}}}_2^{\mathrm{T}} \cdots {\underset{¯}{\ddot{q}}}_6^{\mathrm{T}})}^{\mathrm{T}},\underset{¯}{\kappa }(q)={({\underset{¯}{\kappa }}_1^{}{(q)}^{\mathrm{T}} {\underset{¯}{\kappa }}_2^{}{(q)}^{\mathrm{T}} \cdots {\underset{¯}{\kappa }}_6^{}{(q)}^{\mathrm{T}})}^{\mathrm{T}}$$

(22)

$\underset{¯}{\psi }(q)$ is a square matrix of order 42, and $\underset{¯}{\ddot{q}}$ and $\underset{¯}{\kappa }(q)$ are column matrices of order 42. The dynamic model Equation (21) of the robot has a total of 42 equations. The screw theory expresses the dynamic model of the robot as a set of equations. The form is simple and compact, which is convenient for calculation and control.

4. Quaternion-Based Constraint Equations

In the process of motion, there are certain relations between the generalized coordinates that describing the poses of each rigid body, and the analytical expressions of these relations are called constraint equations. A robot that satisfies the constraint equations can be controlled to move according to the true trajectory. The constraint equations of the robot are established by hinge constraints and quaternion self-constraints.

The robot has 6 rotation hinges, each of which has 1 rotational DOF and provides 5 constraints, namely 3 movement constraints and 2 rotation constraints. Furthermore, the angular coordinates that expressed by quaternions have their self-constraints. Specifically reflected as

(1) The hinge point of $B_j$ and its inside rigid body $B_{j-1}$ are the same point $O_j$,

$${\underset{¯}{r}}_i^{(0)}+{\underset{¯}{A}}^{(0i)}{\underset{¯}{C}}_{ij}^{(i)}-{\underset{¯}{r}}_j^{(0)}+{\underset{¯}{A}}^{(0j)}{\underset{¯}{C}}_{jj}^{(j)}=0$$

(23)

${\underset{¯}{A}}^{(0i)}$ is the coordinate transformation matrix that $(O_i,{\underset{¯}{\mathit{e}}}^{(i)})$ relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$,

$${\underset{¯}{A}}^{(0i)}=\left[\begin{array}{ccc}2({\lambda }_{i0}^2+{\lambda }_{i1}^2)-1& 2({\lambda }_{i1}{\lambda }_{i2}-{\lambda }_{i0}{\lambda }_{i3})& 2({\lambda }_{i1}{\lambda }_{i3}+{\lambda }_{i0}{\lambda }_{i2})\\ 2({\lambda }_{i1}{\lambda }_{i2}+{\lambda }_{i0}{\lambda }_{i3})& 2({\lambda }_{i0}^2+{\lambda }_{i2}^2)-1& 2({\lambda }_{i2}{\lambda }_{i3}-{\lambda }_{i0}{\lambda }_{i1})\\ 2({\lambda }_{i1}{\lambda }_{i3}-{\lambda }_{i0}{\lambda }_{i2})& 2({\lambda }_{i2}{\lambda }_{i3}+{\lambda }_{i0}{\lambda }_{i1})& 2({\lambda }_{i0}^2+{\lambda }_{i3}^2)-1\end{array}\right]$$

(24)

Equation (23) can be projected to the ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)},{\mathit{e}}_3^{(0)}$ direction to obtain three projection expressions, corresponding to 3 movement constraints.

(2) The base vector of the rotation axis of $B_j$ and its inside rigid body $B_{j-1}$ is the same base vector ${\mathit{p}}_j$,

$${\underset{¯}{A}}^{(0i)}{\underset{\_}{p}}_j^{(i)}-{\underset{¯}{A}}^{(0j)}{\underset{\_}{p}}_j^{(j)}=0$$

(25)

Equation (25) can be projected to the direction ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)},{\mathit{e}}_3^{(0)}$ to obtain three projection expressions. Since the projections $p_{j1}^{},p_{j2}^{},p_{j3}^{}$ of the base vector ${\mathit{p}}_j$ in the directions ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)},{\mathit{e}}_3^{(0)}$ have the relationship $p_{j1}^2+p_{j2}^2+p_{j3}^2=1$, only two of the three projection expressions are independent. Only take the projection expressions in the ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)}$ directions, corresponding to 2 rotation constraints.

(3) Quaternions are not independent parameters and are constrained by quaternion self-constraint Equation (15).

The constraint equations of the robot are obtained according to the projection expressions.

$${\Phi }_{11}=-x_1+\frac{l_1}{2}(2({\lambda }_{10}^2+{\lambda }_{11}^2)-1)=0$$

(26)

$${\Phi }_{12}=-y_1+l_1({\lambda }_{11}{\lambda }_{12}+{\lambda }_{10}{\lambda }_{13})=0$$

(27)

$${\Phi }_{13}=-z_1+l_1({\lambda }_{11}{\lambda }_{13}-{\lambda }_{10}{\lambda }_{12})=0$$

(28)

$${\Phi }_{14}=1-2({\lambda }_{10}^2+{\lambda }_{11}^2)=0$$

(29)

$${\Phi }_{15}=-2({\lambda }_{11}{\lambda }_{12}+{\lambda }_{10}{\lambda }_{13})=0$$

(30)

$${\Phi }_{16}={\lambda }_{10}^2+{\lambda }_{11}^2+{\lambda }_{12}^2+{\lambda }_{13}^2-1=0$$

(31)

$${\Phi }_{21}=x_1+\frac{l_1}{2}(2({\lambda }_{10}^2+{\lambda }_{11}^2)-1)-x_2+\frac{l_2}{2}(2({\lambda }_{20}^2+{\lambda }_{21}^2)-1)=0$$

(32)

$${\Phi }_{22}=y_1+l_1({\lambda }_{11}{\lambda }_{12}+{\lambda }_{10}{\lambda }_{13})-y_2+l_2({\lambda }_{21}{\lambda }_{22}+{\lambda }_{20}{\lambda }_{23})=0$$

(33)

$${\Phi }_{23}=z_1+l_1({\lambda }_{11}{\lambda }_{13}-{\lambda }_{10}{\lambda }_{12})-z_2+l_2({\lambda }_{21}{\lambda }_{23}-{\lambda }_{20}{\lambda }_{22})=0$$

(34)

$${\Phi }_{24}=2({\lambda }_{11}{\lambda }_{12}-{\lambda }_{10}{\lambda }_{13}-{\lambda }_{21}{\lambda }_{22}+{\lambda }_{20}{\lambda }_{23})=0$$

(35)

$${\Phi }_{25}=2({\lambda }_{10}^2+{\lambda }_{12}^2-{\lambda }_{20}^2-{\lambda }_{22}^2)=0$$

(36)

$${\Phi }_{26}={\lambda }_{20}^2+{\lambda }_{21}^2+{\lambda }_{22}^2+{\lambda }_{23}^2-1=0$$

(37)

$${\Phi }_{31}=x_2+\frac{l_2}{2}(2({\lambda }_{20}^2+{\lambda }_{21}^2)-1)-x_3+\frac{l_3}{2}(2({\lambda }_{30}^2+{\lambda }_{31}^2)-1)=0$$

(38)

$${\Phi }_{32}=y_2+l_2({\lambda }_{21}{\lambda }_{22}+{\lambda }_{20}{\lambda }_{23})-y_3+l_3({\lambda }_{31}{\lambda }_{32}+{\lambda }_{30}{\lambda }_{33})=0$$

(39)

$${\Phi }_{33}=z_2+l_2({\lambda }_{21}{\lambda }_{23}-{\lambda }_{20}{\lambda }_{22})-z_3+l_3({\lambda }_{31}{\lambda }_{33}-{\lambda }_{30}{\lambda }_{32})=0$$

(40)

$${\Phi }_{34}=2({\lambda }_{21}{\lambda }_{22}-{\lambda }_{20}{\lambda }_{23}-{\lambda }_{31}{\lambda }_{32}+{\lambda }_{30}{\lambda }_{33})=0$$

(41)

$${\Phi }_{35}=2({\lambda }_{20}^2+{\lambda }_{22}^2-{\lambda }_{30}^2-{\lambda }_{32}^2)=0$$

(42)

$${\Phi }_{36}={\lambda }_{30}^2+{\lambda }_{31}^2+{\lambda }_{32}^2+{\lambda }_{33}^2-1=0$$

(43)

$${\Phi }_{41}=x_3+\frac{l_3}{2}(2({\lambda }_{30}^2+{\lambda }_{31}^2)-1)-x_4+\frac{l_4}{2}(2({\lambda }_{40}^2+{\lambda }_{41}^2)-1)=0$$

(44)

$${\Phi }_{42}=y_3+l_3({\lambda }_{31}{\lambda }_{32}+{\lambda }_{30}{\lambda }_{33})-y_4+l_4({\lambda }_{41}{\lambda }_{42}+{\lambda }_{40}{\lambda }_{43})=0$$

(45)

$${\Phi }_{43}=z_3+l_3({\lambda }_{31}{\lambda }_{33}-{\lambda }_{30}{\lambda }_{32})-z_4+l_4({\lambda }_{41}{\lambda }_{43}-{\lambda }_{40}{\lambda }_{42})=0$$

(46)

$${\Phi }_{44}=2({\lambda }_{30}^2+{\lambda }_{31}^2-{\lambda }_{40}^2-{\lambda }_{41}^2)=0$$

(47)

$${\Phi }_{45}=2({\lambda }_{31}{\lambda }_{32}+{\lambda }_{30}{\lambda }_{33}-{\lambda }_{41}{\lambda }_{42}-{\lambda }_{40}{\lambda }_{43})=0$$

(48)

$${\Phi }_{46}={\lambda }_{40}^2+{\lambda }_{41}^2+{\lambda }_{42}^2+{\lambda }_{43}^2-1=0$$

(49)

$${\Phi }_{51}=x_4+\frac{l_4}{2}(2({\lambda }_{40}^2+{\lambda }_{41}^2)-1)-x_5+\frac{l_5}{2}(2({\lambda }_{50}^2+{\lambda }_{51}^2)-1)=0$$

(50)

$${\Phi }_{52}=y_4+l_4({\lambda }_{41}{\lambda }_{42}+{\lambda }_{40}{\lambda }_{43})-y_5+l_5({\lambda }_{51}{\lambda }_{52}+{\lambda }_{50}{\lambda }_{53})=0$$

(51)

$${\Phi }_{53}=z_4+l_4({\lambda }_{41}{\lambda }_{43}-{\lambda }_{40}{\lambda }_{42})-z_5+l_5({\lambda }_{51}{\lambda }_{53}-{\lambda }_{50}{\lambda }_{52})=0$$

(52)

$${\Phi }_{54}=2({\lambda }_{41}{\lambda }_{42}-{\lambda }_{40}{\lambda }_{43}-{\lambda }_{51}{\lambda }_{52}+{\lambda }_{50}{\lambda }_{53})=0$$

(53)

$${\Phi }_{55}=2({\lambda }_{40}^2+{\lambda }_{42}^2-{\lambda }_{50}^2-{\lambda }_{52}^2)=0$$

(54)

$${\Phi }_{56}={\lambda }_{50}^2+{\lambda }_{51}^2+{\lambda }_{52}^2+{\lambda }_{53}^2-1=0$$

(55)

$${\Phi }_{61}=x_5+\frac{l_5}{2}(2({\lambda }_{50}^2+{\lambda }_{51}^2)-1)-x_6+\frac{l_6}{2}(2({\lambda }_{60}^2+{\lambda }_{61}^2)-1)=0$$

(56)

$${\Phi }_{62}=y_5+l_5({\lambda }_{51}{\lambda }_{52}+{\lambda }_{50}{\lambda }_{53})-y_6+l_6({\lambda }_{61}{\lambda }_{62}+{\lambda }_{60}{\lambda }_{63})=0$$

(57)

$${\Phi }_{63}=z_5+l_5({\lambda }_{51}{\lambda }_{53}-{\lambda }_{50}{\lambda }_{52})-z_6+l_6({\lambda }_{61}{\lambda }_{63}-{\lambda }_{60}{\lambda }_{62})=0$$

(58)

$${\Phi }_{64}=2({\lambda }_{50}^2+{\lambda }_{51}^2-{\lambda }_{60}^2-{\lambda }_{61}^2)=0$$

(59)

$${\Phi }_{65}=2({\lambda }_{51}{\lambda }_{52}+{\lambda }_{50}{\lambda }_{53}-{\lambda }_{61}{\lambda }_{62}-{\lambda }_{60}{\lambda }_{63})=0$$

(60)

$${\Phi }_{66}={\lambda }_{60}^2+{\lambda }_{61}^2+{\lambda }_{62}^2+{\lambda }_{63}^2-1=0$$

(61)

Each rigid body is subject to 5 kinematic constraints and 1 quaternion self-constraint, and a robot consisting of 6 rigid bodies has a total of 36 constraints. The dynamic Equation (21) of the robot has 42 equations and after being subjected to 36 constraints, the DOF is 6, which is consistent with the actual situation.

5. Constrained Dynamic Model of Robot

Only if the robot meets the constraints can its motion be in accord with the objective laws. The constraint equations are added to the dynamic Equation (21) by the Lagrange multiplier method, and the constrained dynamic model is obtained. The Lagrange multipliers are proportional to the ideal constraining force or moment.

The constraint equation of the robot is written as

$$\underset{\_}{\Phi }(\underset{¯}{q})={({\underset{\_}{\Phi }}_1{}^{\mathrm{T}} {\underset{\_}{\Phi }}_2{}^{\mathrm{T}} {\underset{\_}{\Phi }}_3{}^{\mathrm{T}} {\underset{\_}{\Phi }}_4{}^{\mathrm{T}} {\underset{\_}{\Phi }}_5{}^{\mathrm{T}} {\underset{\_}{\Phi }}_6{}^{\mathrm{T}})}^{\mathrm{T}}$$

(62)

Here, ${\underset{\_}{\Phi }}_i={({\Phi }_{i1}{}^{} {\Phi }_{i2}{}^{} {\Phi }_{i3}{}^{} {\Phi }_{i4}{}^{} {\Phi }_{i5}{}^{} {\Phi }_{i6}{}^{})}^{\mathrm{T}} (i=1,2,\cdots ,6)$.

In order to be combined with Equation (21), the constraint Equation (62) needs to be written in the generalized acceleration form, therefore the Equation (62) is derived twice with respect to time $t$, and the following equation is obtained,

$${\underset{¯}{\Phi }}_q\underset{¯}{\ddot{q}}+{({\underset{¯}{\Phi }}_q\underset{¯}{\dot{q}})}_q\underset{¯}{\dot{q}}=\underset{¯}{0}\hspace{0.33em}\hspace{0.33em}$$

(63)

Here, ${\underset{\_}{\Phi }}_q$ is the Jacobian matrix of $\underset{\_}{\Phi }(\underset{¯}{q})$, and the generalized coordinates of each rigid body constitute the robot’s generalized coordinate ${\underset{¯}{q}}_{}={({\underset{¯}{q}}_1,{\underset{¯}{q}}_2,\cdots ,{\underset{¯}{q}}_6)}^{\mathrm{T}}={(q_1,q_2,\cdots ,q_{42})}^{\mathrm{T}}$.

$${\underset{¯}{\Phi }}_q=\left[\begin{array}{ccc}\frac{\partial {\Phi }_{11}}{\partial q_1}& \cdots & \frac{\partial {\Phi }_{11}}{\partial q_{42}}\\ ⋮& \ddots & ⋮\\ \frac{\partial {\Phi }_{66}}{\partial q_1}& \cdots & \frac{\partial {\Phi }_{66}}{\partial q_{42}}\end{array}\right]$$

(64)

${({\underset{¯}{\Phi }}_q\underset{¯}{\dot{q}})}_q$ is the Jacobian matrix of ${\underset{¯}{\Phi }}_q\underset{¯}{\dot{q}}$.

According to the Lagrange multiplier method, first, 36 Lagrange multipliers ${\lambda }_{ij}^{\ast } (i,j=1,2,\cdots ,6)$ are introduced into Equation (21) and combined into 6 Lagrange multiplier matrices ${\underset{¯}{\lambda }}_i^{\ast }=({\lambda }_{i1}^{\ast },{\lambda }_{i2}^{\ast },\cdots ,{\lambda }_{i6}^{\ast })\left(i=1,2,\cdots ,6\right)$, corresponding to the rigid body $B_i\left(i=1,2,\cdots ,6\right)$, respectively. ${\underset{¯}{\lambda }}_i^{\ast }$ constitutes the Lagrange matrix ${\underset{¯}{\lambda }}_{}^{\ast }={({\underset{¯}{\lambda }}_1^{\ast },{\underset{¯}{\lambda }}_2^{\ast },\cdots ,{\underset{¯}{\lambda }}_6^{\ast })}^{\mathrm{T}}$ of the robot, from which the Lagrange equation of first kind is obtained.

$$\underset{¯}{\psi }(q)\underset{¯}{\ddot{q}}-\underset{¯}{\kappa }(q)+{\underset{¯}{\Phi }}_q{}^{\mathrm{T}}\underset{¯}{{\lambda }^{\ast }}=0$$

(65)

Second, add constraints to Equation (65), that is, combine Equation (65) with constraint Equation (63).

$$\left[\begin{array}{cc}\underset{¯}{\psi }(q)& {\underset{¯}{\Phi }}_q{}^{\mathrm{T}}\\ {\underset{¯}{\Phi }}_q& \underset{¯}{0}\end{array}\right]\left[\begin{array}{c}\underset{¯}{\ddot{q}}\\ {\underset{¯}{\lambda }}_{}^{\ast }\end{array}\right]=\left[\begin{array}{c}\underset{¯}{\kappa }(q)\\ \underset{¯}{\zeta }\end{array}\right]$$

(66)

Here, $\underset{¯}{\zeta }=-{({\underset{¯}{\Phi }}_q\underset{¯}{\dot{q}})}_q\underset{¯}{\dot{q}}\hspace{0.33em}$.

Equation (66) is the constrained dynamic model of the robot, with a total of 78 equations, which can solve up to 78 unknowns. The expression form of this equation is compact and unified, and the modular expression is easy to control. The force control or drive control of the robot can be realized by planning the trajectory curve of $\underset{¯}{\ddot{q}}$ or ${\underset{¯}{\lambda }}_{}^{\ast }$, respectively. The Lagrange multiplier is proportional to the ideal constraining force or moment. By solving the Lagrange multipliers, the ideal constraining force and moment can be estimated, which can provide data and a theoretical basis for the analysis of the structural strength, reliability, and fatigue life of the robot.

6. Numerical Simulation Example

The robot is numerically simulated by Adams software to verify the correctness and accuracy of the dynamic Equation (66). As a precision machine, industrial robots are not easy to disassemble, which makes it impossible to measure the dynamic parameters of the rigid bodies that make up the robot by physical experiments, that is, the mass is obtained by weighing, and the principal moment of inertia is measured by the pendular motions. At the same time, the ideal constraint force and moment cannot be measured. Therefore, the mass $m_i$, the principal moment of inertia $J_{i_{11}}^{(i)} J_{i_{22}}^{(i)} J_{i_{33}}^{(i)}$ ($J_{i_{11}}^{(i)} J_{i_{22}}^{(i)} J_{i_{33}}^{(i)}$ are the components of ${\mathit{J}}_i$ in the ${\mathit{e}}_1^{(i)},{\mathit{e}}_2^{(i)},{\mathit{e}}_3^{(i)}$ direction, respectively), and the Lagrange multiplier ${\underset{¯}{\lambda }}_{}^{\ast }$ of each rigid body are taken as the 78 unknown parameters to be calculated, and the remaining parameters are sampled as known parameters, then the unknown parameters are solved by Equation (66).

First, the three-dimensional model of the robot is established by Adams software and simulation experiments are carried out.

Each rigid body $B_i$ that composes the robot is simplified to a homogeneous cylindrical rod, the parameters of length $l_i$ and radius $R_{ i}$ are provided, the material type is set to steel, and the value of gravitational acceleration is set to 9.80665. From these, the inertial parameters of the robot are generated, as shown in

Table 1. Physical parameters of robot.

$\mathbf{Rigid} \mathbf{Body} {\mathit{B}}_{\mathit{i}}$	${\mathit{B}}_1$	${\mathit{B}}_2$	${\mathit{B}}_3$	${\mathit{B}}_4$	${\mathit{B}}_5$	${\mathit{B}}_6$
$\mathrm{Length} l_i$$(m$)	1.045	1.245	1.21	0.315	0.29	0.3
$\mathrm{Radius} R_{ i}$$(m$)	0.5	0.3	0.2	0.2	0.2	0.3
$m_i$$(kg$)	6402.6012	2746.0726	1186.1661	308.7953	284.2877	661.7042
$J_{i_{11}}^{(i)}$$(kg\cdot m^2$)	800.3251	123.5733	23.7233	6.1759	5.6858	29.7767
$J_{i_{22}}^{(i)}$$(kg\cdot m^2$)	982.8126	416.4934	156.5838	5.6413	4.8353	19.8511
$J_{i_{33}}^{(i)}$$(kg\cdot m^2$)	982.8126	416.4934	156.5838	5.6413	4.8353	19.8511

.

The purpose of the experiment is to solve the dynamic parameters and Lagrange multipliers of the robot. This means that the dynamic parameters and Lagrange multipliers of each rigid body $B_i$ are regarded as unknown parameters to be solved, and the other 6 parameters of each rigid body $B_i$ are regarded as known parameters, that is, the measured parameters, which are: (1) quaternion coordinates relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$; (2) the force element exerted on $B_i$ by $O_i$ relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$; (3) the active moment exerted on $B_i$ by $O_i$ relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$; (4) the centroid acceleration relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$; (5) the angular velocity relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$; (6) the angular acceleration relative to $(O_0,{\underset{¯}{\mathit{e}}}^{(0)})$.

In order to simplify the experiment and reduce the computational difficulty, the ideal experiment is to solve the dynamic parameters and Lagrange multipliers of the robot with only one movement, which means that the 6 rigid bodies that make up the robot can generate the 6 known parameters in this movement. Since the rigid bodies are connected to each other by hinges, the movement of the inner rigid body will drive the outer rigid body to move together. If the inner rigid body generates the 6 known parameters during the motion, the outer rigid body will generate the 6 known parameters at the same time. $B_1$ is connected to the ground and can only generate the angular acceleration in the ${\mathit{e}}_3^{(0)}$ direction, but cannot generate the angular acceleration in the ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)}$ direction, so $B_2$ needs to move at the same time to generate the angular acceleration in the ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)}$ direction. $B_2$ can generate the 6 known parameters in this movement, and $B_3,B_4,B_5,B_6$ also generates the 6 known parameters at the same time, so $B_3,B_4,B_5,B_6$ does not need to move relative to the inner rigid body.

Simulation settings: The initial state is static, and the posture is shown in Figure 1. Both hinge $O_1$ and hinge $O_2$ rotate at the angular acceleration of $4.7746 rad/s^2$ (namely $15°/s^2$). The direction of the axis of rotation of hinge $O_1$ is ${\mathit{e}}_3^{(0)}$, and the direction of the axis of rotation of hinge $O_2$ is ${\mathit{e}}_2^{(1)}$, and other hinges are locked so that they do not rotate. The simulation termination time is 2 s, and the change of the robot’s posture during the movement is shown in Figure 2. The trajectories of the 6 measured parameters of each rigid body over time are shown in Appendix A.

Second, the data of the motion process is sampled, and then the unknown parameters are solved.

The 1.2 s time (or any other time) is selected to sample the data of the quaternion, force element ${\mathit{F}}_i^e$, active moment ${\mathit{M}}_i^a$, angular velocity, angular acceleration, and centroid acceleration of each rigid body. The sampled data is introduced into Equation (66) to calculate the results of unknown parameters. The calculation results are shown in

Table 2. The solved inertial parameters.

$\mathbf{Rigid} \mathbf{Body} {\mathit{B}}_{\mathit{i}}$	${\mathit{B}}_1$	${\mathit{B}}_2$	${\mathit{B}}_3$	${\mathit{B}}_4$	${\mathit{B}}_5$	${\mathit{B}}_6$
$m_i$$(e_1^{(0)}$$) (kg$)	—	2746.0726	1186.1658	308.7950	284.2880	661.7042
$m_i$$(e_2^{(0)}$$) (kg$)	—	2746.0685	1186.1663	308.7954	284.2878	661.7042
$m_i$$(e_3^{(0)}$$) (kg$)	6402.5982	2746.0727	1186.1656	308.7955	284.2878	661.7042
$J_{i_{11}}^{(i)}$$(kg\cdot m^2$)	800.3261	123.5681	23.7140	6.1793	5.6847	29.7768
$J_{i_{22}}^{(i)}$$(kg\cdot m^2$)	982.8122	416.4211	156.5920	5.6417	4.8364	19.8510
$J_{i_{33}}^{(i)}$$(kg\cdot m^2$)	982.8122	416.5364	156.5840	5.6406	4.8353	19.8511

and

Table 3. The solved Lagrange multipliers.

$\mathbf{Lagrange} \mathbf{Multipliers} {\underset{¯}{\mathit{\lambda }}}_{\mathit{i}}^{\ast }$	${\underset{¯}{\mathit{\lambda }}}_1^{\ast }$	${\underset{¯}{\mathit{\lambda }}}_2^{\ast }$	${\underset{¯}{\mathit{\lambda }}}_3^{\ast }$	${\underset{¯}{\mathit{\lambda }}}_4^{\ast }$	${\underset{¯}{\mathit{\lambda }}}_5^{\ast }$	${\underset{¯}{\mathit{\lambda }}}_6^{\ast }$
${\lambda }_{i1}^{\ast }$	$1.5166\times {10}^{-6}$	$1.5166\times {10}^{-6}$	$-4.1854\times {10}^{-7}$	$-1.6008\times {10}^{-5}$	$3.8787\times {10}^{-5}$	$-2.6473\times {10}^{-5}$
${\lambda }_{i2}^{\ast }$	$5.7217\times {10}^{-4}$	$5.7217\times {10}^{-4}$	$-1.3463\times {10}^{-4}$	$-5.8943\times {10}^{-5}$	$-1.8120\times {10}^{-5}$	$1.3748\times {10}^{-6}$
${\lambda }_{i3}^{\ast }$	$-1.1366\times {10}^5$	$-5.0867\times {10}^4$	$-2.3938\times {10}^4$	$-1.2305\times {10}^4$	$-9.2770\times {10}^3$	$-6.4891\times {10}^3$
${\lambda }_{i4}^{\ast }$	$4.6041\times {10}^5$	$7.7488\times {10}^{13}$	$7.5058\times {10}^{13}$	$-3.4288\times {10}^4$	$8.5585\times {10}^{12}$	$-5.0122\times {10}^3$
${\lambda }_{i5}^{\ast }$	$-4.8692\times {10}^6$	$-4.0621\times {10}^{14}$	$-3.9347\times {10}^{14}$	$-6.5407\times {10}^3$	$-4.4865\times {10}^{13}$	$-9.5612\times {10}^2$
${\lambda }_{i6}^{\ast }$	$8.1974\times {10}^{14}$	$-2.5700\times {10}^{13}$	$-7.9403\times {10}^{14}$	$9.0539\times {10}^{13}$	$-9.0540\times {10}^{13}$	$-\mathrm{1,0207}\times {10}^4$

.

$m_i$($e_1^{(0)}$), $m_i$($e_2^{(0)}$), $m_i$($e_3^{(0)}$) are the calculated masses of $B_i$ in the $e_1^{(0)}$, $e_2^{(0)}$,$e_3^{(0)}$ directions, respectively. Since $B_1$ is fixed to the ground, no acceleration occurs in the $e_1^{(0)}, e_2^{(0)}$ direction, so the mass cannot be obtained in these directions.

Finally, the calculated results are analyzed.

Comparing the true values of inertial parameters in Table 1 with the calculated values of inertial parameters in Table 2, the maximum absolute error of the two is −0.0723 $kg\cdot m^2$. The error comes from the rounding of the significant figures of the data, and the increase of the number of calculations will lead to the accumulation of errors. The error cannot be eliminated, so the very small error verifies the correctness and accuracy of the dynamic model.

The Lagrange multipliers in Table 3 have clear physical meanings, where ${\lambda }_{i1}^{\ast },{\lambda }_{i2}^{\ast },{\lambda }_{i3}^{\ast }$ are proportional to the ideal constraining force of $B_i$ in the ${\mathit{e}}_1^{(0)},{\mathit{e}}_2^{(0)},{\mathit{e}}_3^{(0)}$ direction respectively; ${\lambda }_{i4}^{\ast },{\lambda }_{i5}^{\ast }$ that introduced by the constraint Equation (25) are proportional to the ideal constraining moments; while ${\lambda }_{i6}^{\ast }$ introduced by the quaternion self-constraint Equation (15) does not reflect the ideal constraining moment.

The posture of the robot at 1.2 s is shown in Figure 2e. Since $B_i\left(i,=1,2,\cdots ,6\right)$ is affected by gravity, and the inner rigid body bears more gravity than the outer rigid body, therefore ${\lambda }_{i3}^{\ast }\left(i=1,2,\cdots ,6\right)$ is much larger than ${\lambda }_{i1}^{\ast },{\lambda }_{i2}^{\ast }\left(i=1,2,\cdots ,6\right)$ and ${\lambda }_{i3}^{\ast }\left(i=1,2,\cdots ,6\right)$ decreases with the increase of serial number $i$.

The rotation axes of $O_2,O_3,O_5$ are all in the ${\mathit{e}}_2^{(i)}$ direction and need to bear the torque generated by gravity; the rotation axes of $O_1,O_2,O_6$ are not in the ${\mathit{e}}_2^{(i)}$ direction and do not need to bear the torque generated by gravity, so the ideal constraining moment of $O_2,O_3,O_5$ is large and the ideal constraining moment of $O_1,O_2,O_6$ is small. Therefore, the value of ${\lambda }_{i4}^{\ast },{\lambda }_{i5}^{\ast }\left(i=2,3,5\right)$ is much larger than that of ${\lambda }_{i4}^{\ast },{\lambda }_{i5}^{\ast }\left(i=1,4,6\right)$. The number of outer rigid bodies of $O_2,O_3,O_5$ decreases, and the torque they received also decreases, so ${\lambda }_{i4}^{\ast },{\lambda }_{i5}^{\ast }\left(i=2,3,5\right)$ decreases with the increase of serial number $i$. In the same way, the number of outer rigid bodies of $O_1,O_4,O_6$ decreases, and the torque they received also decreases, so ${\lambda }_{i4}^{\ast },{\lambda }_{i5}^{\ast }\left(i=1,4,6\right)$ decreases with the increase of serial number $i$.

The data show that each rotating hinge of the robot receives great ideal constraining force in the vertical direction, while it receives very little ideal constraining force in the horizontal direction. When the direction of the hinge’s rotation axis is the same as the direction of the moment of the outer rigid body to the hinge, the hinge is subjected to a large ideal constraining moment; when the two directions are perpendicular, the ideal constraining moment of the hinge is small. The calculated values of Lagrange multipliers conform to objective physical laws, which also verifies the correctness of the dynamic model.

7. Conclusions

The dynamics of 6-DOF robot are systematically studied in this paper, and a dynamic modeling method of multibody system based on screw theory is proposed.

(1)

The dynamic and kinematic parameters of each rigid body can be decoupled by this method, and the dynamic model is concisely and compactly expressed as a modular matrix equation, which is convenient for both the solution of the dynamic model and the control.

(2)

Quaternions are used as generalized angular coordinates in this method, which eliminates singularities and makes the dynamic model suitable for motion in a large angular range. Additionally, the computational efficiency is improved.

(3)

The dynamic model established by this method is correct and accurate, which is verified by the numerical example. This method can provide a theoretical basis for the research on the dynamics of 6-DOF robot and its control method.

The future work is to carry out the research on related control methods.