# The Kinematic and Dynamic Modeling and Numerical Calculation of Robots with Complex Mechanisms Based on Lie Group Theory

Authors: Lu-Han Ma; Yong-Bo Zhong; Gong-Dong Wang; Nan Li
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

The kinematic and dynamic models of robots with complex mechanisms such as the closed-chain mechanism and the branch mechanism are often very complex and difficult to be calculated. Aiming at this issue, in this paper, the pose of the component in robots is represented by the Euclidean group and its subgroups with the proposed method. The component’s velocity is derived using the relationship between the Lie group and Lie algebra, and the acceleration and Jacobian matrix are then derived on this basis. The Lagrange equation is expressed by the obtained kinematic parameter expressions. Establishing the model with this method can obtain clear physical meaning and make the expressions uniform and easy to program, which is convenient for computer-aided calculation and parameterization. Calculating by the properties of the Lie group can reduce the calculation and model complexity, especially for calculating the velocity and acceleration, which reduces the calculation error and eases the calculation. Therefore, the proposed modeling and calculation method of kinematics and dynamics of robots is especially suitable for robots with complex mechanisms. As an example, the kinematic and dynamic model of the manipulator developed in our laboratory is established and a working process of it is numerically calculated. Then, the results of the numerical calculation are compared with the results of virtual prototype simulation in ADAMS to verify the correctness.


---

## Body

## 1. Introduction

Industrial robots are usually composed of the base, the end-effector, and several links connected by joints. Robot kinematic model establishes the mapping relation between the pose of the end-effector and the angles or displacements of joints. Robot dynamic model establishes the mapping relation between driving forces needed on joints and the angles or displacements of joints. The establishment of robot kinematic and dynamic models is the basis of robot kinematic analysis, path planning, dynamic analysis, motion control, and so on [1]. Robots are mainly divided into three types: serial robot, parallel robot, and hybrid robot [2]. Compared with the serial robot, the parallel robot has higher stiffness, bearing capacity, and precision, but its workspace is limited [3, 4]. The hybrid robot combines the advantages of the serial robot and the parallel robot [2–4]. Parallel robots and hybrid robots often have complex structures and include complex mechanisms such as the closed-chain mechanism and the branch mechanism. Motions of components in the complex mechanisms often have coupling relationships. So, the expressions of the component’s pose, velocity, and acceleration in the complex mechanisms can become extremely complicated. Therefore, it greatly increases the difficulty of modeling and calculating the kinematic and dynamic models of their mechanisms.The methods to establish kinematic model of robots mainly include vector method, quaternion method, screw theory method, homogeneous matrix method, and so on [5]. Among them, the homogeneous matrix method and the screw theory method are applied widely. But when the mobile robot is analyzed, the screw theory method cannot be directly used and the virtual link technology is needed for a transformation [6]. Currently, the Denavit–Hartenberg method based on the homogeneous matrix is the most widely utilized method for its clear physical meaning and has been programmed [5]. However, the process of establishing kinematic model of robots with complex mechanisms using the Denavit–Hartenberg method is complicated and the solution is complex, especially for the solution of velocity and acceleration.Lagrange method and Newton Euler method are two main methods to establish the dynamic model of the robot [1]. Newton Euler method extends the Newton Euler equation of a single body to multi-body system. It has clear physical meaning and can establish equations directly. Its disadvantage is that eliminating the internal force of the system is very difficult, so it will be very hard to solve the dynamic model of the robot with complex mechanisms. Lagrange method is based on the energy conservation of the mechanical system, which can avoid the calculation of complex internal forces of the object system. So, the Lagrange method is often used for dynamic modeling of complex systems. However, with the increase of the complexity of the system, it will become very difficult to express and differentiate equations of the kinetic and potential energy. With the development of computer technology, the derivation of Lagrange equation with the aid of computer has been widely used.As for the robot whose base, links, and end effector can be regarded as the rigid body, the establishment of the kinematic and dynamic models of its mechanism is essentially to establish the kinematic and dynamic models of multi rigid body system. In recent years, the theory of the Lie group and Lie algebra is more and more applied to kinematics and dynamics analysis of the spatial multi rigid body, and great achievements have been made. Liu [7] presented a method which combined the Lie group and Kane’s equations. With this approach, all the individual quantities involved in Kane’s equations can be expressed with the Lie group and Lie algebra, and in clear physical meanings. It is convenient for implementing calculation by using a symbolic computation package. Based on the Lie group theory, Bai et al. [8] carried out the kinematic and dynamic analysis of the pneumatic hexapod robot and used the pseudospectral method to carry out the optimal control analysis. Ma et al. [5] improved the Denavit–Hartenberg method based on the Lie group theory. The improved method can meet the needs of subsequent calibration and error analysis, and is more suitable for kinematic modeling of robots with complex mechanisms. Lee et al. [9] combined the theory of the Lie group with the discrete variational integrator, and obtained the Lie group discrete variational integrator. Based on the screw theory and Lie group theory, Müller [10] obtained a compact form of the kinematic equations of the tree topology multi-body system represented by adjacent (joint) coordinates under the unified multi-body system framework. Gallardo-Alvarado [2] based on the screw theory and Lie group theory proposed a kinematic analysis method for hybrid robots in closed-form solution. This analysis avoids the use of numerical calculation methods, such as Newton Simpson method, and obtains the simplified acceleration expression of screw form which it is convenient for speed and acceleration analysis. Dai [11] reviewed the Euler–Rodrigues formula representing the rotation motion of the rigid body. Its variations and derivations in the form of vector, quaternion, and Lie group are studied, and the three forms’ intrinsic connections are researched. Based on the Lie group and screw theory, Müller [12] proposed a simple recursive formula of arbitrary derivative of closed-chain constraint with respect to time. The basic operation used is Lie bracket. This method provides a basis for determining the mobility and singularity analysis of general linkage mechanism. Park et al. [13] based on the Lie group theory and Riemannian geometry derived the motion formula for open chain manipulator. Then, the Newton–Euler equation and the Lagrange equation are derived to be computationally effective and easily differentiated.It is known that the displacement subgroups in the Lie group correspond to the homogeneous transformation matrix describing the motion of the rigid body. The rotation part of the homogeneous transformation matrix belongs to the three-dimensional rotation group in the Lie group, the position vector in the homogeneous transformation matrix belongs to the three-dimensional translation group in the Lie group, and the homogeneous transformation matrix belongs to Euclidean group in the Lie group. The Lie algebras corresponding to these three groups represent the rotation velocity of the rigid body, the linear velocity of a point on the rigid body, and the combination of the two velocities, respectively. There is a mapping relationship between the Lie group and Lie algebra. Therefore, the motion of rigid body can be described in the form of Lie group and Lie algebra, and the motion parameters of rigid body can be calculated by using the properties of the Lie group and Lie algebra. Then, the kinematic parameters derived under the framework of the Lie group are adopted to express the Lagrange equation and the advantage of the Lie group frame in kinematics is extended to dynamics. Establishing and calculating in the form of the Lie group and the Lie algebra and by their properties can avoid a large number of trigonometric functions in the middle calculation process. In particular, using the relationship between Lie groups and Lie algebra will avoid a large number of derivations of trigonometric functions and greatly simplify the solution of the component’s velocity and then the solutions of the acceleration and the Lagrange equation are also simplified.In this paper, firstly, the theory of the Lie group is connected with the motion of the rigid body and the description formulas of rigid body motion under the framework of the Lie group are derived. Then, we propose a kinematic modeling method to describe the pose of rigid bodies in the multi rigid body system with the Lie group. The pose of the component in the robot mechanism is expressed by the Euclidean group and the displacement subgroup. The component’s body velocity Lie algebra and the velocities in the body fixed system derived from it are adopted to express the Lagrange equation in the framework of the Lie group. The space velocity Lie algebra is derived from the body velocity. The component’s velocity in the inertial system are derived using the space velocity Lie algebra, and the corresponding acceleration formula and the robot’s Jacobian matrix are derived on this basis. Establishing the model in the form of Lie group and Lie algebra can obtain clear physical meaning and make the expressions uniform and easy to program, which is convenient for computer-aided calculation and parameterization. Calculating in the form of the Lie group and Lie algebra and by their properties can avoid a large number of trigonometric functions in the middle calculation process, which reduce the calculation and model complexity, especially for calculating the velocity and acceleration. And then, the calculation error is decreased and the calculation difficulty is reduced. Therefore, the proposed modeling and calculation method of kinematics and dynamics of robots is especially suitable for robots with complex mechanisms. Finally, as an example, the kinematic and dynamic model of the manipulator developed in our laboratory is established and a working process of it is numerically calculated by the proposed modeling method in this paper. The result of the numerical calculation in MATLAB is compared with that of virtual prototype simulation in ADAMS. It is found that the two results are basically consistent, so the established mathematical model is correct and the numerical calculation method is feasible. The numerical calculation results provide a theoretical basis for the subsequent motion analysis of the manipulator.This paper is organized as follows. In Section2, the description formulas of the rigid body motion under the framework of the Lie group are derived, which is the theoretical basis of this paper. In Section 3, the kinematic modeling method of robot mechanism under the Lie group framework is introduced, and the recursive formulas of component’s velocity and acceleration and the robot’s Jacobian matrix expression are derived. In Section 4, the Lagrange equation under the framework of the Lie group theory is derived. In Section 5, the kinematic model of the developed manipulator is established. In Section 6, the dynamic model of the developed manipulator is established. In Section 7, the kinematic and dynamic models are solved, and the kinematic and dynamic analysis of the developed manipulator is carried out based on the numerical results. Section 8 is the conclusion of this paper.


## 2. The Description of Rigid Body Motion in the Framework of the Lie Group

### 2.1. The Basic Theory of the Lie Group and Lie Algebra

Lie groupG is a differential manifold. Let g∈G, then there is the following [14]:(1)
The binary operation mapping of the Lie groupφ:G×G⟶G, φg1,g2=g1∘g2, g1 and g2 are all C∞.,.(2)
The inverse operation mapping of the Lie groupτ:G⟶G, τg=g−1 and g are both C∞.(3)
The Lie group satisfies law of associationg1∘g2g3=g1g2∘g3.(4)
The Lie group satisfies identity element law. There is a unique identity elemente which makes g∘e=e∘g=g.As for∀g∈G, set up TgG as the tangent space at g. The Lie algebra is defined as the tangent space at the identity element, that is, TeG.The Lie algebra is isomorphic toRk through the invertible linear mapping. There is(1)•∧:Rk⟶TeG,V⟼V^.The left invariant translation mapping of the Lie group at the identity element is(2)lg:G⟶G,e⟼g∘e.Then, the derivative of this mapping defines the diffeomorphism fromTeG to TgG. There is(3)Dlg:TeG⟶TgG,V^⟼Dlg⋅V^.The derivative of the right invariant translation mapping of the Lie group at the identity element is(4)Drg:TeG⟶TgG,V^⟼V^⋅Drg.The Lie groupG acting on its Lie algebra TeG is called the adjoint transformation of Lie algebra, if equation (5) is satisfied.(5)AdgV=gVg−1,V∈TeG,g∈G.


