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

,e 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. ,e 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. ,e 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. ,erefore, 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. ,en, the results of the numerical calculation are compared with the results of virtual prototype simulation in ADAMS to verify the correctness.


Introduction
Industrial robots are usually composed of the base, the endeffector, 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. e 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]. e hybrid robot combines the advantages of the serial robot and the parallel robot [2][3][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. erefore, it greatly increases the difficulty of modeling and calculating the kinematic and dynamic models of their mechanisms. e 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. e 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 closedform solution.
is 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. e basic operation used is Lie bracket. is 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. en, 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. e 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.
e 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. ere is a mapping relationship between the Lie group and Lie algebra. erefore, 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. en, 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. en, we propose a kinematic modeling method to describe the pose of rigid bodies in the multi rigid body system with the Lie group. e pose of the component in the robot mechanism is expressed by the Euclidean group and the displacement subgroup. e 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. e space velocity Lie algebra is derived from the body velocity. e 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. erefore, 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. e 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. e numerical calculation results provide a theoretical basis for the subsequent motion analysis of the manipulator. is paper is organized as follows. In Section 2, 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.

The Description of Rigid Body Motion in the
Framework of the Lie Group 2.1. e Basic eory of the Lie Group and Lie Algebra. Lie group G is a differential manifold. Let g ∈ G, then there is the following [14]: (1) e binary operation mapping of the Lie group φ: G × G ⟶ G, φ(g 1 , g 2 ) � g 1 ∘ g 2 , g 1 and g 2 are all C ∞., . (2) e inverse operation mapping of the Lie group τ: G ⟶ G, τ(g) � g − 1 and g are both C ∞ . (3) e Lie group satisfies law of association (4) e Lie group satisfies identity element law. ere is a unique identity element e which makes g ∘ e � e ∘ g � g.
As for ∀g ∈ G, set up T g G as the tangent space at g. e Lie algebra is defined as the tangent space at the identity element, that is, T e G.
e Lie algebra is isomorphic to R k through the invertible linear mapping. ere is (1) e left invariant translation mapping of the Lie group at the identity element is (2) en, the derivative of this mapping defines the diffeomorphism from T e G to T g G. ere is (3) e derivative of the right invariant translation mapping of the Lie group at the identity element is e Lie group G acting on its Lie algebra T e G is called the adjoint transformation of Lie algebra, if equation (5) is satisfied.

e Rotating Motion of Rigid
Body. e rotating motion of rigid body in the 3D space is described by the rotation matrix R. R ∈ SO(3) and SO(3) � R ∈ R 3×3 : RR T � R T R � E, det R � 1} belongs to the 3D rotation group. Its group operation is matrix multiplication, that is, is the corresponding Lie algebra of SO(3). Its concrete form is antisymmetric matrix space ω ∈ so(3) � s ∈ R 3×3 : s T � −s . And, it is recorded as 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. e antisymmetric matrix has the property ωr � ω × r. en, there is ω× � ω and r is a 3-dimensional column vector.
According to (1), define Mathematical Problems in Engineering e adjoint transformation of the Lie algebra so(3) is where R � According to (3), we obtain where ω B is the angular velocity of the rigid body in the body fixed coordinate system. According to (4), we obtain where ω S is the angular velocity of the rigid body in the inertial coordinate system. e relationship between the two angular velocities is As for elements in so(3), the adjoint transformation can describe the velocity transformation under the coordinate system transformation.
ere is a one to one exponential mapping relationship between SO(3) and so(3). e exponential mapping relationship is

e Translational Motion of Rigid
Body. e translational motion of rigid body in the 3D space is described by the translation matrix p.p ∈ T(3) and T(3) � p ∈ R 3 belongs to the 3D translation group. Its group operation is matrix addition, that is, p 1 ∘ p 2 � p 1 + p 2 . e translation velocity vector is _ p.

