# Kinematic Analysis of the Robot Having Closed Chain Mechanisms Based on an Improved Modeling Method and Lie Group Theory

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

---

## Abstract

The robot kinematic model is the basis of motion control, calibration, error analysis, etc. Considering these factors, the kinematic model needs to meet the requirements of completeness, model continuity, and minimality. DH model as the most widely used method to build robot kinematic model still has problems in completeness, model continuity, and calculation, especially for robots with complex mechanisms such as closed chain mechanism and branch mechanism. In this paper, an improved kinematic modeling method is proposed based on the cooperation of the DH model and the Hayati and Mirmirani model and considering the Lie group concept. The improved model is complete and continuous, and when combining with Lie group to calculate, it avoids numbers of trigonometric functions and antitrigonometric functions in the process so as to optimize the algorithm. With this method, the kinematic model of the closed chain cascade manipulator developed in our laboratory is established, and a working process of it is numerically calculated. The results of the numerical calculation are basically consistent with those of virtual prototype simulation, which means the established kinematic model is correct and the numerical calculation method can solve the problem correctly. The kinematic model and the results of the kinematic analysis provide a theoretical basis for the subsequent motion control, calibration, and error analysis of the robot.


---

## Body

## 1. Introduction

Industrial robots are usually composed of the base, the end-effector, and several links connected by joints. The pose of the end-effector of the robot is determined by the angle or displacement of each joint. Robot kinematic model is to establish the relationship between the pose of the end-effector and the angles or displacements of joints which includes the position and velocity equations. The kinematic model is an important factor affecting the accuracy of robot motion, and it is also the basis of the follow-up calibration, error analysis, robot control [1–3], path planning [4], dynamic analysis, etc.In practical application, due to manufacturing error, assembly error, and other factors, there is often a gap between the actual kinematic parameters of the robot and the design kinematic parameters. So, in many cases, it is necessary to conduct calibration and error analysis on the robot’s kinematic parameters. Considering these factors, a qualified kinematic model is required to meet the requirements of completeness, model continuity, and minimality [5]. Completeness refers to that the kinematic model can adjust the joint variables to make the actuator reach the required position and orientation for any inertial coordinate system and any zero position of any position and orientation. Model continuity refers to the continuous change of model parameters caused by the continuous change of any joint, and there is no singular point in the model. Minimality means that the kinematic model contains only a minimal number of parameters.There are many methods to build a robot kinematic model such as vector method, quaternion method, exponential function method, homogeneous matrix method, and so on. Among them, the vector method is the most basic, but the modeling process is tedious and not general which is often used for simple or basic mechanism modeling. The quaternion method uses redundant four parameters to represent the attitude which can describe the attitude changing process with continuous parameters, reduce the number of equations for each target position, and overcome the singularity problem. However, due to its abstraction and other factors, it has not been widely used. The exponential function method is based on the screw theory. In the process of modeling, only the coordinate systems on the base and the target object need to be established, and the description of the component pose is relatively simple. It has become an effective alternative to the traditional vector method and the DH method. But it cannot be directly used when modeling the mobile base robot [6, 7]. Currently, the most widely used method for establishing robot kinematic model is the DH method based on the homogeneous transformation matrix. This method is to establish coordinate systems at the moving or rotating joints of the robot and then carry out corresponding coordinate transformations to obtain the joint transformation parameter table. Each transformation matrix is composed of four joint transformation parameters.Denavit and Hartenberg proposed the DH method in 1955 [8]. Since then, the DH method has been widely used in the field of robot because of its clear physical meaning and easy programming. In 1986, Craig [9] and Khalil and Kleinfinger [10] adjusted the DH method. Based on the classical DH method, the joint coordinate system is moved from the distal end of the link to the proximal end. Lipkin discussed the two forms of the DH method in detail [11]. According to the basic requirements of the kinematic model mentioned above, the DH method is neither complete nor continuous. The main limitations in practical application are as follows:(1)
When the adjacent joint axes are parallel or nearly parallel, the slight change of joint axis direction will lead to the great change of the pose of the common normal line between adjacent axes. Thus, it will result in the discontinuity of DH parameters.(2)
The origin of the link coordinate system cannot be placed at the required position and may be far away from the link. This will cause difficulties in obtaining DH parameters and limitations in error analysis [12]. This will also cause the base coordinate system and the end-effector coordinate system not arranged arbitrarily, so the kinematic model is incomplete [13].(3)
DH method is based on a homogeneous transformation matrix, and its possible singular points of trigonometric function will lead to no solution of the kinematic model. Especially, when establishing a kinematic model of the robots including complex mechanisms such as closed chain and branch mechanisms and very complex expression of trigonometric functions and antitrigonometric functions will arise. This makes the calculation extremely difficult.Aiming at the limitations of the DH method, many scholars have made improvements to it.On the issue that the kinematic model is of discontinuity when modeling the parallel adjacent joints with the DH method, the common method is to add the rotation transformation to describe the small angle change of joint pose. Hayati and Mirmirani [14] proposed a 4-parameter model in 1985. When the adjacent axes are parallel, the rotation terms around the X and Y axes in the model can describe the slight deflection in the axis direction, thus solving the problem of DH parameter discontinuity. However, it is found that when the two adjacent joints are vertical or nearly vertical, this model has the problem of parameter discontinuity. Kim et al. [15] proposed a 4-parameter model which is essentially the same as Hayati and Mirmirani model. Veitschegger [16–18] and Okada and Mohri [19] put forward a 5-parameter model. For this model, an extra rotation transformation is needed to compensate for deflection in parallel or near parallel joints [16, 17]. Even when the consecutive joints are not parallel, the inclusion of this extra rotation term is convenient for the follow-up error analysis [16, 17]. However, this model has a sensitivity problem when the neighboring coordinate origins are close together [16]. Guo et al. [20] and Huang et al. [21] also used a similar method to solve the problem. Zhuang et al. [13] proposed a 6-parameter model called CPC model which adopts Roberts line representation to ensure the continuity of model parameters and adds two parameters based on Roberts's linear representation to ensure the completeness of the model. Thereafter, Zhuang et al. [22] improved the CPC model. The new model is called the MCPC model. This model is closer to the DH model and more user-friendly.On the issue that the position of coordinate system origin is limited when modeling with the DH method, increasing the number of parameters in the model is a common method. Hsu and Everett [23] proposed a 5-parameter model whose essence is to multiply a translation transformation along the z-axis on the Hayati and Mirmirani model so that the origin of the link coordinate system can be more flexibly arranged at the required position. Stone and Sanderson [24] proposed the S-model, which contains six independent parameters. This model adds a rotation term around the z-axis and a movement term along the z-axis in front of the DH equation. The conversion between the two models is convenient. The model allows the origin of the joint coordinate system to be placed at any position of the joint axis and can rotate any angle around the z-axis [24]. Lin and Chen [25, 26] proposed a 5-parameter model which considered that to completely determine the pose of the joint axis, it needs 3 movement parameters to determine the position and 2 angle parameters to determine the direction. The model can place the origin of the joint coordinate system at any desired position.As for solving the problem of calculating the kinematic model established by the DH method, the Lie group theory is adopted in this paper. In recent years, with the development of computational geometry mechanics, the Lie group theory is more and more applied to the kinematic analysis and dynamic analysis of the space rigid body. It is found that when the concept of Lie group and Lie algebra is introduced into the kinematic model based on the homogeneous transformation matrix, the rotation part of the homogeneous transformation matrix is regarded as the three-dimensional rotating group of the Lie group, and the angular velocity corresponding to the rotation part is regarded as the Lie algebra. So, the original homogeneous matrix can be replaced by an orthogonal matrix and the vectors to avoid large numbers of trigonometric functions and antitrigonometric functions in the calculating process. There is an exponential mapping relationship between the Lie group and the rotation angle, and a one-to-one mapping relationship between the Lie group and the Lie algebra. It will greatly reduce the difficulty of kinematic model calculation, especially the difficulty of the velocity solution. Lee [27, 28] combined the theory of Lie group with the discrete variational integrator and obtained the Lie group discrete variational integrator. This method is used in the dynamics research of the three-dimensional rigid pendulum. Bai et al. [29] used the Lie group theory to conduct the kinematics, dynamics, and optimal control analysis of pneumatic hexapod robot. Ding and Liu [30] used the Lie group and Lie algebra in the dynamics research of the flexibility rod robot. Müller [31], based on the screw theory and Lie group theory, put forward the compact form kinematic equations of the tree-topology multibody system. It is in terms of relative joint coordinate and under a uniform multibody system framework. Based on the Lie group theory, Chen et al. [32] studied the freedom and the bifurcation characteristic of the parallel mechanism through the analysis of the displacement manifolds generated by the branch kinematic chain and the intersection of all branch displacement manifolds.In this paper, a new kinematic modeling method is proposed based on the cooperation of the DH model and the Hayati and Mirmirani model and considering the Lie group concept. Because increasing the number of model parameters can solve the problem of limitation of coordinate system origin position, this paper changes two translation transformations in the Craig’s DH model to three while the rotation transformation is unchanged. Then, combining the concept of a Lie group, the transformation order in the model is changed to ease the follow-up calculation. The improved model is still discontinuous when the adjacent joints are parallel. So, in this paper, Hayati and Mirmirani model with an added translation parameter is used to model parallel joints. It can be seen from the analysis that the kinematic model proposed in this paper is complete and model continuity and contains 5 independent parameters, which is the least compared with the model in the same condition. With this method, the kinematic model of the closed chain cascade manipulator developed in our laboratory is established, and a working process of it is numerically calculated with the Lie group theory. The results of the numerical calculation are basically consistent with those of virtual prototype simulation, which means the established kinematic model is correct, and the numerical calculation method can solve the problem correctly. The kinematic model and the results of the kinematic analysis provide a theoretical basis for the subsequent motion control, calibration, and error analysis of the robot.This paper is organized as follows. In Section2, the basic theory of the Lie group is introduced. Section 3 introduces the improved kinematic modeling method which is both complete and continuous. Firstly, the modeling method and its expression of the homogeneous matrix are introduced. Then, the completeness and continuity of the model are analyzed both from the theory and the geometry aspects. In Section 4, the kinematic model of the closed chain cascade manipulator developed by our laboratory is built. In Section 5, the kinematic analysis of the closed chain cascade manipulator is carried out. In Section 6, the conclusions of this paper are proposed. Section 7 puts forward that future work should be researched on the corresponding calibration algorithm and error model.