### 2.2. The Rotating Motion of Rigid Body

The rotating motion of rigid body in the 3D space is described by the rotation matrixR. R∈SO3 and SO3=R∈R3×3:RRT=RTR=E,detR=1 belongs to the 3D rotation group. Its group operation is matrix multiplication, that is, R1∘R2=R1R2.so3 is the corresponding Lie algebra of SO3. Its concrete form is antisymmetric matrix space ω^∈so3=s∈R3×3:sT=−s. And, it is recorded as ω^=0−ω3ω2ω30−ω1−ω2ω10, where ω1 is the angular velocity component in the x-axis, ω2 is the angular velocity component in the y-axis, and ω3 is the angular velocity component in the z-axis.The antisymmetric matrix has the propertyω^r=ω×r. Then, there is ω×=ω^ and r is a 3-dimensional column vector.According to (1), define(6)•∨:ω^⟼ω=ω1,ω2,ω3T.The adjoint transformation of the Lie algebraso3 is(7)Adgω^=Rω^R−1=0r1⋅ω×r2r1⋅ω×r3r2⋅ω×r10r2⋅ω×r3r3⋅ω×r1r3⋅ω×r20=Rω^=ω^′,where R=r1r2r3.According to (3), we obtain(8)R˙=Rω^B,whereω^B is the angular velocity of the rigid body in the body fixed coordinate system. According to (4), we obtain(9)R˙=ω^SR,where ω^S is the angular velocity of the rigid body in the inertial coordinate system.The relationship between the two angular velocities is(10)ω^S=Adgω^B=Rω^B.As for elements inso3, the adjoint transformation can describe the velocity transformation under the coordinate system transformation.There is a one to one exponential mapping relationship betweenSO3 and so3. The exponential mapping relationship is(11)R=expω^θ=E+θω^+θ22ω^2+θ33ω^3+⋯=E+ω^sinθ+ω^21−cosθ.


### 2.3. The Translational Motion of Rigid Body

The translational motion of rigid body in the 3D space is described by the translation matrixp.p∈T3 and T3=p∈R3 belongs to the 3D translation group. Its group operation is matrix addition, that is, p1∘p2=p1+p2. The translation velocity vector is p˙.


### 2.4. The General Motion of Rigid Body

The general motion of rigid body in the 3D space is described by the affine mappinggr=Rr+p. All of these affine mappings form a six-dimensional Lie group, which is called the Euclidean group SE3. The Euclidean group SE3 is the semidirect product of the 3D rotation group SO3 and the 3D translation group T3, that is, SE3=SO3×T3 which is abbreviated and recorded as R,p. Its homogeneous matrix form is formula (12). The group operation is matrix multiplication, that is, g1∘g2=g1g2.(12)R,p⟶Rp01=g.se3 is the corresponding Lie algebra of SE3, and its concrete form is the screw space ξ^∈se3=ω^,v:ω^∈so3,v∈R3. It is written in the form of matrix as(13)ξ^=ω^RTp˙00=ω^v00,ω,v∈R3.The adjoint transformation of the Lie algebrase3 is(14)Adgξ^=gξ^g−1=Rω^Rv+pRω00=ω^′v′00.It is written in the form of six-dimensional vector as(15)ω′v′=R0p^RRωv.According to (3), we obtain(16)g˙=gξ^B.ξ^B expresses the angular velocity of the rigid body and the linear velocity of the origin of the body fixed coordinate system under the body fixed coordinate system.According to (4), we obtain(17)g˙=ξ^Sg.ξ^S expresses the angular velocity of the rigid body and the linear velocity of the point which is on the body and coincide with the origin of the inertial system under the inertial coordinate system.The relationship betweenξ^B and ξ^S is expressed in the following equations:(18)ξ^S=Adgξ^B=Rω^Rv+pRω00,(19)ξS=R0p^RRξB.As for elements inse3, the adjoint transformation can describe the velocity transformation under the coordinate system transformation.There is a one to one exponential mapping relationship betweenSE3 and se3. The exponential mapping relationship is(20)g=expξ^θ=expω^θE−expω^θω×v+ωωTvθ01.SO3 and T3 are the Lie subgroups of SE3. SE3 and its Lie subgroups are related to the displacement of rigid body and they are called the displacement subgroup.


## 2.1. The Basic Theory of the Lie Group and Lie Algebra

Lie groupG is a differential manifold. Let g∈G, then there is the following [14]:(1)
The binary operation mapping of the Lie groupφ:G×G⟶G, φg1,g2=g1∘g2, g1 and g2 are all C∞.,.(2)
The inverse operation mapping of the Lie groupτ:G⟶G, τg=g−1 and g are both C∞.(3)
The Lie group satisfies law of associationg1∘g2g3=g1g2∘g3.(4)
The Lie group satisfies identity element law. There is a unique identity elemente which makes g∘e=e∘g=g.As for∀g∈G, set up TgG as the tangent space at g. The Lie algebra is defined as the tangent space at the identity element, that is, TeG.The Lie algebra is isomorphic toRk through the invertible linear mapping. There is(1)•∧:Rk⟶TeG,V⟼V^.The left invariant translation mapping of the Lie group at the identity element is(2)lg:G⟶G,e⟼g∘e.Then, the derivative of this mapping defines the diffeomorphism fromTeG to TgG. There is(3)Dlg:TeG⟶TgG,V^⟼Dlg⋅V^.The derivative of the right invariant translation mapping of the Lie group at the identity element is(4)Drg:TeG⟶TgG,V^⟼V^⋅Drg.The Lie groupG acting on its Lie algebra TeG is called the adjoint transformation of Lie algebra, if equation (5) is satisfied.(5)AdgV=gVg−1,V∈TeG,g∈G.


## 2.2. The Rotating Motion of Rigid Body

The rotating motion of rigid body in the 3D space is described by the rotation matrixR. R∈SO3 and SO3=R∈R3×3:RRT=RTR=E,detR=1 belongs to the 3D rotation group. Its group operation is matrix multiplication, that is, R1∘R2=R1R2.so3 is the corresponding Lie algebra of SO3. Its concrete form is antisymmetric matrix space ω^∈so3=s∈R3×3:sT=−s. And, it is recorded as ω^=0−ω3ω2ω30−ω1−ω2ω10, where ω1 is the angular velocity component in the x-axis, ω2 is the angular velocity component in the y-axis, and ω3 is the angular velocity component in the z-axis.The antisymmetric matrix has the propertyω^r=ω×r. Then, there is ω×=ω^ and r is a 3-dimensional column vector.According to (1), define(6)•∨:ω^⟼ω=ω1,ω2,ω3T.The adjoint transformation of the Lie algebraso3 is(7)Adgω^=Rω^R−1=0r1⋅ω×r2r1⋅ω×r3r2⋅ω×r10r2⋅ω×r3r3⋅ω×r1r3⋅ω×r20=Rω^=ω^′,where R=r1r2r3.According to (3), we obtain(8)R˙=Rω^B,whereω^B is the angular velocity of the rigid body in the body fixed coordinate system. According to (4), we obtain(9)R˙=ω^SR,where ω^S is the angular velocity of the rigid body in the inertial coordinate system.The relationship between the two angular velocities is(10)ω^S=Adgω^B=Rω^B.As for elements inso3, the adjoint transformation can describe the velocity transformation under the coordinate system transformation.There is a one to one exponential mapping relationship betweenSO3 and so3. The exponential mapping relationship is(11)R=expω^θ=E+θω^+θ22ω^2+θ33ω^3+⋯=E+ω^sinθ+ω^21−cosθ.


## 2.3. The Translational Motion of Rigid Body

The translational motion of rigid body in the 3D space is described by the translation matrixp.p∈T3 and T3=p∈R3 belongs to the 3D translation group. Its group operation is matrix addition, that is, p1∘p2=p1+p2. The translation velocity vector is p˙.


## 2.4. The General Motion of Rigid Body

The general motion of rigid body in the 3D space is described by the affine mappinggr=Rr+p. All of these affine mappings form a six-dimensional Lie group, which is called the Euclidean group SE3. The Euclidean group SE3 is the semidirect product of the 3D rotation group SO3 and the 3D translation group T3, that is, SE3=SO3×T3 which is abbreviated and recorded as R,p. Its homogeneous matrix form is formula (12). The group operation is matrix multiplication, that is, g1∘g2=g1g2.(12)R,p⟶Rp01=g.se3 is the corresponding Lie algebra of SE3, and its concrete form is the screw space ξ^∈se3=ω^,v:ω^∈so3,v∈R3. It is written in the form of matrix as(13)ξ^=ω^RTp˙00=ω^v00,ω,v∈R3.The adjoint transformation of the Lie algebrase3 is(14)Adgξ^=gξ^g−1=Rω^Rv+pRω00=ω^′v′00.It is written in the form of six-dimensional vector as(15)ω′v′=R0p^RRωv.According to (3), we obtain(16)g˙=gξ^B.ξ^B expresses the angular velocity of the rigid body and the linear velocity of the origin of the body fixed coordinate system under the body fixed coordinate system.According to (4), we obtain(17)g˙=ξ^Sg.ξ^S expresses the angular velocity of the rigid body and the linear velocity of the point which is on the body and coincide with the origin of the inertial system under the inertial coordinate system.The relationship betweenξ^B and ξ^S is expressed in the following equations:(18)ξ^S=Adgξ^B=Rω^Rv+pRω00,(19)ξS=R0p^RRξB.As for elements inse3, the adjoint transformation can describe the velocity transformation under the coordinate system transformation.There is a one to one exponential mapping relationship betweenSE3 and se3. The exponential mapping relationship is(20)g=expξ^θ=expω^θE−expω^θω×v+ωωTvθ01.SO3 and T3 are the Lie subgroups of SE3. SE3 and its Lie subgroups are related to the displacement of rigid body and they are called the displacement subgroup.


## 3. The Kinematic Modeling Method and Kinematic Parameters’ Derivation of Robot Mechanism in the Lie Group Framework

### 3.1. The Kinematic Modeling Method of Robot Mechanism