e General Motion of Rigid
Body. e general motion of rigid body in the 3D space is described by the affine mapping g(r) � Rr + p. All of these affine mappings form a six-dimensional Lie group, which is called the Euclidean group SE(3). e Euclidean group SE(3) is the semidirect product of the 3D rotation group SO(3) and the 3D translation group T(3), that is, SE(3) � SO(3) × T(3) which is abbreviated and recorded as (R, p). Its homogeneous matrix form is formula (12). e group operation is matrix multiplication, that is, g 1 ∘ g 2 � g 1 g 2 .
se (3) is the corresponding Lie algebra of SE(3), and its concrete form is the screw space ξ ∈ se(3) � (ω, v): ω ∈ so(3), v ∈ R 3 . It is written in the form of matrix as e adjoint transformation of the Lie algebra se (3) is It is written in the form of six-dimensional vector as According to (3), we obtain ξ 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 ξ 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. e relationship between ξ B and ξ S is expressed in the following equations: As for elements in se(3), the adjoint transformation can describe the velocity transformation under the coordinate system transformation.
ere is a one to one exponential mapping relationship between SE(3) and se(3). e exponential mapping relationship is

SO(3) and T(3) are the Lie subgroups of SE(3). SE(3)
and its Lie subgroups are related to the displacement of rigid body and they are called the displacement subgroup.

e 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. e 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. e 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. us, the pose of the component can be represented by a translation group and two rotation groups. e proposed kinematic modeling method of robot mechanism in the Lie group framework consists of the following. e defined numbers of the components, joints, and joint systems are shown in Figure 1.
(1) Label robot's components sequentially from the base that was marked as 0 to the end-effector that was marked as n. 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 the z 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. en, the joint coordinate system i is fixed to the component i.
(3) Transformation steps between the adjacent joint coordinate systems are as follows: (A) e coordinate system i − 1 is translated along x i−1 ,y i−1 and z i−1 directions, respectively, to make the origin of it coincide with that of the coordinate system i. e coordinate system 1 is obtained. (B) e coordinate system 1 is rotated around the axis x 1 to make the axis z 1 coincide with the joint axis. e coordinate system 2 is obtained. (C) e coordinate system 2 is rotated around the axis z 2 to make the axis x 1 coincide with the axis x 1 . e coordinate system i is obtained. e transformation described above is expressed in the form of Lie group as e right subscript of the Lie algebra i ξ i represents the component number, and the left superscript of it represents the coordinate system number that describes the motion.

Derivation of Kinematic Parameter Formula of Robot
Mechanism in Lie Group Framework. Set up that, the position vector is expressed as i r p , 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 i r p . e body velocity of component 1 relative to the inertial coordinate system is Figure 1: e notation and joint coordinate system defined by the proposed modeling method.
Mathematical Problems in Engineering 5 (23) e body velocity of component i relative to the coor- e body velocity of component i relative to the inertial coordinate system is Equations (23)-(25) are body velocity's recursive formulas of robot mechanism in the Lie group framework. e body velocity is used to derive the space velocity and the dynamic equations. e 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. e space velocity of component i relative to the inertial coordinate system is en, the rotational angular velocity of component i in the inertial coordinate system is en, the rotational angular acceleration of component i in the inertial coordinate system is e linear velocity of any point p on component i in the inertial coordinate system is e acceleration of any point p on component i in the inertial coordinate system is e Jacobian matrix formula of robot mechanism is derived as follows.
Let the rotation velocity of each joint of the robot be _ θ � [ _ θ 1 , _ θ 2 , . . . , _ θ n ] T and the position of the reference point on the robot end-effector in the inertial coordinate system be 0 r � [ 0 x, 0 y, 0 z] T . e linear velocity of the reference point on the endeffector in the inertial coordinate system is 0 v, and it is expressed as So, the displacement part of Jacobian matrix of the robot is e angular velocity Lie algebra of the reference point on the end-effector in the inertial coordinate system is 0 ω, and it is expressed as en, the angular velocity of the reference point on the end-effector in the inertial coordinate system is So, the rotation part of Jacobian matrix of the robot is