## 2. The Basic Theory of Lie Group

The rotation transformation of rigid body in Euclidean space is represented by the rotation matrixR. R∈SO3, where SO3 is the three-dimensional rotation group in Lie group.(1)SO3=R∈R3×3:RRT=RTR=I,detR=1,where I is the 3 × 3 identity matrix.The translation transformation of a rigid body in Euclidean space is represented by the translation matrixp. p∈R3.The pose transformation of a rigid body in Euclidean space is represented by the homogeneous transformation matrixA. A∈SE3, where SE3is the Euclidean group in the Lie group.(2)SE3=p,R:p∈R3,R∈SO3=R3×SO3.The cross product is a linear operator. For two vectorsa,b∈R3, define(3)a∧=0−a3a2a30−a1−a2a10.There is(4)a×b=a∧b.The symbola^ will be used instead of symbol a∧ in the later part.There is(5)A=Rp01=expξ^θ=expω^θI−expω^θω×v+ωωTvθ01,R=expω^θ=I+θω^+θ22ω^2+θ33ω^3+⋯,R˙=ω^R,ω^=0−ω3ω2ω30−ω1−ω2ω10∈so3 represents the instantaneous angular velocity of the rigid body relative to the reference coordinate system. ω1 is the angular velocity component in the x-axis, ω2 is the angular velocity component in the y-axis, ω3 is the angular velocity component in the z-axis. so3 is the corresponding Lie algebra of SO3, so3=s∈R3×3:sT=−s is the vector space of 3 × 3 antisymmetric matrix. ξ^=ω^v00∈R4×4∈se3, se3 is the corresponding Lie algebra of SE3, se3=v,ω^:v∈R3,ω^∈so3 is the screw space.


## 3. The Improved Kinematic Modeling Method

The DH method only contains four parameters. The origin of the joint coordinate system is determined by the intersection of the common normal line and joint axis, so the origin position of the joint coordinate system cannot be changed. To determine the position and pose of the coordinate system, five parameters are needed. Generally, three translation parameters are used to determine the position of the coordinate system and two rotation parameters are used to determine the attitude of the coordinate system. In this paper, a translation parameter is added to the DH model proposed by Craig [9] and Khalil and Kleinfinger [10], and the joint coordinate system is embedded to the link to represent the pose of the link. Furthermore, to ease the calculation, the transformation order in the model is changed according to the Lie group concept. The translation transformations and rotation transformations are put together, respectively. As for the problem of model discontinuity when adjacent joints are parallel, this paper is based on Hayati and Mirmirani model to solve this problem. In the same way as the DH method, Hayati and Mirmirani model also contains only four parameters. So, this paper adds a translation transformation to the original model. The joint coordinate system is moved to the proximal end of the link and is embedded with the link. Furthermore, the transformation order of the Hayati and Mirmirani model is changed to incorporate the modified DH method. Schröer et al. [5] put forward a detailed theoretical analysis method for judging whether the kinematic model meets the requirement of completeness, continuity, and minimality.


### 3.1. The Modeling Method When Adjacent Joints Are Not Parallel

The modeling method when adjacent joints are not parallel consists of the following steps. The notation of joints, links, and joint coordinate systems are shown in Figure1.(1)
Number links and joints. Numbering the links sequentially, commencing with the base, identified as 0, and terminating at the end-effector, marked asn. Numbering the joints sequentially from 1 to n, joint i connects link i-1 and link i.(2)
Define the joint reference coordinate system. Place the origin of the coordinate system at the desired position on the joint axis. In the Cartesian coordinate system, theZ-axis coincides with the joint axis, the X-axis is parallel to the common normal line of the two adjacent joint axes, and the Y-axis is determined by the Cartesian right-hand rule. The joint coordinate system i is embedded in the link i.(3)
Define reference coordinate systems of the base and the end-effector. According to the actual situation, it can be defined freely on the premise of meeting the required needs and convenient calculation. When transforming from the base coordinate system to link 1, take the method of translating alongx0, y0, and z0 direction followed by the Euler angle.(4)
Transformation steps between adjacent joint coordinate systems.(A)
Conduct translations on the coordinate systemi − 1 along the direction of xi−1, yi−1, and zi−1 by the distance of a, b, and c. Make the origin of the coordinate system i − 1 coincide with the coordinate system i and the coordinate system 1 is obtained.(B)
Rotate the coordinate system 1 around the axisx1 by angle a and make the axis z1 coincide with the joint axis. Thus, the coordinate system 2 is obtained.(C)
Rotate the coordinate system 2 around the axisz2 by angle θ and make the axis x1 coincide with the axis xi. Thus, the coordinate system i is obtained.So far, the transformation from the coordinate systemi − 1 to the coordinate system i is complete. The joint rotation is described by angle θ.(5)
The homogeneous transformation matrix from the coordinate systemi − 1 to coordinate system i is determined. It is represented by the Lie group form as follows:(6)Aii−1=Transxi−1,aTransyi−1,bTranszi−1,cRotx1,αRotz2,θ=Εpxa01Epyb01Epzc01Rxα001Rzθ001=RxαRzθpxa+pyb+pzc01.Figure 1
The coordinate frame assigned by the proposed modeling method when adjacent joints are not parallel.According to [28], DpAPp is not full rank at the model discontinuity, where P represents the elementary transformation between the adjacent joint coordinate systems, p represents parameters of the elementary transformation. So, for the kinematic model, the model discontinuous point is found by detecting the zero points of detDpAPpT⋅DpAPp, considered as a function of parametersp. The set of discontinuous points of this kinematic model is obtained as (7). That is, the model is discontinuous only when the axes of adjacent joints are parallel, which is the same as that of the DH method.(7)APS=n,o,a,p∈Aa=00±1.Analyzing the model continuity from the geometry aspect is done as shown in Figure2. Joint i − 1 and joint i are parallel and the kinematic model parameters are a = L, B = 0, C = 0, α = 0. Joint i is misaligned by a small angle β owing to manufacturing and installation errors. The zi−1 and zi axis intersect some distance away from the origin of the frame i − 1 in the space. At this time, the common normal line of joint i − 1 axis and joint i axis becomes perpendicular to the plane where the two axes are located. The parameters a = L, b = 0, c = 0, α = β all meet the condition of model continuity. However, for the value θ, because the direction of the common normal line of the two axes changes by 90°, the value of θ will change by about 90°. Therefore, when two adjacent joints are parallel, the small change of the joint will lead to the big change of θ, and the kinematic model is not continuous.Figure 2
The jointi is misaligned by a small angle when adjacent joints are parallel.