Referring to the Denavit–Hartenberg model proposed by Craig and Khalil and considering the Lie group theory, we propose a new kinematic modeling method as follows. The two moving transformations in the Denavit–Hartenberg model are changed into three while the angle transformations in it are unchanged, and the transformation order is adjusted to ease the Lie group expression and calculation. The joint coordinate system is embedded in the corresponding rigid body and the pose of the rigid body is expressed by the pose of the body fixed coordinate system. Thus, the pose of the component can be represented by a translation group and two rotation groups. The proposed kinematic modeling method of robot mechanism in the Lie group framework consists of the following. The defined numbers of the components, joints, and joint systems are shown in Figure1.(1)
Label robot’s components sequentially from the base that was marked as 0 to the end-effector that was marked asn. Label robot’s joints sequentially from 1 to n. So, joint i connects component i − 1 and component i.(2)
Define the joint coordinate system. Arrange the origin of the joint coordinate system at any required position on the corresponding joint axis. Make thez axis of the joint coordinate system coincide with the joint axis and the x axis parallel to the common normal line of the joint axis and the next joint axis. Then, the joint coordinate system i is fixed to the component i.(3)
Transformation steps between the adjacent joint coordinate systems are as follows:(A)
The coordinate systemi − 1 is translated along xi−1,yi−1 and zi−1 directions, respectively, to make the origin of it coincide with that of the coordinate system i. The coordinate system 1 is obtained.(B)
The coordinate system 1 is rotated around the axisx1 to make the axis z1coincide with the joint axis. The coordinate system 2 is obtained.(C)
The coordinate system 2 is rotated around the axisz2 to make the axis x1 coincide with the axis x1. The coordinate system i is obtained.Figure 1
The notation and joint coordinate system defined by the proposed modeling method.The transformation described above is expressed in the form of Lie group as(21)gii−1=Transxi−1,aTransyi−1,bTranszi−1,cRotx1,αRotz2,θ=RxαRzθpxa+pyb+pzc01=gii−10eξ^iiθi.The right subscript of the Lie algebraξ^ii represents the component number, and the left superscript of it represents the coordinate system number that describes the motion. gii−1 contains the initial pose gii−10 and rotation angle θi of the component.The pose of the componentn relative to the inertial coordinate system is expressed as(22)gn0=g10g21,…,gnn−1=Rn0Pn001.


### 3.2. Derivation of Kinematic Parameter Formula of Robot Mechanism in Lie Group Framework

Set up that, the position vector is expressed asrip, the left superscript of the position vector represents the coordinate system that describes the vector, the right subscript of the position vector represents the target point, and the homogeneous coordinate of the position vector is expressed as r¯ip.The body velocity of component 1 relative to the inertial coordinate system is(23)V^10B=g10−1g˙10=R10TR˙10R10TP˙1000=ω^10BV10B00=θ˙1ξ^11.The body velocity of componenti relative to the coordinate system i − 1 is(24)V^ii−1B=gii−1−1g˙ii−1=θ˙iξ^ii.The body velocity of componenti relative to the inertial coordinate system is(25)V^i0B=gi−10gii−10eξ^iiθi−1ddtgi−10gii−10eξ^iiθi=Adgii−1−1V^i−10B+V^ii−1B=Ri0TR˙i0Ri0TP˙i000=ω^i0BVi0B00.Equations (23)–(25) are body velocity’s recursive formulas of robot mechanism in the Lie group framework. The body velocity is used to derive the space velocity and the dynamic equations. The space velocity is used to derive the angular velocity of each component and the linear velocity of the target point on components in the inertial coordinate system.The space velocity of componenti relative to the inertial coordinate system is(26)V^i0S=Adgi0V^i0B=R˙i0Ri0T−R˙i0Ri0TPi0+P˙i000=ω^i0SVi0S00.Then, the rotational angular velocity of componenti in the inertial coordinate system is(27)ωi0=ωi0xSωi0ySωi0zS=00100000−1000V^i0S1110+100010001000V^i0S0010.Then, the rotational angular acceleration of componenti in the inertial coordinate system is(28)αi0=ω˙i0.The linear velocity of any pointp on component i in the inertial coordinate system is(29)v0p=vi0S+ω^i0Sr0p=I3×303×1v^i0Sr¯0p.The acceleration of any pointp on component i in the inertial coordinate system is(30)a0p=v˙0p.The Jacobian matrix formula of robot mechanism is derived as follows.Let the rotation velocity of each joint of the robot beθ˙=θ˙1,θ˙2,…,θ˙nT and the position of the reference point on the robot end-effector in the inertial coordinate system be r0=x0,y0,z0T.The linear velocity of the reference point on the end-effector in the inertial coordinate system isv0, and it is expressed as(31)v0=ddtr0=∑i=1n∂r0∂θiθ˙i=Jvθ˙.So, the displacement part of Jacobian matrix of the robot is(32)Jv=∂x0∂θ1∂x0∂θ2⋯∂x0∂θn∂y0∂θ1∂y0∂θ2⋯∂y0∂θn∂z0∂θ1∂z0∂θ2⋯∂z0∂θn.The angular velocity Lie algebra of the reference point on the end-effector in the inertial coordinate system isω^0, and it is expressed as(33)ω^0=ω^S=R˙R−1=∑i=1n∂R∂θiθ˙iR−1=∑i=1n∂R∂θiR−1θ˙i.Then, the angular velocity of the reference point on the end-effector in the inertial coordinate system is(34)ω0=∑i=1n∂R∂θiR−1∨θ˙i=Jωθ˙.So, the rotation part of Jacobian matrix of the robot is(35)Jω=∂R∂θ1R−1∨,∂R∂θ2R−1∨,…,∂R∂θnR−1∨.


## 3.1. The Kinematic Modeling Method of Robot Mechanism

Referring to the Denavit–Hartenberg model proposed by Craig and Khalil and considering the Lie group theory, we propose a new kinematic modeling method as follows. The two moving transformations in the Denavit–Hartenberg model are changed into three while the angle transformations in it are unchanged, and the transformation order is adjusted to ease the Lie group expression and calculation. The joint coordinate system is embedded in the corresponding rigid body and the pose of the rigid body is expressed by the pose of the body fixed coordinate system. Thus, the pose of the component can be represented by a translation group and two rotation groups. The proposed kinematic modeling method of robot mechanism in the Lie group framework consists of the following. The defined numbers of the components, joints, and joint systems are shown in Figure1.(1)
Label robot’s components sequentially from the base that was marked as 0 to the end-effector that was marked asn. Label robot’s joints sequentially from 1 to n. So, joint i connects component i − 1 and component i.(2)
Define the joint coordinate system. Arrange the origin of the joint coordinate system at any required position on the corresponding joint axis. Make thez axis of the joint coordinate system coincide with the joint axis and the x axis parallel to the common normal line of the joint axis and the next joint axis. Then, the joint coordinate system i is fixed to the component i.(3)
Transformation steps between the adjacent joint coordinate systems are as follows:(A)
The coordinate systemi − 1 is translated along xi−1,yi−1 and zi−1 directions, respectively, to make the origin of it coincide with that of the coordinate system i. The coordinate system 1 is obtained.(B)
The coordinate system 1 is rotated around the axisx1 to make the axis z1coincide with the joint axis. The coordinate system 2 is obtained.(C)
The coordinate system 2 is rotated around the axisz2 to make the axis x1 coincide with the axis x1. The coordinate system i is obtained.Figure 1
The notation and joint coordinate system defined by the proposed modeling method.The transformation described above is expressed in the form of Lie group as(21)gii−1=Transxi−1,aTransyi−1,bTranszi−1,cRotx1,αRotz2,θ=RxαRzθpxa+pyb+pzc01=gii−10eξ^iiθi.The right subscript of the Lie algebraξ^ii represents the component number, and the left superscript of it represents the coordinate system number that describes the motion. gii−1 contains the initial pose gii−10 and rotation angle θi of the component.The pose of the componentn relative to the inertial coordinate system is expressed as(22)gn0=g10g21,…,gnn−1=Rn0Pn001.


## 3.2. Derivation of Kinematic Parameter Formula of Robot Mechanism in Lie Group Framework

Set up that, the position vector is expressed asrip, the left superscript of the position vector represents the coordinate system that describes the vector, the right subscript of the position vector represents the target point, and the homogeneous coordinate of the position vector is expressed as r¯ip.The body velocity of component 1 relative to the inertial coordinate system is(23)V^10B=g10−1g˙10=R10TR˙10R10TP˙1000=ω^10BV10B00=θ˙1ξ^11.The body velocity of componenti relative to the coordinate system i − 1 is(24)V^ii−1B=gii−1−1g˙ii−1=θ˙iξ^ii.The body velocity of componenti relative to the inertial coordinate system is(25)V^i0B=gi−10gii−10eξ^iiθi−1ddtgi−10gii−10eξ^iiθi=Adgii−1−1V^i−10B+V^ii−1B=Ri0TR˙i0Ri0TP˙i000=ω^i0BVi0B00.Equations (23)–(25) are body velocity’s recursive formulas of robot mechanism in the Lie group framework. The body velocity is used to derive the space velocity and the dynamic equations. The space velocity is used to derive the angular velocity of each component and the linear velocity of the target point on components in the inertial coordinate system.The space velocity of componenti relative to the inertial coordinate system is(26)V^i0S=Adgi0V^i0B=R˙i0Ri0T−R˙i0Ri0TPi0+P˙i000=ω^i0SVi0S00.Then, the rotational angular velocity of componenti in the inertial coordinate system is(27)ωi0=ωi0xSωi0ySωi0zS=00100000−1000V^i0S1110+100010001000V^i0S0010.Then, the rotational angular acceleration of componenti in the inertial coordinate system is(28)αi0=ω˙i0.The linear velocity of any pointp on component i in the inertial coordinate system is(29)v0p=vi0S+ω^i0Sr0p=I3×303×1v^i0Sr¯0p.The acceleration of any pointp on component i in the inertial coordinate system is(30)a0p=v˙0p.The Jacobian matrix formula of robot mechanism is derived as follows.Let the rotation velocity of each joint of the robot beθ˙=θ˙1,θ˙2,…,θ˙nT and the position of the reference point on the robot end-effector in the inertial coordinate system be r0=x0,y0,z0T.The linear velocity of the reference point on the end-effector in the inertial coordinate system isv0, and it is expressed as(31)v0=ddtr0=∑i=1n∂r0∂θiθ˙i=Jvθ˙.So, the displacement part of Jacobian matrix of the robot is(32)Jv=∂x0∂θ1∂x0∂θ2⋯∂x0∂θn∂y0∂θ1∂y0∂θ2⋯∂y0∂θn∂z0∂θ1∂z0∂θ2⋯∂z0∂θn.The angular velocity Lie algebra of the reference point on the end-effector in the inertial coordinate system isω^0, and it is expressed as(33)ω^0=ω^S=R˙R−1=∑i=1n∂R∂θiθ˙iR−1=∑i=1n∂R∂θiR−1θ˙i.Then, the angular velocity of the reference point on the end-effector in the inertial coordinate system is(34)ω0=∑i=1n∂R∂θiR−1∨θ˙i=Jωθ˙.So, the rotation part of Jacobian matrix of the robot is(35)Jω=∂R∂θ1R−1∨,∂R∂θ2R−1∨,…,∂R∂θnR−1∨.


## 4. Lagrange Equation in the Framework of the Lie Group