Lagrange Equation in the Framework of the Lie Group
Let the robot mechanism be an n-DOF multi rigid body system. E k is the total kinetic energy of the robot, E p is the total potential energy of the robot, q i is the generalized coordinate of the robot, _ q i is the generalized velocity of the robot, F i is the nonpotential generalized force corresponding to the generalized coordinate q i . en, the Lagrange equation of the n-DOF robot mechanism is d dt According to Konig's theorem, the total kinetic energy of the robot is 6 Mathematical Problems in Engineering e total potential energy of the robot is ic is the linear velocity of the centroid of component i in the coordinate system i, 0 i ω B is the angular velocity of component i in the coordinate system i, J i 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 0 r ic is the homogeneous coordinate of the centroid of component i in the inertial coordinate system. e linear velocity of the centroid of component i in the coordinate system i is expressed by the body velocity of component i as where i r ic is the position vector of the centroid of component i in the coordinate system i. e angular velocity of component i in the coordinate system i is expressed by the body velocity of component i as

The Kinematic Model of the Manipulator
e 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 Figure 2. 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. e pin shafts and the rotary bearing can be seen as rotation pairs and the hydraulic cylinders can be seen as the motion pairs. e 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.
Observing the manipulator's structure characteristic, it is found that the manipulator belongs to the closed chain cascade robot [15,16].
is 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.
e 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. e 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.

e Kinematic Model of the Manipulator's Main Kinematic Chain.
e 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.
e pose matrix of the joint coordinate system o 1 x 1 y 1 z 1 relative to the inertial coordinate system o 0 x 0 y 0 z 0 is e pose matrix of the joint coordinate system o 2 x 2 y 2 z 2 relative to o 1 x 1 y 1 z 1 is e pose matrix of the joint coordinate system o 3 x 3 y 3 z 3 relative to o 2 x 2 y 2 z 2 is e pose matrix of the joint coordinate system e pose matrix of the joint coordinate system o 5 x 5 y 5 z 5 relative to o 4 x 4 y 4 z 4 is Mathematical Problems in Engineering 7 e pose matrix of the joint coordinate system o 6 x 6 y 6 z 6 relative to o 5 x 5 y 5 z 5 is en, the pose of each component relative to the inertial coordinate system is expressed as follows. e pose of the component 2 relative to the inertial e pose of the component 3 relative to the inertial e pose of the component 4 relative to the inertial coordinate system o 0 x 0 y 0 z 0 is where there is (the detailed derivation process is in Appendix A and this is adopted in the same condition later.)   Mathematical Problems in Engineering 9 e pose of the component 5 relative to the inertial where there is e pose of the component 6 relative to the inertial coordinate system o 0 x 0 y 0 z 0 is where there is Let the H-point on the fourth manipulator arm shown in Figure 3 represent the position of the end tongs. en, the homogeneous coordinate of the H-point in the inertial coordinate system is 0 r H � 0 6 g 6 r H . According to (23) and (25), the body velocities of components of the manipulator's main kinematic chain are where According to Figure 3(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 Obviously, the body velocity of the telescopic arm relative to the fourth manipulator arm is 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 en, according to (27) and (28), the angular velocity and the angular acceleration of the rotary bearing in the inertial coordinate system are (59) 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) e 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.
In equation (62), items are calculated as follows: where there is s z �

Kinematic Model of the Manipulator's Branch Kinematic
Chains. e 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. e kinematic model of the branch kinematic chain is used as the constraint equations in the kinematic model of the manipulator. Figure 4 shows the first branch kinematic chain and its mechanism diagram. e kinematic model of the first branch kinematic chain is established as follows.
e pose matrix of the joint coordinate system o 7 x 7 y 7 z 7 relative to o 1 x 1 y 1 z 1 is From the geometric relationship of the closed loop BIJ, we obtain (65) Figure 5 shows the second branch kinematic chain and its mechanism diagram. e kinematic model of the second branch kinematic chain is established as follows.
e pose matrix of the joint coordinate system o 8 Mathematical Problems in Engineering 13 e pose matrix of the joint coordinate system o 9 x 9 y 9 z 9 relative to o 8 x 8 y 8 z 8 is e pose matrix of the joint coordinate system o 10 x 10 y 10 z 10 relative to o 8 x 8 y 8 z 8 is From the geometric relationship of the closed loop BKLN, we obtain From the geometric relationship of the closed loop BKMC, we obtain 2 3 g 3 r M � 2 8 g 8 10 g 10 r M .
(70) Figure 6 shows the third branch kinematic chain and its mechanism diagram. e kinematic model of the third branch kinematic chain is established as follows.
e pose matrix of the joint coordinate system e pose matrix of the joint coordinate system o 12 x 12 y 12 z 12 relative to o 11 x 11 y 11 z 11 is 11 12 e pose matrix of the joint coordinate system o 13 x 13 y 13 z 13 relative to o 11 x 11 y 11 z 11 is 11 13 From the geometric relationship of the closed loop COPQ, we obtain 3 r Q � 3 11 g 11 12 g 12 r Q .
From the geometric relationship of the closed loop CORD, we obtain 3 4 g 4 r R � 3 11 g 11 13 g 13 r R .
(75) Figure 7 shows the fourth branch kinematic chain and its mechanism diagram. e kinematic model of the fourth branch kinematic chain is established as follows.
e pose matrix of the joint coordinate system o 14 x 14 y 14 z 14 relative to o 5 x 5 y 5 z 5 is From the geometric relationship of the closed loop FSTV, we obtain Let the lengths of hydraulic cylinders be l 1 mm, l 2 mm, l 3 mm, and l 4 mm. en, related structural parameters are where k can be 7 to 16.