### 3.2. The Modeling Method When Adjacent Joints Are Parallel

To make up for the model discontinuity in Section3.1, two modeling methods are used together to carry out kinematic modeling. In the Hayati and Mirmirani model [14], the rotation terms around the x-axis and the y-axis make the model continuous when the adjacent joint axis is parallel. This paper makes the following three improvements on the Hayati and Mirmirani model. A translation transformation along the z-axis is added to the model so that the origin of the joint coordinate system can be placed at any position, solving the problem of model completeness and error analysis limitation. The joint coordinate system is moved to the proximal end and is embedded with the link. In order to form the docking with the modeling method in Section 3.1, change the transformation order. The notation method of joints, links and joint coordinate systems are shown in Figure 3.Figure 3
The coordinate frame assigned by the modeling method when adjacent joints are parallel.This modeling method is different from the method in Section3.1 only in defining the joint coordinate system and the transformation between the adjacent joint coordinate system. The definition method of the joint coordinate system is as follows. The origin of the joint coordinate system is placed at any desired position on the joint axis. The Z-axis of the coordinate system coincides with the joint axis. Make a plane perpendicular to the joint axis through the coordinate system origin, and the plane intersects the next joint axis at a point. The X-axis of the joint coordinate system is in the direction from the origin of the coordinate system to the intersection point. The Y-axis of the joint coordinate system is determined by the Cartesian right-hand rule.The transformation method between coordinate systems of adjacent joints is as follows.(1)
Translate the coordinate systemi − 1 along xi−1 by the distance to align oi−1 with oi. Coordinate system 1 is obtained.(2)
Coordinate system 2 is obtained from rotating coordinate system 1 aroundx1 for angle α. Rotate coordinate system 2 around y2 for angle β to align z2 axis with that of joint i. Coordinate system 3 is obtained.(3)
Translate the coordinate system 3 alongz3 for the distance d to align the coordinate system origin to the required position. Coordinate system 4 is obtained.(4)
Rotate coordinate system 4 aroundz4 for angle θ to align x4 with xi. Coordinate system i is obtained.So far, the transformation from the coordinate systemi − 1 to the coordinate system i is complete. This method needs 2 translation parameters and 3 rotation parameters to describe the relative pose of the coordinate system i to i − 1. The joint rotation is described by angle θ.The homogeneous transformation matrix from the coordinate systemi − 1 to coordinate system i is determined. It is represented by the Lie group form as follows:(8)Aii−1=Transxi−1,aRotx1,αRoty2,βTransz3,dRotz4,θ=Epxa01Rxα001Ryβ001Epzd01Rzθ001=RxαRyβRzθRxαRyβpzd+pxa01.


### 3.3. The Kinematic Model Discussion

It can be seen from the above that the proposed kinematic modeling method obtained by cooperating Sections3.1 and 3.2 is continuous in any position of adjacent joints. According to [5], the number of independent parameters of a kinematic model satisfying minimum and completeness is 4r+2t+6, where r is the number of rotary joints and tis the number of prismatic joints. The number of independent parameters for the proposed method is 5r+2t+6, which meets the requirement of completeness, and the number of parameters is the least among the models that meet the same condition.This model not only solves the problem of incompleteness and discontinuity in the establishment of kinematic model with DH method but also introduces the concept of Lie group to avoid the trigonometric function form in the model, which lays a foundation for the subsequent numerical calculation and optimizes the algorithm.


## 3.1. The Modeling Method When Adjacent Joints Are Not Parallel

The modeling method when adjacent joints are not parallel consists of the following steps. The notation of joints, links, and joint coordinate systems are shown in Figure1.(1)
Number links and joints. Numbering the links sequentially, commencing with the base, identified as 0, and terminating at the end-effector, marked asn. Numbering the joints sequentially from 1 to n, joint i connects link i-1 and link i.(2)
Define the joint reference coordinate system. Place the origin of the coordinate system at the desired position on the joint axis. In the Cartesian coordinate system, theZ-axis coincides with the joint axis, the X-axis is parallel to the common normal line of the two adjacent joint axes, and the Y-axis is determined by the Cartesian right-hand rule. The joint coordinate system i is embedded in the link i.(3)
Define reference coordinate systems of the base and the end-effector. According to the actual situation, it can be defined freely on the premise of meeting the required needs and convenient calculation. When transforming from the base coordinate system to link 1, take the method of translating alongx0, y0, and z0 direction followed by the Euler angle.(4)
Transformation steps between adjacent joint coordinate systems.(A)
Conduct translations on the coordinate systemi − 1 along the direction of xi−1, yi−1, and zi−1 by the distance of a, b, and c. Make the origin of the coordinate system i − 1 coincide with the coordinate system i and the coordinate system 1 is obtained.(B)
Rotate the coordinate system 1 around the axisx1 by angle a and make the axis z1 coincide with the joint axis. Thus, the coordinate system 2 is obtained.(C)
Rotate the coordinate system 2 around the axisz2 by angle θ and make the axis x1 coincide with the axis xi. Thus, the coordinate system i is obtained.So far, the transformation from the coordinate systemi − 1 to the coordinate system i is complete. The joint rotation is described by angle θ.(5)
The homogeneous transformation matrix from the coordinate systemi − 1 to coordinate system i is determined. It is represented by the Lie group form as follows:(6)Aii−1=Transxi−1,aTransyi−1,bTranszi−1,cRotx1,αRotz2,θ=Εpxa01Epyb01Epzc01Rxα001Rzθ001=RxαRzθpxa+pyb+pzc01.Figure 1
The coordinate frame assigned by the proposed modeling method when adjacent joints are not parallel.According to [28], DpAPp is not full rank at the model discontinuity, where P represents the elementary transformation between the adjacent joint coordinate systems, p represents parameters of the elementary transformation. So, for the kinematic model, the model discontinuous point is found by detecting the zero points of detDpAPpT⋅DpAPp, considered as a function of parametersp. The set of discontinuous points of this kinematic model is obtained as (7). That is, the model is discontinuous only when the axes of adjacent joints are parallel, which is the same as that of the DH method.(7)APS=n,o,a,p∈Aa=00±1.Analyzing the model continuity from the geometry aspect is done as shown in Figure2. Joint i − 1 and joint i are parallel and the kinematic model parameters are a = L, B = 0, C = 0, α = 0. Joint i is misaligned by a small angle β owing to manufacturing and installation errors. The zi−1 and zi axis intersect some distance away from the origin of the frame i − 1 in the space. At this time, the common normal line of joint i − 1 axis and joint i axis becomes perpendicular to the plane where the two axes are located. The parameters a = L, b = 0, c = 0, α = β all meet the condition of model continuity. However, for the value θ, because the direction of the common normal line of the two axes changes by 90°, the value of θ will change by about 90°. Therefore, when two adjacent joints are parallel, the small change of the joint will lead to the big change of θ, and the kinematic model is not continuous.Figure 2
The jointi is misaligned by a small angle when adjacent joints are parallel.


## 3.2. The Modeling Method When Adjacent Joints Are Parallel