Let the robot mechanism be an n-DOF multi rigid body system.Ek is the total kinetic energy of the robot, Ep is the total potential energy of the robot, qi is the generalized coordinate of the robot, q˙i is the generalized velocity of the robot, Fi is the nonpotential generalized force corresponding to the generalized coordinate qi. Then, the Lagrange equation of the n-DOF robot mechanism is(36)ddt∂Ek∂q˙i−∂Ek∂qi+∂Ep∂qi=Fi,i=1,2,…,n.According to Konig’s theorem, the total kinetic energy of the robot is(37)Ek=∑i=1n12mivi0icBTvi0icB+12ωi0BTJiωi0B.The total potential energy of the robot is(38)Ep=∑i=1nmigr¯0ic,where mi is the mass of component i, vi0icB is the linear velocity of the centroid of component i in the coordinate system i, ωi0B is the angular velocity of component i in the coordinate system i, Ji is the inertia tensor matrix of component i relative to the coordinate system whose origin is at the component’s centroid and coordinate axis’ direction is the same as that of coordinate system i, g=0,0,9.8,0, and r¯0ic is the homogeneous coordinate of the centroid of component i in the inertial coordinate system.The linear velocity of the centroid of componenti in the coordinate system i is expressed by the body velocity of component i as(39)vi0icB=vi0B+ω^i0Briic=I3×303×1v^i0Br¯0ic,where riic is the position vector of the centroid of component i in the coordinate system i.The angular velocity of componenti in the coordinate system i is expressed by the body velocity of component i as(40)ωi0B=ωi0xBωi0yBωi0zB=00100000−1000V^i0B1110+100010001000V^i0B0010.


## 5. The Kinematic Model of the Manipulator

The researched manipulator mainly consists of a base, the slewing bearing, four manipulator arms, five hydraulic cylinders, six connecting rods, and three stage telescopic arms embedded in the third manipulator arms. Its main mechanical structure is shown in Figure2. During its working process, the telescopic arms are only used to lengthen after the manipulator forms the final pose and the three telescopic arms move with the same velocity simultaneously. So, the three stage telescopic arms are treated as one motion pair. The pin shafts and the rotary bearing can be seen as rotation pairs and the hydraulic cylinders can be seen as the motion pairs. The hydraulic cylinder's cylinder block and the piston rod need not be considered separately. When conducting analysis, the hydraulic cylinder is regarded as one component with variable length. Applying the kinematic modeling method of robot mechanism proposed in Section 3 to the researched manipulator, components 0–16, joints 1–16, and joint coordinate systems 0–16 are obtained. We can see that joint 1 is the rotation pair with vertical axis and other rotation pairs are parallel and perpendicular to joint 1.Figure 2
The main mechanical structure of the developed manipulator.Observing the manipulator’s structure characteristic, it is found that the manipulator belongs to the closed chain cascade robot [15, 16]. This kind of robot has one main kinematic chain, which is the shortest open chain from the base to the end-effector and several branch chains. When establishing a kinematic model of this kind of robot, the main kinematic chain and branch chains are analyzed separately. The researched manipulator’s main kinematic chain consists of the base, the four manipulator arms, and the telescopic arms. Its mechanical structure and mechanism diagram are shown in Figure 3. The researched manipulator has four branch chains which consist of the hydraulic cylinders and the connecting rods. Its mechanical structure and mechanism diagram are shown in Figures 4–7.Figure 3
(a) The mechanical structure of the manipulator’s main kinematic chain. (b) The mechanism diagram of the manipulator’s main kinematic chain and the established joint coordinate systems.
(a)(b)Figure 4
(a) The mechanical structure of the manipulator’s first kinematic branch chain. (b) The mechanism diagram of the manipulator’s first kinematic branch chain and the established joint coordinate systems.
(a)(b)Figure 5
(a) The mechanical structure of the manipulator’s second kinematic branch chain. (b) The mechanism diagram of the manipulator’s second kinematic branch chain and the established joint coordinate systems.
(a)(b)Figure 6
(a) The mechanical structure of the manipulator’s third kinematic branch chain. (b) The mechanism diagram of the manipulator’s third kinematic branch chain and the established joint coordinate systems.
(a)(b)Figure 7
(a) The mechanical structure of the manipulator’s fourth kinematic branch chain. (b) The mechanism diagram of the manipulator’s fourth kinematic branch chain and the established joint coordinate systems.
(a)(b)


### 5.1. The Kinematic Model of the Manipulator’s Main Kinematic Chain

The kinematic model of the main kinematic chain is used as the governing equations in the kinematic model of the manipulator. According to equation (21) and the established joint coordinate systems in Figure 3(b), the transformation matrices between the adjacent joint coordinate systems are obtained as follows.The pose matrix of the joint coordinate systemo1x1y1z1 relative to the inertial coordinate system o0x0y0z0 is(41)g10=Rotz,θ1=Rzθ1001.The pose matrix of the joint coordinate systemo2x2y2z2 relative to o1x1y1z1 is(42)g21=Transr1ABRotx,90°Rotz,θ2=Rx90∘Rzθ2r1AB01.The pose matrix of the joint coordinate systemo3x3y3z3 relative to o2x2y2z2 is(43)g32=Transx,lBCRotz,θ3=Rzθ3pxlBC01.The pose matrix of the joint coordinate systemo4x4y4z4 relative to o3x3y3z3 is(44)g43=Transx,lCDRotz,θ4=Rzθ4pxlCD01.The pose matrix of the joint coordinate systemo5x5y5z5 relative to o4x4y4z4 is(45)g54=Transx,lDERotz,θ5Transx,lEF=Rzθ5Rzθ5pxlEF+pxlDE01.The pose matrix of the joint coordinate systemo6x6y6z6 relative to o5x5y5z5 is(46)g65=Transr5FGRotz,θ6=Rzθ6r5FG01.Then, the pose of each component relative to the inertial coordinate system is expressed as follows.The pose of the component 2 relative to the inertial coordinate systemo0x0y0z0 is(47)g20=g10g21=Rzθ1Rx90∘Rzθ2Rzθ1r1AB01.The pose of the component 3 relative to the inertial coordinate systemo0x0y0z0 is(48)g30=g20g32=Rzθ1Rx90∘Rzθ2+θ3Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB01.The pose of the component 4 relative to the inertial coordinate systemo0x0y0z0 is(49)g40=g30g43=R40p4001,where there is (the detailed derivation process is in Appendix A and this is adopted in the same condition later.)(50)R40=Rzθ1Rx90∘Rzθ2+θ3+θ4,p40=Rzθ1Rx90∘Rzθ2+θ3pxlCD+Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB.The pose of the component 5 relative to the inertial coordinate systemo0x0y0z0 is(51)g50=g40g54=R50p5001,where there is(52)R50=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5,p50=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5pxlEF+Rzθ1Rx90∘Rzθ2+θ3+θ4pxlDE+Rzθ1Rx90∘Rzθ2+θ3pxlCD+Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB.The pose of the component 6 relative to the inertial coordinate systemo0x0y0z0 is(53)g60=g50g65=R60p6001,where there is(54)R60=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5+θ6,p60=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5r5FG+Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5pxlEF+Rzθ1Rx90∘Rzθ2+θ3+θ4pxlDE+Rzθ1Rx90∘Rzθ2+θ3pxlCD+Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB.Let theH-point on the fourth manipulator arm shown in Figure 3 represent the position of the end tongs. Then, the homogeneous coordinate of the H-point in the inertial coordinate system is r¯0H=g60r¯6H.According to (23) and (25), the body velocities of components of the manipulator’s main kinematic chain are(55)V^10B=θ˙1ξ^11,V^i0B=Adgii−1−1V^i−10B+θ˙iξ^ii,i=2,3,4,5,6,where g−1=RT−RTp01.According to Figure3(b), components 1, 2, 3, 4, and 6 all rotate around the z axis of their own body-fix coordinate system. So, their Lie algebra is(56)ξ^11=ξ^22=ξ^33=ξ^44=ξ^66=0−100100000000000.Obviously, the body velocity of the telescopic arm relative to the fourth manipulator arm is(57)V^54B=θ˙5ξ^55=000l˙EF000000000000.In order to obtain the kinematic parameters of the component in the inertial system, the space velocities of components are calculated. According to (26), the space velocity of the rotary bearing is(58)V^10S=Adg10V^10B.Then, according to (27) and (28), the angular velocity and the angular acceleration of the rotary bearing in the inertial coordinate system are(59)ω10=00100000−1000V^10S1110+100010001000V^10S0010,α10=ω˙10.According to (29) and (30), the linear velocity and the acceleration of the centroid of the rotary bearing in the inertial coordinate system are(60)v01c=I3×303×1V^10Sr¯01c,a01c=v˙01c.The derivation process of the space velocity, the rotating angular velocity, the angular acceleration, the linear velocity, and the acceleration of centroid of the manipulator arms in the inertial coordinate system are exactly the same as that of the rotary bearing. The derivation process is in detail in Appendix B.According to equation (32), the displacement part of the Jacobian matrix of the manipulator is(61)Jv=∂x0H∂θ1∂x0H∂θ2∂x0H∂θ3∂x0H∂θ4∂x0H∂lEF∂x0H∂θ6∂y0H∂θ1∂y0H∂θ2∂y0H∂θ3∂y0H∂θ4∂y0H∂lEF∂y0H∂θ6∂z0H∂θ1∂z0H∂θ2∂z0H∂θ3∂z0H∂θ4∂z0H∂lEF∂z0H∂θ6.According to equation (35), the rotation part of the Jacobian matrix of the manipulator is(62)Jω=∂R60∂θ1R60−1∨,∂R60∂θ2R60−1∨,∂R60∂θ3R60−1∨,∂R60∂θ4R60−1∨,∂R60∂lEFR60−1∨,∂R60∂θ6R60−1∨.In equation (62), items are calculated as follows:(63)∂R60∂θ1R60−1=s^zR60R60−1=s^z,∂R60∂θ2R60−1=∂R60∂θ3R60−1=∂R60∂θ4R60−1=∂R60∂θ6R60−1=AdR60s^z=00−cosθ100−sinθ1cosθ1sinθ10,∂R60∂lEFR60−1=0,where there is s^z=0−10100000 according to the Lie group theory.


### 5.2. Kinematic Model of the Manipulator’s Branch Kinematic Chains