Dynamic Model of the Manipulator
Analyzing the structure of the manipulator, it can be seen that the manipulator has six degrees of freedom. ey 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 , whereE k is the total kinetic energy of the manipulator and E p is the total potential energy of the manipulator. According to equation (37), the total kinetic energy of the manipulator is calculated as follows.
e total kinetic energy of the base, the four manipulator arms, the three telescopic arms, and the connecting rod 6 is e total kinetic energy of the four hydraulic cylinders is e connecting rods 1, 2, 3, 4, and 5 are all in pairs, and the total kinetic energy of them is 10,12,13,14).
(86) en, the total kinetic energy of the manipulator is According to equation (38), the total potential energy of the manipulator is E p � 6 i�1 m i g 0 r ic + m 7(tube) g 0 r 7(tube)c + m 7(rod) g 0 r 7(rod)c + m 8(tube) g 0 r 8(tube)c + m 8(rod) g 0 r 8(rod)c + m 11(tube) g 0 r 11(tube)c + m 11(rod) g 0 r 11(rod)c + m 16(tube) g 0 r 16(tube)c + m 16(rod) g 0 r 16(rod)c + 2m 9 g 0 r 9c + 2m 10 g 0 r 10c + 2m 12 g 0 r 12c + 2m 13 g 0 r 13c + 2m 14 g 0 r 14c + m 15 g 0 r 15c , e 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. e parameters of the first hydraulic cylinder are calculated as follows. e 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.
e linear velocity of the centroid of the first hydraulic cylinder's cylinder tube in the coordinate system 7 is When analyzing the velocity of the centroid of the first hydraulic cylinder's piston rod, the composition theorem of rigid body motion is used. e linear velocity of the centroid of the first hydraulic cylinder's piston rod is expressed in coordinate system 7 as e rotation angular velocity of the first hydraulic cylinder in the coordinate system 7 is 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. e relationship between the driving force applied by the first hydraulic cylinder on the first manipulator arm and the generalized force M 2 is where 2 M 2 is the moment-vector of generalized force M 2 in the coordinate system 2 and 2 F 1 is the driving force applied by the first hydraulic cylinder on the first manipulator arm in the coordinate system 2. e drive of the second hydraulic cylinder to the second manipulator arm is transmitted through connecting rod 1. According to Figure 8(b), we can see that the component force F 9 along connecting rod 1 and the component force F 10 along connecting rod 2 are obtained through decomposing the driving force of the second hydraulic cylinder on its action point. eir relationship is where 8 F 9 is the push force applied by the second hydraulic cylinder on the connecting rod 1 in the coordinate system 8, 8 F 10 is the push force applied by the second hydraulic cylinder on the connecting rod 2 in the coordinate system 8, and 8 F 2 is the total force of the second hydraulic cylinder in coordinate system 8. According to Figure 8(b), the relationship of the F 9 and the generalized force M 3 is where 3 M 3 is the moment-vector of generalized force M 3 in the coordinate system 3 and 3 F 9 is the push force applied by the connecting rod 1 on the second manipulator arm in the coordinate system 3. e 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. eir force analysis diagrams are Figures 8(c) and 8(d). e calculation process is not explained in detail here. And the equations describe their relationships are shown in Appendix E. 16 Mathematical Problems in Engineering (d) Figure 8: e driving force of each hydraulic cylinder and its corresponding generalized force. e 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.