To make up for the model discontinuity in Section3.1, two modeling methods are used together to carry out kinematic modeling. In the Hayati and Mirmirani model [14], the rotation terms around the x-axis and the y-axis make the model continuous when the adjacent joint axis is parallel. This paper makes the following three improvements on the Hayati and Mirmirani model. A translation transformation along the z-axis is added to the model so that the origin of the joint coordinate system can be placed at any position, solving the problem of model completeness and error analysis limitation. The joint coordinate system is moved to the proximal end and is embedded with the link. In order to form the docking with the modeling method in Section 3.1, change the transformation order. The notation method of joints, links and joint coordinate systems are shown in Figure 3.Figure 3
The coordinate frame assigned by the modeling method when adjacent joints are parallel.This modeling method is different from the method in Section3.1 only in defining the joint coordinate system and the transformation between the adjacent joint coordinate system. The definition method of the joint coordinate system is as follows. The origin of the joint coordinate system is placed at any desired position on the joint axis. The Z-axis of the coordinate system coincides with the joint axis. Make a plane perpendicular to the joint axis through the coordinate system origin, and the plane intersects the next joint axis at a point. The X-axis of the joint coordinate system is in the direction from the origin of the coordinate system to the intersection point. The Y-axis of the joint coordinate system is determined by the Cartesian right-hand rule.The transformation method between coordinate systems of adjacent joints is as follows.(1)
Translate the coordinate systemi − 1 along xi−1 by the distance to align oi−1 with oi. Coordinate system 1 is obtained.(2)
Coordinate system 2 is obtained from rotating coordinate system 1 aroundx1 for angle α. Rotate coordinate system 2 around y2 for angle β to align z2 axis with that of joint i. Coordinate system 3 is obtained.(3)
Translate the coordinate system 3 alongz3 for the distance d to align the coordinate system origin to the required position. Coordinate system 4 is obtained.(4)
Rotate coordinate system 4 aroundz4 for angle θ to align x4 with xi. Coordinate system i is obtained.So far, the transformation from the coordinate systemi − 1 to the coordinate system i is complete. This method needs 2 translation parameters and 3 rotation parameters to describe the relative pose of the coordinate system i to i − 1. The joint rotation is described by angle θ.The homogeneous transformation matrix from the coordinate systemi − 1 to coordinate system i is determined. It is represented by the Lie group form as follows:(8)Aii−1=Transxi−1,aRotx1,αRoty2,βTransz3,dRotz4,θ=Epxa01Rxα001Ryβ001Epzd01Rzθ001=RxαRyβRzθRxαRyβpzd+pxa01.


## 3.3. The Kinematic Model Discussion

It can be seen from the above that the proposed kinematic modeling method obtained by cooperating Sections3.1 and 3.2 is continuous in any position of adjacent joints. According to [5], the number of independent parameters of a kinematic model satisfying minimum and completeness is 4r+2t+6, where r is the number of rotary joints and tis the number of prismatic joints. The number of independent parameters for the proposed method is 5r+2t+6, which meets the requirement of completeness, and the number of parameters is the least among the models that meet the same condition.This model not only solves the problem of incompleteness and discontinuity in the establishment of kinematic model with DH method but also introduces the concept of Lie group to avoid the trigonometric function form in the model, which lays a foundation for the subsequent numerical calculation and optimizes the algorithm.


## 4. Establishing Kinematic Model of the Manipulator

The robot studied in this paper consists of five mechanical arms, and its main structure is shown in Figure4. The first arm is driven by motor and the second to the fifth arm are driven by hydraulic cylinders. Analyzing its structure, we can see that the manipulator belongs to the closed chain cascade robot [33]. This kind of robot can be divided into one main kinematic chain and several branch chains. The main kinematic chain can be regarded as a series robot, and each rigid body of the main kinematic chain belongs to different closed chain mechanisms, which can be regarded as several parallel robots. In this paper, the strategy of establishing the kinematic model is to establish kinematic equations of the main kinematic chain first and then establish kinematic equations of each branch chain respectively. The kinematic equations of each branch chain are the constraint equations of the main chain equations.Figure 4
The main structure model of the manipulator.


### 4.1. Establishing Kinematic Model of the Main Kinematic Chain

The main kinematic chain of the manipulator consists of the base and five mechanical arms. The first mechanical arm rotates in the vertical direction relative to the base, so the first mechanical arm is connected to the base by a rotating pair with vertical axis. The five mechanical arms are linked by pin shafts which are treated as rotating pairs. The third and the fourth mechanical arms are all telescopic devices. Each arm has two telescopic arms inside, which can be simplified as one mechanical arm and one telescopic arm are connected by a moving pair.According to the proposed kinematic modeling method proposed in Section3, the coordinate systems 0–7 are established, as shown in Figure 5. To establish kinematic model, the designed parameter that is the nominal parameter is used without considering error. Nominally, joint 2 to joint 7 are parallel joints and joint 1 is perpendicular to others. So, in Figure 5, coordinate systems 1 and 2 are established according to Figure 1 and the nominal transformation parameter is α=90°. Other coordinate systems are established according to Figure 3, and the nominal transformation parameters are α=0° and β=0°. To facilitate the calculation, the inertial coordinate system 0 is defined as the pose of the coordinate system 1 when the manipulator is facing the wellhead. As required, the origin of each coordinate system is arranged on the plane of x0o0z0. According to the theory in Section 3, the kinematic model of the main kinematic chain is as follows.Figure 5
The coordinate distribution of different joints on the manipulator.The pose matrix of the coordinate systemo1x1y1z1 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(9)A10=Rotz,θ1=Rzθ1001.Then, the attitude and position matrixes of the first mechanical arm in the inertial coordinate system are respectively expressed as(10)R10=Rzθ1,(11)p10=0.The pose matrix of the coordinate systemo2x2y2z2 relative to coordinate system o1x1y1z1 can be expressed as follows:(12)A21=Transz,lADRotx,90°Rotz,θ2=Rx90°Rzθ2pzlAD01.The pose matrix of the coordinate systemo2x2y2z2 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(13)A20=A10A21.Then, the attitude and position matrixes of the second mechanical arm in the inertial coordinate system are respectively expressed as(14)R20=Rzθ1Rx90°Rzθ2,(15)p20=Rzθ1pzlAD.The pose matrix of the coordinate systemo3x3y3z3 relative to coordinate system o2x2y2z2 can be expressed as follows:(16)A32=Transx,lDGRotz,θ3=Rzθ3pxlDG01.The pose matrix of the coordinate systemo3x3y3z3 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(17)A30=A20A32.Then, the attitude and position matrixes of the third manipulator in the inertial coordinate system are respectively expressed as(18)R30=Rzθ1Rx90°Rzθ2Rzθ3,(19)p30=Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.According to the Lie group theory,Rzθ2 means rotating around the z2 axis by angle θ2, Rzθ3 means rotating around the z3 axis by angle θ3. The z2 axis and the z3 axis have the same direction, so thatRzθ2Rzθ3 can be seen as the third mechanical arm rotates around the z3 axis by angle θ2+θ3, shown as follows. Similar situations will not be explained in the following article.(20)R30=Rzθ1Rx90°Rzθ2+θ3.The pose matrix of the coordinate systemo4x4y4z4 relative to coordinate system o3x3y3z3 can be expressed as follows:(21)A43=Transx,lGHRotz,θ4Transx,lHI=Rzθ4Rzθ4pxlHI+pxlGH01.The pose matrix of the coordinate systemo4x4y4z4 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(22)A40=A30A43.Then, the attitude and position matrixes of the telescopic arm of the third arm in the inertial coordinate system are respectively expressed as(23)R40=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4=Rzθ1Rx90°Rzθ2+θ3+θ4,(24)p40=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4pxlHI+pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD=Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the coordinate systemo5x5y5z5 relative to coordinate system o4x4y4z4 can be expressed as follows:(25)A54=TranslIL4Rotz,θ5=Rzθ5lIL401.The pose matrix of the coordinate systemo5x5y5z5 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(26)A50=A40A54.Then, the attitude and position matrixes of the fourth arm in the inertial coordinate system are respectively expressed as(27)R50=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5,(28)p50=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4lIL4+Rzθ1Rx90°Rzθ2Rzθ3Rzθ4pxlHI+pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD=Rzθ1Rx90°Rzθ2+θ3+θ4lIL4+Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the coordinate systemo6x6y6z6 relative to coordinate system o5x5y5z5 can be expressed as follows:(29)A65=Transx,lLMRotz,θ6Transx,lMN=Rzθ6Rzθ6pxlMN+pxlLM01.The pose matrix of the coordinate systemo6x6y6z6 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(30)A60=A50A65.Then, the attitude and position matrixes of the telescopic arm of the fourth arm in the inertial coordinate system are respectively expressed as(31)R60=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6,(32)p60=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6pxlMN+Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5pxlLM+p50=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6pxlMN+Rzθ1Rx90°Rzθ2+θ3+θ4+θ5pxlLM+Rzθ1Rx90°Rzθ2+θ3+θ4lIL4+Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the coordinate systemo7x7y7z7 relative to coordinate system o6x6y6z6 can be expressed as follows:(33)A76=TranslNO6Rotz,θ7=Rzθ7lNO601.The pose matrix of the coordinate systemo7x7y7z7 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(34)A70=A60A76.Then, the attitude and position matrixes of the fifth arm in the inertial coordinate system are respectively expressed as(35)R70=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6Rzθ7=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6+θ7,(36)p70=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6lNO6+p60=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6lNO6+Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6pxlMN+Rzθ1Rx90°Rzθ2+θ3+θ4+θ5pxlMN+Rzθ1Rx90°Rzθ2+θ3+θ4lIL4+Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the fifth manipulator in the inertial coordinate system is used to determine the pose of the grab in space. Let the geometric center of the end face of the fifth manipulator be pointR, and determine the position of the grab in space by the position coordinate of point R in the inertial system. The position coordinate of R in the inertial system are(37)rR0=R70rR7+p70.


