A General Kinematic Method for − Type Manipulator Based on Screw Theory Using Dual-Quaternion

Rule Bai, Zhifeng Liu

1 Abstract— The kinematic problem is an essential algorithm back to instantaneous rigid body motion central axis theorem problem of the manipulator control. To improve the proposed by Italian mathematician Giulio Mozziin 1763, performance of the robots, it has been widely researched in reasonable central axis theorem proposed by the French robotics. In this paper, a general kinematic method for R−θ type mathematician and physicist Louis Poinsot 1806, and rigid manipulator is created using the combination of screw theory displacement theory presented by the French mathematician and dual-quaternion, it can be applied to different type of R−θ Michel Floreal Charles in1830 [4]. Based on the theorems of robot with different arm length. Screw theory provides a global Chasles and Poinsot, Robert S. Ball developed a complete geometric representation of the kinematics of manipulator which theory of screws which was published in 1900 [5]. Many greatly simplifies the analysis of the mechanism and can avoid researchers have applied screw theory to their robot control the singularities due to using local coordinates. With this method, algorithm. J.M. McCarthy presented a detailed research on the kinematics of the R−θ type manipulator is expressed by dual- dual 3x3 orthogonal matrices and its application to velocity quaternion representing the screw motion of each joint of the analysis by using screw theory [6]. R.M. Murray et al. solved manipulator. By dual-quaternion only eight numbers are used to express a screw motion. Meanwhile, the motion transformation 3-DOF and 6-DOF robot manipulator kinematics by using by dual-quaternion takes fewer calculations than the screw theory with 4 homogeneous transformation matrix homogenous matrix. Thus, comparing with the traditional D-H operators and Paden-Kahan sub-problems [3]. parameter method, this method provides a compact, The core of manipulator kinematics is rigid body computationally efficient algorithm of kinematics of R−θ type transformation. Several representation methods can be used to wafer transfer robot which needs less storage space and does not describe the rigid body transformation, such as Euler angles, suffer from singularity problem. exponential coordinates and quaternions. Euler angles and

exponential coordinates are both in 3-dimensional I. INTRODUCTION representation of SO(3). It is a fundamental topological fact that singularities can never be eliminated in any 3-dimensional During the process of IC manufacturing, wafers are representation of SO(3). Whereas, quaternions give a global transported between many workstations back and forth. The parameterization of SO(3), at the cost of using four numbers efficiency of wafer transportation is crucial to the development instead of three to represent a rigid body transformation [3]. of IC industry. Wafer transfer robot is the key equipment in the Hence quaternions provide an efficient way to represent rigid process of IC manufacturing. Improving the performance of body transformation and do not suffer from singularity wafer transfer robot will exert a positive effect on the accurate problem. However, quaternions can only represent a half of the positioning of wafers and the efficiency of transferring. The rigid body transformation, rotation. Because a full 3- R−θ type manipulator is the most common robot used during dimensional rigid body transformation is composed of rotation the wafer transfer [1]. and translation, dual-quaternion can be used to unify the This paper will focus on a kinematic solution of R−θ type rotation and the translation. It is a combination of dual numbers wafer transfer robot. Kinematic problem is an essential and quaternion mathematics. It is, moreover, compact and algorithm problem of the manipulator control. It directly computationally efficient. Y. Aydin and S. Kucuk gave an effects the response speed and the working efficiency of a inverse kinematic solution for industrial robot manipulator manipulator. As a result, the optimizing the kinematic using dual-quaternion and provided several examples [7]. Ni algorithm has become a primary study of many scholars. The Zhensong, Liao Qizheng et al. solved inverse kinematics of kinematics can be divided into two parts, forward kinematics spatial 6R manipulator using dual-quaternion [8]. and inverse kinematics. The forward kinematics is In this paper, using the features of the R−θ type straightforward and easy to solve. The inverse kinematics, by manipulator, we create a general kinematic method of this type contrast, is not an easy task and suffers multi-solution problem. of robot with dual-quaternion representing the screw motion of Numerous techniques can be applied to the manipulator each joints of the manipulator. The screw theory provides a kinematics, and Denavit-Hartenberg(D-H) parameter method global geometric representation of the kinematics of [2] is most widely used currently. In recent decades, however, manipulator, it can avoid the singularities due to using local screw theory is adopted more often, it provides a global coordinates. By dual-quaternion only eight numbers are used geometric representation of the kinematics of manipulator to express a screw motion. Meanwhile the motion which greatly simplifies the analysis of the mechanism [3]. transformation using dual-quaternion takes fewer calculations translation of line in cartesian space. It initially can be traced than homogenous matrix [11]. This makes the forward