The four hydraulic cylinders of the manipulator and their connecting rods are the four branch kinematic chains of the manipulator, which form seven closed chains together with the main kinematic chain of the manipulator. The kinematic model of the branch kinematic chain is used as the constraint equations in the kinematic model of the manipulator.Figure4 shows the first branch kinematic chain and its mechanism diagram. The kinematic model of the first branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo7x7y7z7 relative to o1x1y1z1 is(64)g71=Transr1AIRotx,90∘Rotz,θ7=Rx90∘Rzθ7r1AI01.From the geometric relationship of the closed loop BIJ, we obtain(65)g61=Transr1ABRotx,90∘Rotz,θ6=Rx90∘Rzθ6r1AB01.Figure5 shows the second branch kinematic chain and its mechanism diagram. The kinematic model of the second branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo8x8y8z8 relative to o2x2y2z2 is(66)g82=Transr2BKRotz,θ8=Rzθ8r2BK01.The pose matrix of the joint coordinate systemo9x9y9z9 relative to o8x8y8z8 is(67)g98=Transx,lKLRotz,θ9=Rzθ9pxlKL01.The pose matrix of the joint coordinate systemo10x10y10z10 relative to o8x8y8z8 is(68)g108=Transx,lKLRotz,θ10=Rzθ10pxlKL01.From the geometric relationship of the closed loop BKLN, we obtain(69)r¯2N=g82g98r¯9N.From the geometric relationship of the closed loop BKMC, we obtain(70)g32r¯3M=g82g108r¯10M.Figure6 shows the third branch kinematic chain and its mechanism diagram. The kinematic model of the third branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo11x11y11z11 relative to o3x3y3z3 is(71)g113=Transr3CORotz,θ11=Rzθ11r3CO01.The pose matrix of the joint coordinate systemo12x12y12z12 relative to o11x11y11z11 is(72)g1211=Transx,lOPRotz,θ12=Rzθ12pxlOP01.The pose matrix of the joint coordinate systemo13x13y13z13 relative to o11x11y11z11 is(73)g1311=Transx,lOPRotz,θ13=Rzθ13pxlOP01.From the geometric relationship of the closed loop COPQ, we obtain(74)r¯3Q=g113g1211r¯12Q.From the geometric relationship of the closed loop CORD, we obtain(75)g43r¯4R=g113g1311r¯13R.Figure7 shows the fourth branch kinematic chain and its mechanism diagram. The kinematic model of the fourth branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo14x14y14z14 relative to o5x5y5z5 is(76)g145=Transr5FSRotz,θ14=Rzθ14r5FS01.The pose matrix of the joint coordinate systemo15x15y15z15 relative to o14x14y14z14 is(77)g1514=Transx,lSTRotz,θ15=Rzθ15pxlST01.The pose matrix of the joint coordinate systemo16x16y16z16 relative to o15x15y15z15 is(78)g1615=Transr15TURotz,θ16=Rzθ16r15TU01.From the geometric relationship of the closed loop FSTV, we obtain(79)g65r¯6V=g145g1514r¯15V.From the geometric relationship of the closed loop FSWG, we obtain(80)g65r¯6W=g145g1514g1615r¯16W.Let the lengths of hydraulic cylinders bel1mm, l2mm, l3mm, and l4mm. Then, related structural parameters are r¯7J=l1,0,0,1T, pxlKL=l2,0,0T, pxlOP=l3,0,0T, and r¯16W=l5,0,0,1T, and telescopic speeds of hydraulic cylinders are v1=dl1/dtmm/s, v2=dl2/dtmm/s, v3=dl3/dtmm/s, and v4=dl4/dtmm/s.The Lie group expression of the pose matrix of each component of the manipulator’s branch kinematic chains is obtained. Then, the body velocities of these components are calculated as follows:(81)V^70B=Adg71−1V^10B+θ˙7ξ^77,V^80B=Adg82−1V^20B+θ˙8ξ^88,V^90B=Adg98−1V^80B+θ˙9ξ^99,V^100B=Adg108−1V^80B+θ˙10ξ^1010,V^110B=Adg113−1V^30B+θ˙11ξ^1111,V^120B=Adg1211−1V^110B+θ˙12ξ^1212,V^130B=Adg1311−1V^110B+θ˙13ξ^1313,V^140B=Adg145−1V^50B+θ˙14ξ^1414,V^150B=Adg1514−1V^140B+θ˙15ξ^1515,V^160B=Adg1615−1V^150B+θ˙16ξ^1616.According to Figures4–7, component 7 to component 16 all rotate around the z axis of their own body-fix coordinate system. So, their Lie algebra is(82)ξ^kk=0−100100000000000,where k can be 7 to 16.


## 5.1. The Kinematic Model of the Manipulator’s Main Kinematic Chain

The kinematic model of the main kinematic chain is used as the governing equations in the kinematic model of the manipulator. According to equation (21) and the established joint coordinate systems in Figure 3(b), the transformation matrices between the adjacent joint coordinate systems are obtained as follows.The pose matrix of the joint coordinate systemo1x1y1z1 relative to the inertial coordinate system o0x0y0z0 is(41)g10=Rotz,θ1=Rzθ1001.The pose matrix of the joint coordinate systemo2x2y2z2 relative to o1x1y1z1 is(42)g21=Transr1ABRotx,90°Rotz,θ2=Rx90∘Rzθ2r1AB01.The pose matrix of the joint coordinate systemo3x3y3z3 relative to o2x2y2z2 is(43)g32=Transx,lBCRotz,θ3=Rzθ3pxlBC01.The pose matrix of the joint coordinate systemo4x4y4z4 relative to o3x3y3z3 is(44)g43=Transx,lCDRotz,θ4=Rzθ4pxlCD01.The pose matrix of the joint coordinate systemo5x5y5z5 relative to o4x4y4z4 is(45)g54=Transx,lDERotz,θ5Transx,lEF=Rzθ5Rzθ5pxlEF+pxlDE01.The pose matrix of the joint coordinate systemo6x6y6z6 relative to o5x5y5z5 is(46)g65=Transr5FGRotz,θ6=Rzθ6r5FG01.Then, the pose of each component relative to the inertial coordinate system is expressed as follows.The pose of the component 2 relative to the inertial coordinate systemo0x0y0z0 is(47)g20=g10g21=Rzθ1Rx90∘Rzθ2Rzθ1r1AB01.The pose of the component 3 relative to the inertial coordinate systemo0x0y0z0 is(48)g30=g20g32=Rzθ1Rx90∘Rzθ2+θ3Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB01.The pose of the component 4 relative to the inertial coordinate systemo0x0y0z0 is(49)g40=g30g43=R40p4001,where there is (the detailed derivation process is in Appendix A and this is adopted in the same condition later.)(50)R40=Rzθ1Rx90∘Rzθ2+θ3+θ4,p40=Rzθ1Rx90∘Rzθ2+θ3pxlCD+Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB.The pose of the component 5 relative to the inertial coordinate systemo0x0y0z0 is(51)g50=g40g54=R50p5001,where there is(52)R50=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5,p50=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5pxlEF+Rzθ1Rx90∘Rzθ2+θ3+θ4pxlDE+Rzθ1Rx90∘Rzθ2+θ3pxlCD+Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB.The pose of the component 6 relative to the inertial coordinate systemo0x0y0z0 is(53)g60=g50g65=R60p6001,where there is(54)R60=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5+θ6,p60=Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5r5FG+Rzθ1Rx90∘Rzθ2+θ3+θ4+θ5pxlEF+Rzθ1Rx90∘Rzθ2+θ3+θ4pxlDE+Rzθ1Rx90∘Rzθ2+θ3pxlCD+Rzθ1Rx90∘Rzθ2pxlBC+Rzθ1r1AB.Let theH-point on the fourth manipulator arm shown in Figure 3 represent the position of the end tongs. Then, the homogeneous coordinate of the H-point in the inertial coordinate system is r¯0H=g60r¯6H.According to (23) and (25), the body velocities of components of the manipulator’s main kinematic chain are(55)V^10B=θ˙1ξ^11,V^i0B=Adgii−1−1V^i−10B+θ˙iξ^ii,i=2,3,4,5,6,where g−1=RT−RTp01.According to Figure3(b), components 1, 2, 3, 4, and 6 all rotate around the z axis of their own body-fix coordinate system. So, their Lie algebra is(56)ξ^11=ξ^22=ξ^33=ξ^44=ξ^66=0−100100000000000.Obviously, the body velocity of the telescopic arm relative to the fourth manipulator arm is(57)V^54B=θ˙5ξ^55=000l˙EF000000000000.In order to obtain the kinematic parameters of the component in the inertial system, the space velocities of components are calculated. According to (26), the space velocity of the rotary bearing is(58)V^10S=Adg10V^10B.Then, according to (27) and (28), the angular velocity and the angular acceleration of the rotary bearing in the inertial coordinate system are(59)ω10=00100000−1000V^10S1110+100010001000V^10S0010,α10=ω˙10.According to (29) and (30), the linear velocity and the acceleration of the centroid of the rotary bearing in the inertial coordinate system are(60)v01c=I3×303×1V^10Sr¯01c,a01c=v˙01c.The derivation process of the space velocity, the rotating angular velocity, the angular acceleration, the linear velocity, and the acceleration of centroid of the manipulator arms in the inertial coordinate system are exactly the same as that of the rotary bearing. The derivation process is in detail in Appendix B.According to equation (32), the displacement part of the Jacobian matrix of the manipulator is(61)Jv=∂x0H∂θ1∂x0H∂θ2∂x0H∂θ3∂x0H∂θ4∂x0H∂lEF∂x0H∂θ6∂y0H∂θ1∂y0H∂θ2∂y0H∂θ3∂y0H∂θ4∂y0H∂lEF∂y0H∂θ6∂z0H∂θ1∂z0H∂θ2∂z0H∂θ3∂z0H∂θ4∂z0H∂lEF∂z0H∂θ6.According to equation (35), the rotation part of the Jacobian matrix of the manipulator is(62)Jω=∂R60∂θ1R60−1∨,∂R60∂θ2R60−1∨,∂R60∂θ3R60−1∨,∂R60∂θ4R60−1∨,∂R60∂lEFR60−1∨,∂R60∂θ6R60−1∨.In equation (62), items are calculated as follows:(63)∂R60∂θ1R60−1=s^zR60R60−1=s^z,∂R60∂θ2R60−1=∂R60∂θ3R60−1=∂R60∂θ4R60−1=∂R60∂θ6R60−1=AdR60s^z=00−cosθ100−sinθ1cosθ1sinθ10,∂R60∂lEFR60−1=0,where there is s^z=0−10100000 according to the Lie group theory.


## 5.2. Kinematic Model of the Manipulator’s Branch Kinematic Chains