### 4.2. Establishing Kinematic Model of Branch Kinematic Chains

The four hydraulic cylinders of the manipulator are four branch chains of the robot, which form four closed chains with the main chain manipulator respectively. When establishing the kinematic model, the hydraulic cylinder can be regarded as one component, and the length of hydraulic cylinders 1, 2, 3, and 4 can be set as variablesl1, l2, l3, and l4 respectively. Nominally, joint 8, 9, 10, and 11 are parallel. According to the modeling method in Section 3, in Figure 5, the coordinate systems 8, 9, 10, and 11 are established according to Figure 3 and the transformation matrixes between the coordinate systems are obtained as follows:(38)A81=TransrAB1Rotx,90°Rotz,θ8=Rx90°Rzθ8rAB101,A92=TransrDE2Rotz,θ9=Rzθ9rDE201,A104=TransrIJ4Rotz,θ10=Rzθ10rIJ401,A116=TransrNP6Rotz,θ11=Rzθ11rNP601.The first closed chain is composed of hydraulic cylinder 1, the first and the second mechanical arms. Equation (39) can be obtained from the relation that the distance between point C and point B in coordinate system 1 is equal to the length of hydraulic cylinder 1. Equation (40) can be obtained from the relation that the position vector of point C on the second mechanical arm and point C on hydraulic cylinder 1 in coordinate system 1 is equal.(39)rC1−rB1=l1,(40)R21rC2+p21=R81rC8+p81,where r8c=l1,0,0T.According to the homogeneous transformation matrix of coordinate system 2 relative to coordinate system 1, the position vector of pointC on the second mechanical arm in coordinate system 1 can be obtained as follows:(41)rC1=R21rC2+p21.Bringing (41) into (43) and expanding it can obtain equation (42). Equation (42) shows the function relationship about the rotation of the second mechanical arm driven by the telescopic motion of the hydraulic cylinder 1.(42)rCT2rC2+2p21−rB1TR21rC2+p21−rB1Tp21−rB1=l12.Differentiating (42) can obtain equation (43), which shows the relationship between the telescopic velocity of hydraulic cylinder 1 and the rotation angular velocity of the second mechanical arm relative to the first arm.(43)p21−rB1TRx90°ω^21Rzθ2rC2=l1v1,where according to Lie group theory there is(44)ω^21=0−ω321ω221ω3210−ω121−ω221ω1210=0−θ˙20θ˙200000.According to equations (40) and (42), the relationship between the relative angle of hydraulic cylinder 1 to the first mechanical arm and the relative angle of the second mechanical arm to the first mechanical arm can be obtained.The second closed chain is composed of hydraulic cylinder 2, the second and the third mechanical arms. Equation (45) can be obtained from the relation that the distance between point E and point F in coordinate system 2 is equal to the length of hydraulic cylinder 2. Equation (46) can be obtained from the relation that the position vector of point F on the third mechanical arm and point F on hydraulic cylinder 2 in coordinate system 2 are equal.(45)rF2−rE2=l2,(46)R32rF3+p32=R92rF9+p92.According to the homogeneous transformation matrix of coordinate system 3 relative to coordinate system 2, the position vector of pointF on the third mechanical arm in coordinate system 2 can be obtained as follows:(47)rF2=R32rF3+p32.Bringing (47) into (45) and expanding it can obtain equation (48). Equation (48) shows the function relationship about the rotation of the third mechanical arm driven by the telescopic motion of the hydraulic cylinder 2.(48)rFT3rF3+2p32−rE2TR32rF3+p32−rE2Tp32−rE2=l22.Differentiating (48) can obtain equation (49), which shows the relationship between the telescopic velocity of hydraulic cylinder 2 and the rotation angular velocity of the third mechanical arm relative to the second arm.(49)p32−rE2Tω^32R32rF3=l2v2,where(50)ω^32=0−ω332ω232ω3320−ω132−ω232ω1320=0−θ˙30θ˙300000.According to equation (46) and equation (48), the relationship between the relative angle of hydraulic cylinder 2 to the second mechanical arm and the relative angle of the third mechanical arm to the second mechanical arm can be obtained.The third closed chain is composed of hydraulic cylinder 3, the third and the fourth mechanical arms. Equation (51) can be obtained from the relation that the distance between point J and point K in coordinate system 4 is equal to the length of hydraulic cylinder 3. Equation (52) can be obtained from the relation that the position vector of point K on the fourth mechanical arm and point K on hydraulic cylinder 3 in coordinate system 4 are equal.(51)rK4−rJ4=l3,(52)R54rK5+p54=R104rK10+p104.According to the homogeneous transformation matrix of coordinate system 5 relative to coordinate system 4, the position vector of pointK on the fourth mechanical arm in coordinate system 4 can be obtained as follows:(53)rK4=R54rK5+p54.Bringing (53) into (51) and expanding it can obtain equation (54). Equation (54) shows the function relationship about the rotation of the fourth mechanical arm driven by the telescopic motion of the hydraulic cylinder 3.(54)rKT5rK5+2p54−rJ4TR54rK5+p54−rJ4Tp54−rJ4=l32.Differentiating (54) can obtain equation (55), which shows the relationship between the telescopic velocity of hydraulic cylinder 3 and the rotation angular velocity of the fourth mechanical arm relative to the third arm.(55)p54−rJ4Tω^54R54rK5=l3v3,where(56)ω^54=0−ω354ω254ω3540−ω154−ω254ω1540=0−θ˙50θ˙500000.According to equation (52) and equation (54), the relationship between the relative angle of hydraulic cylinder 3 to the third mechanical arm and the relative angle of the fourth mechanical arm to the third mechanical arm can be obtained.The fourth closed chain is composed of hydraulic cylinder 4, the fourth and the fifth mechanical arms. Equation (57) can be obtained from the relation that the distance between point P and point Q in coordinate system 6 is equal to the length of hydraulic cylinder 4. Equation (58) can be obtained from the relation that the position vector of point Q on the fourth mechanical arm and point Q on hydraulic cylinder 4 in coordinate system 6 are equal.(57)rQ6−rP6=l4,(58)R76rQ7+p76=R116rQ11+p116.According to the homogeneous transformation matrix of coordinate system 7 relative to coordinate system 6, the position vector of pointQ on the fifth mechanical arm in coordinate system 6 can be obtained as follows:(59)rQ6=R76rQ7+p76.Bringing (59) into (59) and expanding it can obtain equation (60). Equation (60) shows the function relationship about the rotation of the fifth mechanical arm driven by the telescopic motion of the hydraulic cylinder 4.(60)rQT7rQ7+2p76−rP6TR76rQ7+p76−rP6Tp76−rP6=l42.Differentiating (60) can obtain equation (61), which shows the relationship between the telescopic velocity of hydraulic cylinder 4 and the rotation angular velocity of the fifth mechanical arm relative to the fourth arm.(61)p76−rP6Tω^76R76rQ7=l4v4,where(62)ω^76=0−ω376ω276ω3760−ω176−ω276ω1760=0−θ˙70θ˙700000.According to equation (58) and equation (60), the relationship between the relative angle of hydraulic cylinder 4 to the fourth mechanical arm and the relative angle of the fifth mechanical arm to the fourth mechanical arm can be obtained.


## 4.1. Establishing Kinematic Model of the Main Kinematic Chain