*Resrach supported by Beijing major science and technology projects Rule Bai is with Beijing Key Laboratory of Advanced Manufacturing (D17110400590000). Technology in Beijing University of Technology, China (email: Zhifeng Liu is with Beijing Key Laboratory of Advanced Manufacturing [email protected]). Technology in Beijing University of Technology, China (email: [email protected]).

kinematics more compact and computationally efficient. For III. MATH PRELIMINARY inverse kinematics, it’s convenient to extract the position information of the tool frame from the representation of screw A. Quaternion motion in dual-quaternion, hence the inverse kinematic can be A quaternion can be considered as composed of a scalar and solved by geometric method. The simulation in the end shows a vector, thus the solution we create is feasible, and the computational efficiency is greatly improved comparing to using the D-H (1) parameter method. = +++

II. R−Θ TYPE MANIPULATOR In this equation, is the scalar component of , = The structure of R−θ type manipulator is shown in Fig. 1. (,,) is the vector component of , hence a shorthand It has one prismatic joint (d) and five revolute joints notation of is =(,), and , , are three orthogonal unit vectors, which satisfy the relations (, , , ，).

∙=∙=∙=∙∙=−1 (2)

A quaternion with =0 is called pure quaternion, and a quaternion with = is called real quaternion. The quaternion addition can be described as:

(3) + =( +, +)

The quaternion multiplication can be described as:

v (4) =( − ∙, + + ×) Figure 1. The structure of R−θ type manipulator The prismatic joint d achieves the heave motion in z Multiplication of quaternions is distributive and direction. The revolute joint is responsible to the rotation of associative, but not commutative. the whole arm of the robot. The revolute joint decides the The conjugate of a quaternion =( ,) is given by ∗ = pose of end-effector. The combination of motions of revolute (,−) and the magnitude of a quaternion is joints , and achieves the R direction rectilinear motion of the end-effector. ∗ (5) ||q|| = = + + + A. Analysis of R direction rectilinear motion The R direction rectilinear motion is the essential motion The inverse of a quaternion satisfies the relation of the R−θ type manipulator. R direction rectilinear motion can be described with Fig. 2. The end effector is attached to the (6) point T. To achieve the rectilinear motion of the end effector, = =1 and must satisfy the relation =, at the same time the relation between and should be : : =1:−2:1. The unit quaternion is a set of quaternions whose magnitude equals 1 (||q|| =1). It is worth noting that the inverse of a unit quaternion equals the conjugate of the same quaternion. Thus, for a unit quaternion q, we have

=∗ (7)

Given a rotation about an axis by an angle , an associate unit quaternion can be defined as

=(cos ,sin ( )) (8)

Figure 2. R direction rectilinear motion B. Dual-Quaternion Dual-quaternion is a combination of the dual-number theory [9] and quaternion. It can be given by

(9) = + with =0but ≠0 = (1, )(0, ) (18)

Where and are quaternions, is known as the dual- The dual-quaternion vector transformation can be operator. described by If we have =ς (19) = + (10) = + Where is a dual-quaternion representing a 3-dimensional The dual-quaternion addition can be described as: vector. is a dual-quaternion representing the transformation. is a dual-quaternion representing the result 3- (11) + = ( +) +（ +） dimensional vector. The dual-quaternion multiplication can be described as: C. Screw Theory A rigid body motion consists a rotation about a space axis (12) with an angle and a translation along the same axis by an = +( +) amount of [5]. We define such a motion a screw motion. To further encourage this analogy, we introduce a pith of a screw Like the quaternion, multiplication of dual-quaternions is to be the ratio of translation to rotation, ℎ=/ (assuming distributive and associative, but not commutative. ≠0). Hence the amount of translation d can be expressed as ℎ. By convention, if a screw motion is pure translation, ℎ= The conjugate of a dual-quaternion = + is given ∗ ∗ ∗ ∞ and we still define ℎ = . Thus, the screw motion can be by ς = + and the magnitude of a dual-quaternion is described by Fig. 3

|||| =ς∗ (13)

The inverse of a dual-quaternion ς satisfies the relation

ς =ς=1 (14)

∗ ς = (15) |||| . It is crucial to note, similar to the unit quaternion, that a unit Figure 3. Screw Motion dual quaternion has the magnitude of 1. Where We can use a unit dual-quaternion representing a rigid body is an unit vector specifying the direction of the rotation motion which is composed of rotation and translation. axis. p is a random point of the rigid body. (16) q is a random point on the rotation axis. = + () represents the rotation about axis by angle . The final position of point p is given by: Where =(cos ,sin ( )) represents the rotation about (20) =+()(−) +ℎ an axis by an angle . = , is the quaternion form of the translation We are using and representing the position vector of vector t. point p and q. A shorthand notation of is =(,)(,). The Or in homogeneous matrix form: most used dual-quaternion in kinematics of manipulator has only rotation or translation, thus () − () + ℎ The dual-quaternion only has rotation = (21) 1 011