The four hydraulic cylinders of the manipulator and their connecting rods are the four branch kinematic chains of the manipulator, which form seven closed chains together with the main kinematic chain of the manipulator. The kinematic model of the branch kinematic chain is used as the constraint equations in the kinematic model of the manipulator.Figure4 shows the first branch kinematic chain and its mechanism diagram. The kinematic model of the first branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo7x7y7z7 relative to o1x1y1z1 is(64)g71=Transr1AIRotx,90∘Rotz,θ7=Rx90∘Rzθ7r1AI01.From the geometric relationship of the closed loop BIJ, we obtain(65)g61=Transr1ABRotx,90∘Rotz,θ6=Rx90∘Rzθ6r1AB01.Figure5 shows the second branch kinematic chain and its mechanism diagram. The kinematic model of the second branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo8x8y8z8 relative to o2x2y2z2 is(66)g82=Transr2BKRotz,θ8=Rzθ8r2BK01.The pose matrix of the joint coordinate systemo9x9y9z9 relative to o8x8y8z8 is(67)g98=Transx,lKLRotz,θ9=Rzθ9pxlKL01.The pose matrix of the joint coordinate systemo10x10y10z10 relative to o8x8y8z8 is(68)g108=Transx,lKLRotz,θ10=Rzθ10pxlKL01.From the geometric relationship of the closed loop BKLN, we obtain(69)r¯2N=g82g98r¯9N.From the geometric relationship of the closed loop BKMC, we obtain(70)g32r¯3M=g82g108r¯10M.Figure6 shows the third branch kinematic chain and its mechanism diagram. The kinematic model of the third branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo11x11y11z11 relative to o3x3y3z3 is(71)g113=Transr3CORotz,θ11=Rzθ11r3CO01.The pose matrix of the joint coordinate systemo12x12y12z12 relative to o11x11y11z11 is(72)g1211=Transx,lOPRotz,θ12=Rzθ12pxlOP01.The pose matrix of the joint coordinate systemo13x13y13z13 relative to o11x11y11z11 is(73)g1311=Transx,lOPRotz,θ13=Rzθ13pxlOP01.From the geometric relationship of the closed loop COPQ, we obtain(74)r¯3Q=g113g1211r¯12Q.From the geometric relationship of the closed loop CORD, we obtain(75)g43r¯4R=g113g1311r¯13R.Figure7 shows the fourth branch kinematic chain and its mechanism diagram. The kinematic model of the fourth branch kinematic chain is established as follows.The pose matrix of the joint coordinate systemo14x14y14z14 relative to o5x5y5z5 is(76)g145=Transr5FSRotz,θ14=Rzθ14r5FS01.The pose matrix of the joint coordinate systemo15x15y15z15 relative to o14x14y14z14 is(77)g1514=Transx,lSTRotz,θ15=Rzθ15pxlST01.The pose matrix of the joint coordinate systemo16x16y16z16 relative to o15x15y15z15 is(78)g1615=Transr15TURotz,θ16=Rzθ16r15TU01.From the geometric relationship of the closed loop FSTV, we obtain(79)g65r¯6V=g145g1514r¯15V.From the geometric relationship of the closed loop FSWG, we obtain(80)g65r¯6W=g145g1514g1615r¯16W.Let the lengths of hydraulic cylinders bel1mm, l2mm, l3mm, and l4mm. Then, related structural parameters are r¯7J=l1,0,0,1T, pxlKL=l2,0,0T, pxlOP=l3,0,0T, and r¯16W=l5,0,0,1T, and telescopic speeds of hydraulic cylinders are v1=dl1/dtmm/s, v2=dl2/dtmm/s, v3=dl3/dtmm/s, and v4=dl4/dtmm/s.The Lie group expression of the pose matrix of each component of the manipulator’s branch kinematic chains is obtained. Then, the body velocities of these components are calculated as follows:(81)V^70B=Adg71−1V^10B+θ˙7ξ^77,V^80B=Adg82−1V^20B+θ˙8ξ^88,V^90B=Adg98−1V^80B+θ˙9ξ^99,V^100B=Adg108−1V^80B+θ˙10ξ^1010,V^110B=Adg113−1V^30B+θ˙11ξ^1111,V^120B=Adg1211−1V^110B+θ˙12ξ^1212,V^130B=Adg1311−1V^110B+θ˙13ξ^1313,V^140B=Adg145−1V^50B+θ˙14ξ^1414,V^150B=Adg1514−1V^140B+θ˙15ξ^1515,V^160B=Adg1615−1V^150B+θ˙16ξ^1616.According to Figures4–7, component 7 to component 16 all rotate around the z axis of their own body-fix coordinate system. So, their Lie algebra is(82)ξ^kk=0−100100000000000,where k can be 7 to 16.


## 6. Dynamic Model of the Manipulator

Analyzing the structure of the manipulator, it can be seen that the manipulator has six degrees of freedom. They are the rotation of the rotary bearing driven by the motor, the rotations of the first to the fourth manipulator arm driven by four hydraulic cylinders, and the telescopic motion of the telescopic arm, respectively. Taking the rotation anglesθ1, θ2, θ3,θ4, θ6 and the telescopic length lEF as the generalized coordinates, the corresponding nonpotential generalized forces are the driving moments M1, M2, M3, M4, and M5 on rotation joints and the driving force FEF on the telescopic arm. Because the telescopic arm’s motion and force condition are relatively simple, the driving force of the corresponding hydraulic cylinder can be obtained easily. Here, we only focus on the driving moments M1, M2, M3, M4, and M5. Then, the Lagrange equation of the manipulator is(83)ddt∂Ek∂θ˙i−∂Ek∂θi+∂Ep∂θi=Mi,i=1,2,3,4,6,whereEk is the total kinetic energy of the manipulator and Ep is the total potential energy of the manipulator.According to equation (37), the total kinetic energy of the manipulator is calculated as follows.The total kinetic energy of the base, the four manipulator arms, the three telescopic arms, and the connecting rod 6 is(84)E1=∑i=1612mivi0icBTvi0icB+12ωi0BTJiωi0B+12m15v15015cBTv15015cB+12ω150BTJ15ω150B.The total kinetic energy of the four hydraulic cylinders is(85)E2=∑i12mitubevi0itubecBTvi0itubecB+12ωi0BTJitubeωi0B+12mirodvi0irodcBTvi0irodcB+12ωi0BTJirodωi0B,i=7,8,11,16.The connecting rods 1, 2, 3, 4, and 5 are all in pairs, and the total kinetic energy of them is(86)E3=∑i212mivi0icBTvi0icB+12ωi0BTJiωi0B,i=9,10,12,13,14.Then, the total kinetic energy of the manipulator is(87)Ek=E1+E2+E3.According to equation (38), the total potential energy of the manipulator is(88)Ep=∑i=16migr¯0ic+m7tubegr¯07tubec+m7rodgr¯07rodc+m8tubegr¯08tubec+m8rodgr¯08rodc+m11tubegr¯011tubec+m11rodgr¯011rodc+m16tubegr¯016tubec+m16rodgr¯016rodc+2m9gr¯09c+2m10gr¯010c+2m12gr¯012c+2m13gr¯013c+2m14gr¯014c+m15gr¯015c,where g=0,0,9.8,0.The unknown parameters in equations (84)–(86) and (88) are calculated as follows. The homogeneous coordinate of each component’s centroid in the inertial coordinate system is calculated according to the obtained homogeneous matrix. The centroid linear velocities and the rotating angular velocities of the rotary bearing, the four manipulator arms, and the three telescopic arms in their body-fixed coordinate systems are calculated according to equations (39) and (40). The detailed derivation process is shown in Appendix C. The centroid linear velocities of hydraulic cylinder’s piston rod in the body-fixed coordinate system are calculated on the basis of equation (39) with a slight modification. The parameters of the first hydraulic cylinder are calculated as follows. The parameters of the second, the third, the fourth, and the fifth hydraulic cylinder are calculated with the same method as the first hydraulic cylinder and their detailed derivation process is shown in Appendix D.The linear velocity of the centroid of the first hydraulic cylinder’s cylinder tube in the coordinate system 7 is(89)v707tubecB=I3×303×1V^70Br¯77tubec.When analyzing the velocity of the centroid of the first hydraulic cylinder’s piston rod, the composition theorem of rigid body motion is used. The linear velocity of the centroid of the first hydraulic cylinder’s piston rod is expressed in coordinate system 7 as(90)v707tubecB=I3×303×1V^70Br¯77tubec+l˙100.The rotation angular velocity of the first hydraulic cylinder in the coordinate system 7 is(91)ω70B=00100000−1000V^70B1110+100010001000V^70B0010.Since the driving force of the hydraulic cylinder is the basis for force control, force position hybrid control, and energy consumption control, etc., the relationship between the generalized force and the driving force of the hydraulic cylinder should be constructed.The relationship between the driving force applied by the first hydraulic cylinder on the first manipulator arm and the generalized forceM2 is(92)M22=r2BJ×F21,F21=100001000010g72F1,0,0,1T=100001000010g21−1g71F1,0,0,1T,where M22 is the moment-vector of generalized force M2 in the coordinate system 2 and F21 is the driving force applied by the first hydraulic cylinder on the first manipulator arm in the coordinate system 2.The drive of the second hydraulic cylinder to the second manipulator arm is transmitted through connecting rod 1. According to Figure8(b), we can see that the component force F9 along connecting rod 1 and the component force F10 along connecting rod 2 are obtained through decomposing the driving force of the second hydraulic cylinder on its action point. Their relationship is(93)F89+F810=F82,F89=100001000010g98F9,0,0,1T,F89=100001000010g98F9,0,0,1T,where F89 is the push force applied by the second hydraulic cylinder on the connecting rod 1 in the coordinate system 8, F810 is the push force applied by the second hydraulic cylinder on the connecting rod 2 in the coordinate system 8, and F82 is the total force of the second hydraulic cylinder in coordinate system 8.Figure 8
The driving force of each hydraulic cylinder and its corresponding generalized force. The driving force of (a) the first hydraulic cylinder and its corresponding generalized force, (b) the second hydraulic cylinder and its corresponding generalized force, (c) the third hydraulic cylinder and its corresponding generalized force, and (d) the fifth hydraulic cylinder and its corresponding generalized force.
(a)(b)(c)(d)According to Figure8(b), the relationship of the F9 and the generalized force M3 is(94)M33=r3CM×F39,F39=100001000010g93F9,0,0,1T=100001000010g32−1g82g98F9,0,0,1T,where M33 is the moment-vector of generalized force M3 in the coordinate system 3 and F39 is the push force applied by the connecting rod 1 on the second manipulator arm in the coordinate system 3.The relationship between the driving forces of the third hydraulic cylinder and the fifth hydraulic cylinder and their corresponding generalized forces is calculated with the same method as the second hydraulic cylinder. Their force analysis diagrams are Figures8(c) and 8(d). The calculation process is not explained in detail here. And the equations describe their relationships are shown in Appendix E.


## 7. Kinematic and Dynamic Analysis of the Manipulator