The main kinematic chain of the manipulator consists of the base and five mechanical arms. The first mechanical arm rotates in the vertical direction relative to the base, so the first mechanical arm is connected to the base by a rotating pair with vertical axis. The five mechanical arms are linked by pin shafts which are treated as rotating pairs. The third and the fourth mechanical arms are all telescopic devices. Each arm has two telescopic arms inside, which can be simplified as one mechanical arm and one telescopic arm are connected by a moving pair.According to the proposed kinematic modeling method proposed in Section3, the coordinate systems 0–7 are established, as shown in Figure 5. To establish kinematic model, the designed parameter that is the nominal parameter is used without considering error. Nominally, joint 2 to joint 7 are parallel joints and joint 1 is perpendicular to others. So, in Figure 5, coordinate systems 1 and 2 are established according to Figure 1 and the nominal transformation parameter is α=90°. Other coordinate systems are established according to Figure 3, and the nominal transformation parameters are α=0° and β=0°. To facilitate the calculation, the inertial coordinate system 0 is defined as the pose of the coordinate system 1 when the manipulator is facing the wellhead. As required, the origin of each coordinate system is arranged on the plane of x0o0z0. According to the theory in Section 3, the kinematic model of the main kinematic chain is as follows.Figure 5
The coordinate distribution of different joints on the manipulator.The pose matrix of the coordinate systemo1x1y1z1 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(9)A10=Rotz,θ1=Rzθ1001.Then, the attitude and position matrixes of the first mechanical arm in the inertial coordinate system are respectively expressed as(10)R10=Rzθ1,(11)p10=0.The pose matrix of the coordinate systemo2x2y2z2 relative to coordinate system o1x1y1z1 can be expressed as follows:(12)A21=Transz,lADRotx,90°Rotz,θ2=Rx90°Rzθ2pzlAD01.The pose matrix of the coordinate systemo2x2y2z2 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(13)A20=A10A21.Then, the attitude and position matrixes of the second mechanical arm in the inertial coordinate system are respectively expressed as(14)R20=Rzθ1Rx90°Rzθ2,(15)p20=Rzθ1pzlAD.The pose matrix of the coordinate systemo3x3y3z3 relative to coordinate system o2x2y2z2 can be expressed as follows:(16)A32=Transx,lDGRotz,θ3=Rzθ3pxlDG01.The pose matrix of the coordinate systemo3x3y3z3 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(17)A30=A20A32.Then, the attitude and position matrixes of the third manipulator in the inertial coordinate system are respectively expressed as(18)R30=Rzθ1Rx90°Rzθ2Rzθ3,(19)p30=Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.According to the Lie group theory,Rzθ2 means rotating around the z2 axis by angle θ2, Rzθ3 means rotating around the z3 axis by angle θ3. The z2 axis and the z3 axis have the same direction, so thatRzθ2Rzθ3 can be seen as the third mechanical arm rotates around the z3 axis by angle θ2+θ3, shown as follows. Similar situations will not be explained in the following article.(20)R30=Rzθ1Rx90°Rzθ2+θ3.The pose matrix of the coordinate systemo4x4y4z4 relative to coordinate system o3x3y3z3 can be expressed as follows:(21)A43=Transx,lGHRotz,θ4Transx,lHI=Rzθ4Rzθ4pxlHI+pxlGH01.The pose matrix of the coordinate systemo4x4y4z4 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(22)A40=A30A43.Then, the attitude and position matrixes of the telescopic arm of the third arm in the inertial coordinate system are respectively expressed as(23)R40=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4=Rzθ1Rx90°Rzθ2+θ3+θ4,(24)p40=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4pxlHI+pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD=Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the coordinate systemo5x5y5z5 relative to coordinate system o4x4y4z4 can be expressed as follows:(25)A54=TranslIL4Rotz,θ5=Rzθ5lIL401.The pose matrix of the coordinate systemo5x5y5z5 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(26)A50=A40A54.Then, the attitude and position matrixes of the fourth arm in the inertial coordinate system are respectively expressed as(27)R50=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5,(28)p50=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4lIL4+Rzθ1Rx90°Rzθ2Rzθ3Rzθ4pxlHI+pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD=Rzθ1Rx90°Rzθ2+θ3+θ4lIL4+Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the coordinate systemo6x6y6z6 relative to coordinate system o5x5y5z5 can be expressed as follows:(29)A65=Transx,lLMRotz,θ6Transx,lMN=Rzθ6Rzθ6pxlMN+pxlLM01.The pose matrix of the coordinate systemo6x6y6z6 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(30)A60=A50A65.Then, the attitude and position matrixes of the telescopic arm of the fourth arm in the inertial coordinate system are respectively expressed as(31)R60=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6,(32)p60=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6pxlMN+Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5pxlLM+p50=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6pxlMN+Rzθ1Rx90°Rzθ2+θ3+θ4+θ5pxlLM+Rzθ1Rx90°Rzθ2+θ3+θ4lIL4+Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the coordinate systemo7x7y7z7 relative to coordinate system o6x6y6z6 can be expressed as follows:(33)A76=TranslNO6Rotz,θ7=Rzθ7lNO601.The pose matrix of the coordinate systemo7x7y7z7 relative to the inertial coordinate system o0x0y0z0 can be expressed as follows:(34)A70=A60A76.Then, the attitude and position matrixes of the fifth arm in the inertial coordinate system are respectively expressed as(35)R70=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6Rzθ7=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6+θ7,(36)p70=Rzθ1Rx90°Rzθ2Rzθ3Rzθ4Rzθ5Rzθ6lNO6+p60=Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6lNO6+Rzθ1Rx90°Rzθ2+θ3+θ4+θ5+θ6pxlMN+Rzθ1Rx90°Rzθ2+θ3+θ4+θ5pxlMN+Rzθ1Rx90°Rzθ2+θ3+θ4lIL4+Rzθ1Rx90°Rzθ2+θ3+θ4pxlHI+Rzθ1Rx90°Rzθ2+θ3pxlGH+Rzθ1Rx90°Rzθ2pxlDG+Rzθ1pzlAD.The pose matrix of the fifth manipulator in the inertial coordinate system is used to determine the pose of the grab in space. Let the geometric center of the end face of the fifth manipulator be pointR, and determine the position of the grab in space by the position coordinate of point R in the inertial system. The position coordinate of R in the inertial system are(37)rR0=R70rR7+p70.


## 4.2. Establishing Kinematic Model of Branch Kinematic Chains