=(cos ,sin ( ))(0, ) (17) So, the transformation homogeneous matrix can be expresses by: The dual-quaternion only has translation

() − () + ℎ ς =ς ς ς ς ς ς (28) = (22) 01 It is worth noting that in our R−θ type manipulator, ς is a If we define =×, the screw motion can be pure translation screw motion with the displacement d, expressed with dual-quaternion as follow ς , ς , ς , ς and ς are pure rotation screw motions corresponding to angles , , , and .

The axes of the screw motions of each joint can be =(cos ,sin ( )) expressed by 2 2 (23) (− sin ( ), sin ( ) + cos ) 2 2 2 2 2 = = = = = (0,0,1) (29) = (0,1,0) Because every function of dual number satisfies the relation We can choose the axis points (+) =() +() (24) = (0,0,0) =(0,,0) =(0, + ,0) =(0, +2 ,0) Thus, this dual-quaternion can be written in this form [10] =(0, +2,) Thus, we have the screw motion of each joint =(cos , (+)) (25) ς = (1,0,0,0) 0,0,0, ς =(cos,0,0,)(0,0,0,0) IV. KINEMATICS OF R−Θ TYPE MANIPULATOR ς =(cos ,0,0, )(0, ,0,0) A. Forward kinematics (30) ς =(cos,0,0,)(0, ( + ) , 0,0) The forward kinematics of a manipulator determines the ς =(cos ,0,0, )(0, ( +2 ) , 0,0) configuration of the end effector, given the joint angles or displacement of each joint of the manipulator. ς =(cos,0, , 0)(0, − , 0,0) As illustrated in Fig. 4, the R−θ type manipulator has one The forward kinematics can be expressed by the equation prismatic joint (d) and five revolute joints (, , , ，). We attach two coordinate frame to the manipulator, the base frame, B, and the tool frame, T. (31) =ςςςςςς = +

where =(,,,) =(,,,) If we define = , = , =cos ( − ), =sin ( − ), =cos + +⋯ and, ⋯ = + +⋯ . The eight elements of can ⋯ be solved out as below

Figure 4. R−θ type manipulator (32) We define the initial configuration of the manipulator as = shown in Figure 3.2. We use representing the initial transformation between tool and base frame in dual quaternion (33) form, it is given by =−

(26) = (1,0,0,0)(0,0, , ) (34) =

We let represent the result transformation between tool and base frame. The relation between and is (35) =

(27) =ς =− − − − (36) We are using ς to represent the screw motion of each joint, (( + ( +)) + ( +2) − thus ( + ( +)))

=− − − + According to the geometric properties of R−θ type ( ( + ( + ) ) + ( +2 ) − (37) manipulator, the horizontal distance between the origin of base frame and the origin of tool frame is defined by the second and ( + ( +))) third entry of . Thus the can be solved by the law of cosines.

= − − + (38) ( ( + ( + ) ) + ( +2 ) − (46) = (( + − ( +))/2) ( + ( +)))

(47) =−2 = + − + (39) (( + ( +)) + ( +2) − ( + ( +))) (48) = Note: , and satisfy the relation, : : = To solve out, we have equation 1: −2: 1.

(49) B. Inverse kinematics ςςςςςς = The inverse kinematics is the problem that given a desired configuration of tool frame, to find joint angles which can For now, , , ς, ς, ς, ς and ς are known, the achieve the configuration. Thus, for the inverse kinematics of is the only unknown in this equation, thus we have the R−θ type manipulator, is given (50) =2cos ( +) (40) = + = (,,,) +(,,,) V. SIMULATION CASE The goal is to find the corresponding , , , , . and . Our kinematic algorithm can be applied to different R−θ type manipulator with different arm length. We choose a R−θ To find the displacement () of the joint 1, we can use the type manipulator whose structure parameters are shown in relation between and Table 1.

= (41) TABLE I. STRUCTURE PARAMETERS

Where is the quaternion form of the translation vector t, value 500 60 150 150 80 thus Each joint angle or displacement has its constraint which is shown in Table 2 (42) =2

TABLE II. CONSTRAINTS OF JOINT ANGLE OR DISPLACEMENT

=(2( + + +), 2( − + −), range [0,300] (−, ] [ , ] −2 (−, ] (43) 2 2 2( − − +), 2( + − − )) We arrange each joint a reasonable value which is shown in Table 3 to give the forward kinematic solution. The displacement () of the joint 1 is along the axis, hence TABLE III. DISPLACEMENT AND ANGLES OF EACH JOINT