The initial configuration of the manipulator isθ1=0∘, θ2=133.67∘, θ3=−133.67∘, θ4=−150∘, lEF=0, andθ6=−180∘.The working process of the manipulator is as follows:(1)
In 0–10 s, the first and the second manipulator arms rotate to the required position of the expansion pose(2)
In 10–20 s, the third manipulator arm rotates to the required position of the expansion pose(3)
In 20–30 s, the fourth manipulator arm rotates to the required position of the expansion pose(4)
In 30–40 s, the first to the fourth manipulator arms and the base rotate to the pose that the fourth manipulator arm is vertical and the pointH is on top of the transported object with 1 m distance from the ground(5)
In 40–50 s, the manipulator keeps pausing for 10 s, during which the tongs at the end of the fourth manipulator grabs the transported object(6)
In 50–60 s, the first to the fourth manipulator arms and the base rotate to the preparation pose before entering the pipeline(7)
In 60–70 s, the first to the third manipulator arms rotate to keep the third and the fourth manipulator arms vertical and enter the pipeline along its vertical straight line at a constant speed(8)
In 70–75 s, the telescopic arms extend(9)
In 75–80 s, the fourth manipulator arm rotates and the telescopic arms extend to form the designated poseAccording to the structural dimension of the manipulator and the known conditions, assign the parameters in the established mathematical model. Then, the numerical solution is conducted.The numerical solution path of the manipulator’s kinematic and dynamic models is shown in Table1.Table 1
The numerical solution path of the kinematic and dynamic models of the manipulator.
Assign initial values to variablesr0N, v0N.For loop:Calculated variablesgij, θ, l.According togij, V^B, Jv, Jω can be obtained.According togij and V^B, V^S can be obtained, and then ω0, α0, v0ic, a0ic can also be obtained.According togij and V^B, Ek, Ep can be obtained, and then Mi, Fi can also be obtained.Assign new values to variablesr0N, v0N.EndThe variation curves of kinematic parameters of the manipulator in its working process are shown in Figures9–14.Figure 9
The variation curves of the manipulator joint’s kinematic parameters. (a) The manipulator’s joint rotation angle. (b) The manipulator’s joint rotation angular velocity.
(a)(b)Figure 10
The variation curves of the hydraulic cylinder’s kinematic parameters. (a) The length of the hydraulic cylinder. (b) The hydraulic cylinder’s joint rotation angle.
(a)(b)Figure 11
The first manipulator arm’s kinematic parameters. (a) The velocity of the first manipulator arm’s centroid. (b) The rotation angular velocity of the first manipulator arm.
(a)(b)Figure 12
The second manipulator arm’s kinematic parameters. (a) The velocity of the second manipulator arm’s centroid. (b) The rotation angular velocity of the second manipulator arm.
(a)(b)Figure 13
The third manipulator arm’s kinematic parameters. (a) The velocity of the third manipulator arm’s centroid. (b) The rotation angular velocity of the third manipulator arm.
(a)(b)Figure 14
The fourth manipulator arm’s kinematic parameters. (a) The velocity of the fourth manipulator arm’s centroid. (b) The rotation angular velocity of the fourth manipulator arm.
(a)(b)In Figure9(a), θ1, θ2, θ3, θ4, and θ6 are joint angles of the slewing bearing and the first to the fourth manipulator arms, respectively, θ5is the joint angle of the telescopic arm which equals 0 because of its prismatic pair essence. The rotation angles of θ1, θ2, θ3, θ4, and θ6 are about 45°,74.8°,84.3°,159.4°, and 180° respectively. In the 60 s–70 s time period, the manipulator arm’s joint trajectory is planned in the Cartesian space according to the known conditions, so the acceleration at both ends of the trajectory cannot be controlled. Therefore, at times 60 s and 70 s, curves of θ2, θ3, θ4, and θ6 appear at the cusp point which means the sudden change of angular velocity and will lead to the impact and vibration of the mechanical system. In the 75 s–80 s time period, the fourth manipulator arm rotates with a constant velocity of 6°/s and other manipulator arms are static. So, curves of θ6 appear at the cusp point at time 70s. During other time periods, the manipulator arm’s joint trajectory is planned with the method of quintic polynomial trajectory planning in joint space and the angular acceleration at both ends of the trajectory is set to 0. So, except for times equal to 60 s and 70 s, these curves are continuous and smooth, which means all these joints rotate smoothly during the time. As for the slewing bearing, it rotates at a constant velocity of 4.5°/s within the movement stage and has a certain angular acceleration at the beginning and at the end of the motion. In Figure 9(b), ω1, ω2,ω3, ω4, and ω6 are joint angular velocities of the slewing bearing and the first to the fourth manipulator arms, respectively. Figures 9(a) and 9(b) should have a corresponding derivative relationship relative to time. Comparing variation trends of the two figures’ curves, it is in accordance with the mathematical law. From the first to the fourth manipulator arms, the maximum angular velocities of the joint rotation are about 22°/s, 20°/s, 15°/s, and 29°/s, respectively. At 10s, 20s, 30s, 40s, and 50s, the rotation velocities of the manipulator arms' joints are all equal 0. This phenomenon is consistent with that the velocity at both ends of the quintic polynomial trajectory planning section is set to 0. At 60s and 70s, the rotation velocity curves of manipulator arm joints all appear in the step phenomenon. The variation law and key point value of the rotation angle and angular velocity of each joint are consistent with the planned setting, which to a certain extent shows the correctness of the calculation.In Figure10(a), l1, l2, l3, l4, and l5 are lengths of the first to the fifth hydraulic cylinder, respectively. The length variation ranges of the first hydraulic cylinder to the fifth hydraulic cylinder are 1199 mm–1609 mm, 1262 mm–981 mm, 1362 mm–999 mm, 1643 mm–2759 mm, and 537 mm–426 mm, respectively. Checking the strokes of the selected hydraulic cylinders, the length variation ranges are all within the allowable values. The curve’s variation trend of the length of each hydraulic cylinder is completely consistent with that of the joint rotation angle of the corresponding driven manipulator arm. The fourth hydraulic cylinder drives the telescopic arm, and it extends at a constant velocity of about 223 mm/s from 70 s to 75 s. At both ends of its motion stage, the length curve appears at the cusp point. As for other hydraulic cylinders, at the times 60 s and 70 s, their length curves appear at the cusp point. The cusp points on the length curves mean the sudden change of the telescopic velocities of hydraulic cylinders. And this will cause the flow of the hydraulic cylinder increase sharply and even control system to delay. This phenomenon and the hydraulic cylinder’s buffer can reduce the impact and vibration of the mechanical system caused by the velocity mutation to a certain extent. At other times, curves are continuous and smooth, which means telescopic movements of the hydraulic cylinders are stable. In Figure 10(b), θ7, θ8, θ11, and θ16 are joint angles of the first, the second, the third, and the fifth hydraulic cylinders, respectively. According to the mechanical structure of the manipulator, the fourth hydraulic cylinder is fixed on the third manipulator arm, so there is no joint angle of it. The joint rotation angles of the first, the second, the third, and the fifth hydraulic cylinders are about 69.7°, 3.4°, 5.8°, and 61.4°, respectively. Comparing Figures 9(a) and 10(b), it can be seen that the joint angle of the hydraulic cylinder is smaller than that of the corresponding manipulator arm driven by it. The joint rotation angle of the first hydraulic cylinder is almost the same as that of the first manipulator arm. While the joint rotation angle of the second, the third, and the fifth hydraulic cylinders are far less than that of the second, the third, and the fourth manipulator arms, respectively, driven by them. In particular, the joint rotation angles of the second and the third hydraulic cylinder are very small. From this, it can be seen that the designed mechanism form can not only achieve a large range of rotation and even parallel folding but also have a relatively small rotation range of the joint between the hydraulic cylinder and the manipulator arm. Comparing with Figures 9(a) and 10(b), it can also be seen that the variation trend of joint angle curve of the first hydraulic cylinder and the fifth hydraulic cylinder is consistent with that of the manipulator arm driven by them, while the variation trend of joint angle curve of the second hydraulic cylinder and the third hydraulic cylinder is slightly different from that of the manipulator arm driven by them.In Figure11, v is the velocity of the mass center of the first manipulator arm and ω is the angular velocity of the first manipulator arm. Analyzing Figure 11(a), conclusions are as follows. The maximum velocity of the mass center of the first manipulator arm is about 250 mm/s and occurs at the time 70 s. The velocity changes abruptly at the times 60 s and 70 s, and the abrupt change in the x-axis direction is relatively large. The velocity curve is smooth at other times. In the time periods 30 s–40 s and 50 s–60 s, both the x-direction velocity and the Y-direction velocity show the approximate trigonometric function variation law. This means that in these two time periods, the slewing support mainly affects the motion of the first manipulator arm while the joint rotation of the first manipulator arm is small. The y-direction velocity is 0 at other times, which is in accordance with the prescribed trajectory. Analyzing Figure 11(b), conclusions are as follows. The maximum angular velocity of the first manipulator arm is about 23°/s and occurs at the time 70 s. In Figure 11(a) and Figure 11(b), the resultant velocity and resultant angular velocity have the same variation trend except time periods 30 s–40 s and 50 s–60 s, which is because the first manipulator arm makes a fixed-axis rotation at these moments. Observing the variation law of angular velocity in time periods 30 s–40 s and 50 s–60 s, it is found that the resultant angular velocity is basically 4.5°/s, which is the rotation speed of the slewing support, and the angular velocity in each direction varies in accordance with the motion law driven by the slewing support. So, the slewing support mainly affects the motion of the first manipulator arm in this period, which is consistent with the conclusion in Figure 11(a).In Figure12, v is the velocity of the mass center of the second manipulator arm and ω is the angular velocity of the second manipulator arm. Analyzing Figure 12(a), conclusions are as follows. The maximum velocity of the mass center of the second manipulator arm is about 350 mm/s and occurs at the time 70 s. The velocity changes abruptly at the times 30 s, 40 s, 50 s, 60 s, and 70 s, and the velocity curves are smooth at other times. Analyzing Figure 12(b), conclusions are as follows. The maximum angular velocity of the second manipulator arm is about 16.8°/s and occurs at the time 68 s. Observing the variation law of angular velocity in time periods 30 s–40 s and 50 s–60 s, it is found that the resultant angular velocity is basically 4.5°/s which is the rotation speed of the slewing support, and the angular velocity in each direction varies in accordance with the motion law driven by the slewing support. So, the slewing support mainly affects the motion of the second manipulator arm in this period, which is the same condition as the first manipulator arm. The angular velocity changes abruptly at the times 30 s, 40 s, 50 s, 60 s, and 70 s, and the velocity curves are smooth at other times.In Figure13, v is the velocity of the mass center of the third manipulator arm and ω is the angular velocity of the third manipulator arm. Analyzing Figure 13(a), conclusions are as follows. The maximum velocity of the mass center of the third manipulator arm is about 365 mm/s and it moves along the z-axis at the same speed in the time period of 60 s–70 s which is consistent with the prescribed trajectory. The velocity changes abruptly at the times 30 s, 40 s, 50 s, 60 s, and 70 s, and the velocity curves are smooth at other times. Analyzing Figure 13(b), conclusions are as follows. The maximum angular velocity of the third manipulator arm is about 13°/s and occurs at the time 15 s. The angular velocity changes abruptly at the times 30 s, 40 s, 50 s, and 60 s, and the velocity curves are smooth at other times.In Figure14, v is the velocity of the mass center of the fourth manipulator arm and ω is the angular velocity of the fourth manipulator arm. Analyzing Figure 14(a), conclusions are as follows. The maximum velocity of the mass center of the fourth manipulator arm is about 670 mm/s. It makes the uniform linear motion along the z-axis at the speed of 365.5 mm/s in the time period of 60 s–70 s and at the speed of 670 mm/s in the time period of 70 s–75 s, which is consistent with the prescribed trajectory. The velocity changes abruptly at the times 30 s, 40 s, 50 s, 60 s, and 70 s, and the velocity curves are smooth at other times. Analyzing Figure 14(b), conclusions are as follows. The maximum angular velocity of the third manipulator arm is about 30°/s and occurs at the time 25 s. The angular velocity changes abruptly at the times 30 s, 40 s, 50 s, 60 s, and 75 s, and the velocity curves are smooth at other times.In Figure15, S is the displacement of the H-point,V is the velocity of the H-point, and a is the acceleration of the H-point. The motion of H-point represents the motion of the gripper, which represents the transportation effect of the manipulator. Observing the displacement curve, it is shown that its values of the points on the expansion pose, the grabbing pose, the preparation pose, and the designated pose are consistent with the set values and the calculated displacement is consistent with the trajectory planned in the Cartesian space in the 60 s–70 s time period. It means the obtained calculated joint trajectory is correct and enables the manipulator to reach the required position. Curves of displacements in x-direction and y-direction appear at cusp points at times 30 s, 40 s, 50 s, and 60 s. Because the slewing bearing can only affect the displacements of the two directions, it has start or stop status at these times. Curves of displacements in x-direction and z-direction appear at cusp points at 75 s due to the start of the fourth manipulator arm’s uniform rotation. Curves of displacements in z-direction appear at cusp points at times 60 s and 70 s due to the start and stop of the extension of telescopic arms. At other times, the displacement curves are continuous and smooth, which means good motion characteristics embody the superiority of quintic polynomial trajectory planning in joint space. Analyzing Figure 15(b), conclusions are as follows. Figures 15(a) and 15(b) have corresponding derivative relationship relative to time, which is in accordance with the mathematical law. The velocity is about 670 mm/s when telescopic arms are lengthened and the velocities are all within 400 mm/s at other times, which is within the allowable value. The velocity curve is generally continuous and smooth, while there is a step change at the cusp point of the displacement curve. Analyzing Figure 15(c), conclusions are as follows. Figures 15(b) and 15(c) have corresponding derivative relationship relative to time, which is in accordance with the mathematical law. The acceleration curves are generally continuous and smooth but increase sharply at state mutation times. But, the values at acceleration surge points are only theoretical values that the velocity change completes within 0.5 s. These are just for reference. In the engineering practice, due to the delay of the hydraulic control system and the hydraulic cylinder‘s buffer, the actual values are far less than the theoretical values. Through observing, the tongs are not in critical positions at these times. So, the impact and vibration of the mechanical system at these times will not affect the transportation effect. The acceleration’s theoretical value reaches maximum at about 1300 mm/s2 in the z-direction at 75 s. At this time, there are only the fourth manipulator arm and the load in the end to move. In this theoretical extreme state, it is found that the generated inertia force is only about 113 N through calculation. The structure can support such impact.Figure 15
The H-point’s kinematic parameters. (a) The displacement of the H-point in the inertial system. (b) The velocity of the H-point in the inertial system. (c) The acceleration of the H-point in the inertial system.
(a)(b)(c)In Figure16, F1, F2, F3, and F5 are driving forces of the first, the second, the third, and the fifth hydraulic cylinder, respectively. From the figure, it can be seen that the maximum driving force provided by each hydraulic cylinder is about 19843 N, 25874 N, 6428 N, and 2928 N, respectively. Checking the performance parameters of the selected hydraulic cylinders, it is found that the maximum driving force of each hydraulic cylinder is within the allowable range. The first hydraulic cylinder only provides push force before the 4 s, and always provides pull force after the 4s due to the left shift of the overall mass center position of the manipulator. The second hydraulic cylinder always provides push force. The third hydraulic cylinder provides push force before the 15 s and provides pull force after the 15 s due to the third and the fourth manipulator arms rotating to expand. After 77.5s, its driving force changes from the pull force to the push force. Because the position of the centroid of the kinematic chain behind the third manipulator arm changes caused by the rotation of the fourth manipulator arm. The fifth hydraulic cylinder provides push force before the 16 s and provides pull force after the 16 s due to the fourth manipulator arm rotating to expand. After the 74.5 s, its driving force turns into push force because of the centroid position changes caused by the fourth manipulator arm that drives the load to rotate. The driving force curves of hydraulic cylinders all increase sharply at the times 60 s, 70 s, and 75 s because of the sudden change of velocity at these moments. The manipulator changes from the no-load condition to the on-load condition, so the driving forces of the hydraulic cylinders have a step variation at this time. At other times, the driving force curve of each hydraulic cylinder is continuous and smooth, which means the manipulator has good mechanical properties.Figure 16
The driving force of the hydraulic cylinder.Analyzing the above results, conclusions can be obtained as follows:(1)
Most of the time, the kinematic curves and force curves are continuous and smooth, which shows good motion characteristics. The 30 s, 40 s, 50 s, 60 s, 70 s, and 75 s are times when curves change abruptly and will lead to the impact and vibration of the mechanical system. But, in engineering practice, the control system has delay at these time points and the actual values are far less than the theoretical values. In addition, the hydraulic cylinder of the manipulator is equipped with buffer, which can further reduce the impact of velocity mutation. And, at these time points, the manipulator is not in the key positions such as grasping or entering the wellhead. So, the manipulator’s motion characteristics can meet the working requirements.(2)
The maximum kinematic parameters of each manipulator’s component and the maximum output force of each hydraulic cylinder are all within the allowable range. The length range of the hydraulic cylinder is within its stroke range.(3)
The manipulator arm’s rotation angular velocity and angular acceleration have a corresponding derivative relationship relative to time, which is in accordance with the mathematical law. The motion law of the center of mass of the third and the fourth manipulator arm conforms to the prescribed trajectory. To some extent, these show the reliability of the calculation results.(4)
The tongs whose position is represented by the H-point at the end of the manipulator can reach the prescribed positions, and its motion characteristics can meet the working requirements. The manipulator can complete the prescribed task.The manipulator’s virtual prototype simulation is carried out in ADAMS. The results of the virtual prototype simulation are compared with those of the numerical calculation and they are basically consistent, which is shown in Figures17 to 22. It means that the established mathematical model is correct, the numerical calculation method is correct and feasible, and the result curves are correct. Figure 17(a) shows the two results of the displacement of the H-point and the gap between them is within 0.6%. Figure 17(b) shows the two results of the velocity of the H-point. From Figure 17(b), we can see that the gap is relatively large at the state mutation times and the gap is within 5% at other times. On analyzing, the reason is found to be that the time steps of the virtual prototype simulation and the proposed numerical calculation are 0.01 s and 0.5 s, respectively, and ADAMS uses the vibration algorithm at the state mutation times. So, the gap between the two results of velocities is relatively large, especially at the state mutation times, and the gap curve is only for reference. The same variation trend and basically consistent values can prove that the velocity is correct. In addition, in the engineering practice, due to the delay of the hydraulic control system and the effect of the hydraulic cylinder’s buffer, the velocity is reduced largely at the state mutation times. The same condition is also found with Figures 18 and 19(b). Figure 17(c) shows the two results of the acceleration of the H-point and its situation is the same as the analysis of the velocity. Figure 19(a) shows the two results of the manipulator’s joint rotation angle and the gap between them is within 0.6%. Figure 20 shows the two results of the hydraulic cylinder’s joint rotation angle and the gap between them is within 0.09%. Figure 21 shows the length of the hydraulic cylinder and the gap between them is within 0.003%. The length of the hydraulic cylinder obtained from the numerical calculation in this paper is directly imported into ADAMS as driving functions for simulation, so the gap is from the fitting error. Figure 22 shows the two results of the driving force of the hydraulic cylinder. From Figure 22, we can see that the gap is relatively large at the state mutation times and the gap is within 5.5% at other times. The reason for the large gap at the state mutation times is the same as Figure 17(b).Figure 17
The H-point’s kinematic parameters in the inertial coordinate system. (a) The displacement of the H-point and the gap between two results. (b) The velocity of the H-point and the gap between two results. (c) The acceleration of the H-point and the gap between two results.
(a)(b)(c)Figure 18
The four manipulator arms’ kinematic parameters in the inertial coordinate system. (a) The velocities of the four manipulator arms’ centroids. (b) The rotation angular velocities of the four manipulator arms.
(a)(b)Figure 19
The variation curves of the manipulator joints’ kinematic parameters. (a) The manipulator’s joint rotation angles and the gap between two results. (b) The manipulator’s joint rotation angular velocities.
(a)(b)Figure 20
The hydraulic cylinders’ joint rotation angles and the gap between two results.Figure 21
The lengths of hydraulic cylinders and the gap between two results.Figure 22
The driving forces of hydraulic cylinders and the gap between two results.