The four hydraulic cylinders of the manipulator are four branch chains of the robot, which form four closed chains with the main chain manipulator respectively. When establishing the kinematic model, the hydraulic cylinder can be regarded as one component, and the length of hydraulic cylinders 1, 2, 3, and 4 can be set as variablesl1, l2, l3, and l4 respectively. Nominally, joint 8, 9, 10, and 11 are parallel. According to the modeling method in Section 3, in Figure 5, the coordinate systems 8, 9, 10, and 11 are established according to Figure 3 and the transformation matrixes between the coordinate systems are obtained as follows:(38)A81=TransrAB1Rotx,90°Rotz,θ8=Rx90°Rzθ8rAB101,A92=TransrDE2Rotz,θ9=Rzθ9rDE201,A104=TransrIJ4Rotz,θ10=Rzθ10rIJ401,A116=TransrNP6Rotz,θ11=Rzθ11rNP601.The first closed chain is composed of hydraulic cylinder 1, the first and the second mechanical arms. Equation (39) can be obtained from the relation that the distance between point C and point B in coordinate system 1 is equal to the length of hydraulic cylinder 1. Equation (40) can be obtained from the relation that the position vector of point C on the second mechanical arm and point C on hydraulic cylinder 1 in coordinate system 1 is equal.(39)rC1−rB1=l1,(40)R21rC2+p21=R81rC8+p81,where r8c=l1,0,0T.According to the homogeneous transformation matrix of coordinate system 2 relative to coordinate system 1, the position vector of pointC on the second mechanical arm in coordinate system 1 can be obtained as follows:(41)rC1=R21rC2+p21.Bringing (41) into (43) and expanding it can obtain equation (42). Equation (42) shows the function relationship about the rotation of the second mechanical arm driven by the telescopic motion of the hydraulic cylinder 1.(42)rCT2rC2+2p21−rB1TR21rC2+p21−rB1Tp21−rB1=l12.Differentiating (42) can obtain equation (43), which shows the relationship between the telescopic velocity of hydraulic cylinder 1 and the rotation angular velocity of the second mechanical arm relative to the first arm.(43)p21−rB1TRx90°ω^21Rzθ2rC2=l1v1,where according to Lie group theory there is(44)ω^21=0−ω321ω221ω3210−ω121−ω221ω1210=0−θ˙20θ˙200000.According to equations (40) and (42), the relationship between the relative angle of hydraulic cylinder 1 to the first mechanical arm and the relative angle of the second mechanical arm to the first mechanical arm can be obtained.The second closed chain is composed of hydraulic cylinder 2, the second and the third mechanical arms. Equation (45) can be obtained from the relation that the distance between point E and point F in coordinate system 2 is equal to the length of hydraulic cylinder 2. Equation (46) can be obtained from the relation that the position vector of point F on the third mechanical arm and point F on hydraulic cylinder 2 in coordinate system 2 are equal.(45)rF2−rE2=l2,(46)R32rF3+p32=R92rF9+p92.According to the homogeneous transformation matrix of coordinate system 3 relative to coordinate system 2, the position vector of pointF on the third mechanical arm in coordinate system 2 can be obtained as follows:(47)rF2=R32rF3+p32.Bringing (47) into (45) and expanding it can obtain equation (48). Equation (48) shows the function relationship about the rotation of the third mechanical arm driven by the telescopic motion of the hydraulic cylinder 2.(48)rFT3rF3+2p32−rE2TR32rF3+p32−rE2Tp32−rE2=l22.Differentiating (48) can obtain equation (49), which shows the relationship between the telescopic velocity of hydraulic cylinder 2 and the rotation angular velocity of the third mechanical arm relative to the second arm.(49)p32−rE2Tω^32R32rF3=l2v2,where(50)ω^32=0−ω332ω232ω3320−ω132−ω232ω1320=0−θ˙30θ˙300000.According to equation (46) and equation (48), the relationship between the relative angle of hydraulic cylinder 2 to the second mechanical arm and the relative angle of the third mechanical arm to the second mechanical arm can be obtained.The third closed chain is composed of hydraulic cylinder 3, the third and the fourth mechanical arms. Equation (51) can be obtained from the relation that the distance between point J and point K in coordinate system 4 is equal to the length of hydraulic cylinder 3. Equation (52) can be obtained from the relation that the position vector of point K on the fourth mechanical arm and point K on hydraulic cylinder 3 in coordinate system 4 are equal.(51)rK4−rJ4=l3,(52)R54rK5+p54=R104rK10+p104.According to the homogeneous transformation matrix of coordinate system 5 relative to coordinate system 4, the position vector of pointK on the fourth mechanical arm in coordinate system 4 can be obtained as follows:(53)rK4=R54rK5+p54.Bringing (53) into (51) and expanding it can obtain equation (54). Equation (54) shows the function relationship about the rotation of the fourth mechanical arm driven by the telescopic motion of the hydraulic cylinder 3.(54)rKT5rK5+2p54−rJ4TR54rK5+p54−rJ4Tp54−rJ4=l32.Differentiating (54) can obtain equation (55), which shows the relationship between the telescopic velocity of hydraulic cylinder 3 and the rotation angular velocity of the fourth mechanical arm relative to the third arm.(55)p54−rJ4Tω^54R54rK5=l3v3,where(56)ω^54=0−ω354ω254ω3540−ω154−ω254ω1540=0−θ˙50θ˙500000.According to equation (52) and equation (54), the relationship between the relative angle of hydraulic cylinder 3 to the third mechanical arm and the relative angle of the fourth mechanical arm to the third mechanical arm can be obtained.The fourth closed chain is composed of hydraulic cylinder 4, the fourth and the fifth mechanical arms. Equation (57) can be obtained from the relation that the distance between point P and point Q in coordinate system 6 is equal to the length of hydraulic cylinder 4. Equation (58) can be obtained from the relation that the position vector of point Q on the fourth mechanical arm and point Q on hydraulic cylinder 4 in coordinate system 6 are equal.(57)rQ6−rP6=l4,(58)R76rQ7+p76=R116rQ11+p116.According to the homogeneous transformation matrix of coordinate system 7 relative to coordinate system 6, the position vector of pointQ on the fifth mechanical arm in coordinate system 6 can be obtained as follows:(59)rQ6=R76rQ7+p76.Bringing (59) into (59) and expanding it can obtain equation (60). Equation (60) shows the function relationship about the rotation of the fifth mechanical arm driven by the telescopic motion of the hydraulic cylinder 4.(60)rQT7rQ7+2p76−rP6TR76rQ7+p76−rP6Tp76−rP6=l42.Differentiating (60) can obtain equation (61), which shows the relationship between the telescopic velocity of hydraulic cylinder 4 and the rotation angular velocity of the fifth mechanical arm relative to the fourth arm.(61)p76−rP6Tω^76R76rQ7=l4v4,where(62)ω^76=0−ω376ω276ω3760−ω176−ω276ω1760=0−θ˙70θ˙700000.According to equation (58) and equation (60), the relationship between the relative angle of hydraulic cylinder 4 to the fourth mechanical arm and the relative angle of the fifth mechanical arm to the fourth mechanical arm can be obtained.


## 5. Kinematic Analysis of the Manipulator