Kinematic and Dynamic Analysis of the Manipulator e initial configuration of the manipulator is
e 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 According to the structural dimension of the manipulator and the known conditions, assign the parameters in the established mathematical model. en, the numerical solution is conducted. e numerical solution path of the manipulator's kinematic and dynamic models is shown in Table 1.
e variation curves of kinematic parameters of the manipulator in its working process are shown in Figures  9-14.
In Figure 9(a), θ 1 , θ 2 , θ 3 , θ 4 , and θ 6 are joint angles of the slewing bearing and the first to the fourth manipulator arms, respectively, θ 5 is the joint angle of the telescopic arm which equals 0 because of its prismatic pair essence. e 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. erefore, 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. is 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. e 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 Figure 10(a), l 1 , l 2 , l 3 , l 4 , and l 5 are lengths of the first to the fifth hydraulic cylinder, respectively. e 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. e 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. e 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. e 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. is 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. e 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. e 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 Figure 11, 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. e maximum velocity of the mass center of the first manipulator arm is about 250 mm/s and occurs at the time 70 s. e velocity changes abruptly at the times 60 s and 70 s, and the abrupt change in the x-axis direction is relatively large. e 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. is 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. e y-direction velocity is 0 at other times, which is in accordance with the prescribed trajectory. Analyzing Figure 11(b), conclusions are as follows. e 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 Figure 12, 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 In Figure 13, 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 In Figure 14, 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      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. e 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. e 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. e 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. ese 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. rough 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. e acceleration's theoretical value reaches maximum at about 1300 mm/s 2 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. e structure can support such impact.
In Figure 16, F 1 , F 2 , F 3 , and F 5 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. e first hydraulic cylinder only provides push force before the e second hydraulic cylinder always provides push force. e 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. e 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. e 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. e 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.
Analyzing the above results, conclusions can be obtained as follows:

24
Mathematical Problems in Engineering  (1) Most of the time, the kinematic curves and force curves are continuous and smooth, which shows good motion characteristics. e 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) e maximum kinematic parameters of each manipulator's component and the maximum output force of each hydraulic cylinder are all within the allowable range. e length range of the hydraulic cylinder is within its stroke range.
(3) e 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. e 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) e 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. e manipulator can complete the prescribed task. e manipulator's virtual prototype simulation is carried out in ADAMS. e results of the virtual prototype simulation are compared with those of the numerical calculation and they are basically consistent, which is shown in Figures  17 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. e 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. e 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

28
Mathematical Problems in Engineering 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%. e 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. e reason for the large gap at the state mutation times is the same as Figure 17(b).

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. en, 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. e 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) e 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. en, the motion analysis of the manipulator is conducted according to the calculation results. e results show the following. e 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. e tongs at the end of the manipulator can reach the prescribed positions and its motion characteristics can meet the working requirements. e manipulator can complete the prescribed task.
According to equation (11), there is where ω z � So, there is e linear velocity and the acceleration of the centroid of the first manipulator arm in the inertial coordinate system are (B.5) e linear velocity and the acceleration of the centroid of the second manipulator arm in the inertial coordinate system are (B.8) e linear velocity and the acceleration of the centroid of the third manipulator arm in the inertial coordinate system are (B.11) e linear velocity and the acceleration of the centroid of the fourth manipulator arm in the inertial coordinate system are e homogeneous coordinate of the centroid of the first manipulator arm in the inertial coordinate system is e linear velocity of the centroid of the first manipulator arm in the coordinate system 2 is e rotating angular velocity of the first manipulator arm in the coordinate system 2 is e homogeneous coordinate of the centroid of the second manipulator arm in the inertial coordinate system is e linear velocity of the centroid of the second manipulator arm in the coordinate system 3 is e rotating angular velocity of the second manipulator arm in the coordinate system 3 is e homogeneous coordinate of the centroid of the total three telescopic arms in the inertial system is 0 r 5c � 0 5 g 5 r 5c . (C. 16) e linear velocity of the centroid of the total three telescopic arms in the coordinate system 5 is