(44) =2(− + − +)− value 50 2/3 /6 −2 /6 Using the formulas that we derivate in chapter 3, we have determines the position of the origin of tool frame in x- y plane of base frame, therefore can be solved out by the = (0.48296, −0.22414,0.12941,0.83652) second and third entry of (−255.9114, −202.8104,34.9066, 88.0076) Because in chapter 3 shows the position of the origin of (45) = ( ) tool frame in x-y plane of base frame, with analyzing the , we can determine the range of . When <0, 0< <,

when 0, − < <0, when =0 and VI. CONCLUSION 0, =0, when =0 and <0, =. The range of In this paper, we created a general kinematic method for can be determined by in , if 0, 0 , R−θ type manipulator using the combination of screw theory if <0, − < <0. and dual-quaternion. It can be applied to different R−θ type In this circumstance, manipulator with different arm length. Comparing to the D-H parameter method, our method has few advantages. Using = （0， − 346.2436， − 199.9038，550） dual-quaternion is convenient to extract the position = 0.12941 information of the tool frame, hence the inverse kinematic can be solved easily by geometric method. It only uses eight Thus numbers to represent the configuration of the end-effector, meanwhile it avoids a large number of matrixes inverse 0< < multiply operations, makes the kinematic algorithm more compact and computationally efficient. It has been tested that 0 the computational efficiency has been improved 41%. Then, given above-solved , using the inverse kinematic Furthermore, it only defines two coordinates (the base formula in chapter 3, we can obtain two groups of solutions of coordinate and the tool coordinate) so that the expression is each joint which are shown in Table 4. simple and can avoid the singularities due to using local coordinates. Thus, it can be applied to the trajectory planning TABLE IV. INVERSE KINEMATIC SOLUTION and manipulator control system more easily.

EFERENCES 1 50 2/3 /6 −/3 /6 /6 R [1] Cong M., Du Y., Shen B. H., Jin L. G. Robotic Wafer Handling 2 50 2/3 −/6 /3 −/6 /6 System for Integrated Circuit Manufacturing; A Review [J]. Robot. The first group of solutions is identical to the values in 2007, 29(3): 261-266. [2] Denavit J., Hartenberg R.S. A kinematic notation for lower pair Table 4.3, hence the method of kinematic solution in this paper mechanisms based on matrices[J]. Journal of Applied Mechanics. in correct. 1995, 21(5): 215-221. [3] M. Murray, Z. Li, and S.S. Sastry. A mathematical introduction to To test how our method can boost the computational robotic manipulation [M]. Boca Raton FL: CRC Press, 1994. efficiency, we set an experiment that we write two programs to [4] Dai J. S. Geometrical Foundations and Screw Algebra for Mechanisms calculate the kinematics of above-mentioned manipulator and Robotics, Higher Education Press, also Screw Algebra and using our method and D-H parameter method respectively in Kinematic Approaches for Mechanisms and Robotics [M]. Springer, Matlab. Then we evaluate the calculation time by tic-toc London, 2014. command in Matlab at the same computational environment. [5] R. S. Ball. The Theory of Screws [M]. Cambridge University Press, 1900. We run each program six times to eliminate the error due to the [6] J. M. McCarthy. Dual orthogonal Matrices in manipulator kinematics state difference of the computer in different time, and the result [J]. International Journal of Robotics Researches. 1986, 5:45 51. is in Table 5. [7] Y. Aydin and S. Kucuk. Quaternion Based Inverse Kinematics for Industrial Robot Manipulators with Euler Wrist [A]. IEEE International Conference on Mechatronics [C]. 2006, July 3–5: 581– TABLE V. CALCULATION TIME OF EACH METHOD 586. [8] Ni Zhensong, Liao Qizheng, Wei Shimin, LI Ruihua. Dual Four 1 2 3 4 5 6 Element Method for Inverse Kinematics Analysis of Spatial 6R A 0.00214 0.00437 0.00220 0.00204 0.00516 0.00207 Manipulator [J]. Journal of Mechanical Engineering. 2009, 45(11): 25-29. B 0.00400 0.00606 0.00413 0.00462 0.00759 0.00432 [9] W. Clifford. Mathematical Papers [M]. Chelsea Pub Co, 1986. [10] K. Daniilidis. Hand-Eye Calibration Using Dual Quaternions [J]. The A for Screw theory in dual-quaternion, B for D-H parameter International Journal of Robotics Research. 1999, 18(3): 286–298. To make the expression clearer, we draw a bar diagram to [11] Emre Sariyildiz, Hakan Temeltas. A Comparison Study of Three show the average calculation time of each method in Fig. 5. It Screw Theory Based Kinematic Solution Methods for the Industrial is clear that the computational efficiency has boosted 41%. Robot Manipulators [A]. Proceedings of the IEEE International Conference on Mechatronics and Automation [C], 2011, August 7 - 10, Beijing, China D-H parameter Screw theory in dual-quaternion

0 0.002 0.004 0.006 time

Figure 5. average calculation time of each method