## 8. Conclusion

In this paper, firstly, the theory of the Lie group is connected with the motion of rigid body and the formulas describing the motion of rigid body in the framework of Lie group are derived. Then, based on the Lie group framework, the kinematics parameter expressions of each component of robot are derived by the proposed kinematic modeling method of robot mechanisms, and the Lagrange equation is expressed on this basis. Finally, as an example, the proposed modeling method is utilized to establish the kinematic and dynamic models of the manipulator developed in our laboratory, and the model solution and kinematic and dynamic analyses are carried out. The conclusions are as follows:(1)
Based on the Lie group theory, this paper proposes a method for establishing and calculating the kinematic and dynamic models of robots and is especially suitable for robots with complex mechanisms. Establishing the model in the form of the Lie group and Lie algebra can obtain clear physical meaning and make the expressions uniform and easy to program, which is convenient for computer-aided calculation and parameterization. Calculating in the form of the Lie group and Lie algebra and by their properties can avoid a large number of trigonometric functions in the middle calculation process, which reduce the calculation and model complexity, especially for calculating the velocity and acceleration. And then, the calculation error is decreased and the calculation difficulty is reduced.(2)
The kinematic and dynamic models of the manipulator developed in our laboratory are established and their working process is numerically calculated with the method proposed in this paper. Then, the motion analysis of the manipulator is conducted according to the calculation results. The results show the following. The maximum values of the manipulator components’ kinematic parameters and the hydraulic cylinder’s output force are all within the allowable range. Combined with the engineering practice, analyzing the variation trends of the curves of the kinematic and dynamic parameters shows that the manipulator has good motion characteristics in this working process and can meet the working requirements. The tongs at the end of the manipulator can reach the prescribed positions and its motion characteristics can meet the working requirements. The manipulator can complete the prescribed task.



---

*Source: 6014256-2021-10-19.xml*