Taking a working process of the manipulator for analysis, the actual movement of the working process is as follows. The first mechanical arm rotates to make the mechanical arm turn from the initial position to the target position to grab the target object and then return to the initial position. Next, the second, third, fourth, and fifth mechanical arms cooperate with each other to make the object pass through the wellhead with the specified attitude, and finally, put the object into the pipeline in the required position. According to the kinematic model established in Section4, the quantitative analysis of the working process is as follows.The kinematic parameters of the initial position of the manipulator are as follows:(63)θ1=0,θ2=88.79,θ3=−49.2,θ5=−86.76,θ7=−40.62.(1)
In 0–5 s, the first mechanical arm rotates at a constant speed to make the end of the arm move from the initial position to the position of the target object. When the position of the object is known, that is the position vector of the mark pointR in the inertial coordinate system isrR0=1952.7,1127.4,583.8T. Thus, the first mechanical arm rotates about 30° in this process. So, θ1=6t, t, is the time from the beginning of the movement to the current moment.(2)
In 5 s–10 s, the end-effector grabs and fixes the target object. The manipulator is in a static state during this period, and the kinematic parameters remain unchanged.(3)
In 10 s–15 s, the first mechanical arm rotates at a constant speed to return to its initial position. The rotating angle of the first mechanical arm isθ1=30−6t−10.(4)
In 15 s–25 s, the first and second mechanical arms do not move, the third, fourth, and fifth mechanical arms rotate at a constant speed so that the mechanical arms are to the preparation position before entering the pipeline at the moment 25 s. In the process of movementθ1=0, θ2=88.79, the position vector of R point in the inertial coordinate system at 25 s is rR0=1237,0,1020T, the attitude matrix of the fifth manipulator in the inertial coordinate system is R70=01000−1−100.(5)
In 25 s–27 s, the mechanical arm is paused and adjusted.(6)
In 27 s–37 s, the first and second mechanical arms do not move. The third, fourth, and fifth mechanical arms rotate to keep the fifth mechanical arm vertical and enter the narrow part of the pipeline along the vertical straight line at a constant speed. In the process of movementθ1=0, θ2=88.79, the position vector of R point in the inertial coordinate system isrR0=1237,0,1020−216.6t−27, the attitude matrix of the fifth manipulator in the inertial coordinate system is R70=01000−1−100.(7)
In 37 s–39 s, the mechanical arm is paused and adjusted.(8)
In 39 s–49 s, the first mechanical arm does not move. The second, third, fourth, and fifth mechanical arms rotate so that the fifth mechanical arm remains vertical and continues to enter the pipeline along the vertical straight line at a constant speed and finally reaches the designated position of the target object. In the process of movementθ1=0, the position vector of R point in the inertial coordinate system is r0R=1237,0,−1146−216.6t−30, the attitude matrix of the fifth manipulator in the inertial coordinate system is R70=01000−1−100.According to the structural dimension of the manipulator, the structural parameters in its kinematic model can be determined as follows:(64)pzlAD=0,0,1030T,pxlDG=968.7,0,0T,pxlGH=756,0,0T,θ4=−1.19,4rIL=857.3,56.2,0T,pxlLM=785.8,0,0T,θ6=−1.01,6rNO=820.2,22,0T,rB1=−85.4,154.5,0T,rC2=−209,170.8,0T,rE2=181.3,247.8,0T,rF3=−177.2,99.1,0T,rJ4=206.2,166.2,0T,rK5=−136.9,113.9,0T,rP6=954.8,125.1,0T,rQ7=699.6,97.4,0T,rR7=1277.4,−57.6,0T,rAB1=−85.4,0,154.5T,rDE2=181.3,247.8,0T,rIJ4=206.2,166.2,0T,rNP6=954.8,125.1,0T.According to the kinematic model, structural parameters, and motion parameters of the manipulator, the variation curve of the kinematic parameters of each mechanical arm is shown in Figures6 and 7, and the variation curve of the kinematic parameters of each hydraulic cylinder is shown in Figures 8–10. For verifying the correctness of the calculation results, this paper establishes the three-dimensional model of the manipulator and imports it into Adams for virtual prototype simulation and compares the simulation results and the numerical calculation results. It is found that the two results are basically the same, and the difference is within 1% of MATLAB results. So, the kinematic model established in this paper is correct; the numerical calculation method can solve the problem correctly. The reason for the result deviation is the difference between the analysis step and the rounding error in the calculation process.Figure 6
The variation curve of the manipulator joint rotation angle changing with time.Figure 7
The variation curve of the rotation angle velocity of the manipulator joint rotation angle changing with time.Figure 8
The variation curve of the hydraulic cylinder joint rotation angle changing with time.Figure 9
The variation curve of the length of the hydraulic cylinder changing with time.Figure 10
The variation curve of the telescopic velocity of the hydraulic cylinder changing with time.According to Figure6, the variation amplitudes of the joint angle from the first to the fifth arm are about 30°, 81°, 98°, 132°, and 55° respectively. Each curve is smooth, continuous and has linear or parabolic character within each motion state. It means that during each motion state, the joint rotation of each arm has continuous angular velocity and angular acceleration, and the motion is stable. At the moment when the motion states are connected, the curves are continuous but change abruptly, which will lead to a large angular acceleration at that time and cause the vibration and impact of the robot. Observing the manipulator position in which motion states are connected, it is found that the target object is not in the narrow part of the pipeline, and the impact of the pause stage also reduces this influence.According to Figure8, the variation amplitudes of the joint angle from the first to the fourth hydraulic cylinder are about 14°, 6°, 10°, and 15° respectively. It is found that the variation amplitude of the joint angle of the hydraulic cylinder is much smaller than that of the manipulator and the joint angle curve of the hydraulic cylinder is basically consistent with that of the mechanical arm. During each motion state, each curve is smooth, continuous and has linear or parabolic character. At the moment when the motion states are connected, the curves are continuous but change abruptly. However, the sudden change of the joint angle of the hydraulic cylinder is smaller than that of the mechanical arm, especially that of the second and third hydraulic cylinders is not obvious. Moreover, and the quality of the hydraulic cylinder and the mechanical arm is at the same level, the vibration and impact caused by the hydraulic cylinder at the moment of connection are less than that of the mechanical arm, and the rotation and operation characteristics of the hydraulic cylinder are better.According to Figure9, the length variation amplitudes of the first to the fourth hydraulic cylinders are about 345 mm, 302 mm, 319 mm, and 123 mm, respectively. The length curve of each hydraulic cylinder is smooth, continuous and has linear or parabolic character within each motion state, which indicates that the velocity and acceleration of each hydraulic cylinder are continuous and the motion is stable in each motion state. When the motion states are connected, the curves are continuous but change abruptly, which will lead to large acceleration, a surge of the hydraulic cylinder flow, vibration and impact of the mechanical arm, and even delay of the control system. The pause phase reduces this effect to some extent.According to Figure7, the variation amplitudes of the joint angle velocity of the first to the fifth mechanical arms are about 12 s−1, 13 s−1, 20 s−1, 30 s−1, and 14 s−1, respectively. During each motion state, the joint angular velocity curves are continuous, smooth, and uniform most of the time, which means that the manipulator has good motion characteristics. The curve is not continuous at the time of motion state connection, and a step occurs, resulting in a sudden increase of angular acceleration. The maximum angular acceleration of the manipulator joint rotation is about 15 s−2, which occurs at 39 s on the third manipulator joint.According to Figure10, the variation amplitudes of the telescopic velocity of the first to fourth hydraulic cylinders are about 2589 mm/s, 1886 mm/s, 4200 mm/s, and 2408 mm/s, respectively. Compared with Figures 9 and 7, it is found that the variation trend of the telescopic velocity curve of the hydraulic cylinder is consistent with that of the rotation angular velocity curve of the mechanical arm joint. The hydraulic cylinder has good telescopic motion characteristics during each motion state. The maximum telescopic acceleration of the hydraulic cylinder is about 2752 mm/s2, which appears at 39 s on the place of the hydraulic cylinder 2.Rotation angle in Figure6 and rotation angle velocity in Figure 7 should have a derivative relationship relative to time. Observing the two figures, as the moment curves in Figure 6 change abruptly, the corresponding points in Figure 7 appear step phenomenon. When curves in Figure 6 have linear variation, the corresponding curves in Figure 7 have a constant value. While curves in Figure 6 have a parabolic change, the corresponding curves in Figure 7 also appear parabola. The obtained curves conform to the derivative relationship law. Hydraulic cylinder length in Figure 9 and telescopic velocity in Figure 10 should also have a derivative relation relative to time. In the same way, the obtained curves in the two figures also conform to the derivative relationship law through contrasting. This can verify the correctness of the results to some extent.According to the above results of kinematic analysis, the following conclusions can be drawn.(1)
Each mechanical arm and each hydraulic cylinder have good motion characteristics within each motion state. The curves of the rotation angle, angular velocity, movement displacement, and movement velocity of each joint with time are continuous and smooth, and the manipulator can run smoothly within each movement state.(2)
At the moment when each motion state is connected, the curves of the rotation angle, angular velocity, movement displacement, and movement velocity of each joint change abruptly with time. At these moments, each joint has acceleration, which will lead to the surge of hydraulic cylinder flow, mechanism vibration and impact, and even the delay of the control system. However, the impact is reduced in the pause stage. Furthermore, at the moment of state connection, the mechanical arm does not require a high motion characteristic.(3)
In this paper, the three-dimensional model of the manipulator is established and imported into Adams for virtual prototype simulation. The results of ADAMS simulation are compared with Matlab calculation results. It is found that the two results are basically the same, which verifies the correctness of the kinematic model and algorithm established in this paper.(4)
The kinematic model of the robot can support the following calibration and error analysis research. The results of the kinematic analysis provide the theoretical basis for the robot’s motion control.


## 6. Conclusion

In this paper, a complete and continuous kinematic modeling method is proposed by combining the advantages of existing kinematic modeling methods and introducing the idea of the Lie group. The method is then used to analyze the closed chain cascade manipulator developed in our laboratory and the simulation results of MATLAB and Adams are compared. The conclusions of this paper are as follows:(1)
In this paper, a new kinematic modeling method is proposed based on the cooperation of the DH model and the Hayati and Mirmirani model and considering the Lie group concept. The improved model is complete and continuous and can support the following robot calibration, error analysis, etc.(2)
When the proposed kinematic modeling method is combined with the Lie group theory to conduct numerical calculation, it can avoid numbers of trigonometric functions and antitrigonometric functions in the process. Especially when modeling the robots with complex mechanisms, the complex expression of trigonometric functions and inverse trigonometric functions can make the solution extremely difficult. The singularity of the trigonometric function in the calculation process can lead to no solution. So this strategy can optimize the algorithm.(3)
The proposed kinematic modeling method is used to establish the kinematic model of the closed chain cascaded manipulator developed in our laboratory, and the numerical calculation with the Lie group of its working process is carried out. The results of the numerical calculation are basically consistent with those of virtual prototype simulation in Adams, which means that the established kinematic model is correct and the numerical calculation method can solve the problem correctly. The kinematic model and the results of the kinematic analysis provide a theoretical basis for the robot’s subsequent research, such as motion control, calibration, and error analysis.(4)
As for robots with complex mechanisms and accuracy requirements needed to consider error, the method proposed in this paper can establish the robot kinematic model effectively and support the following calibration and error analysis.


## 7. Expectation

This kinematic modeling method can support the following research of calibration and error analysis. The kinematic model is the theoretical basis for the robot’s motion control, calibration, and error analysis. The corresponding calibration algorithm, error model, and motion control algorithm need to be proposed next. It is very effective in research robots with complex mechanisms and accuracy requirements needed to consider an error.



---

*Source: 9356147-2020-09-09.xml*