0INTECH
open science | open minds
OPEN Vy ACCESS ARTICLE
International Journal of Advanced Robotic Systems
An Efficient Inverse Kinematic Algorithm for a PUMA560-Structured Robot Manipulator
Regular Paper
Huashan Liu1*, Wuneng Zhou1, Xiaobo Lai2 and Shiqiang Zhu3
1 College of Information Science and Technology, Donghua University, Shanghai, China
2 College of Information Technology, Zhejiang Chinese Medical University, Hangzhou, China
3 State Key Laboratory of Fluid Power Transmission and Control, Zhejiang University, Hangzhou, China * Corresponding author E-mail: watson683@163.com
Received 24 Jan 2013; Accepted 18 Mar 2013 DOI: 10.5772/56403
© 2013 Liu et al.; licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/3.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.
Abstract This paper presents an efficient inverse kinematics (IK) approach which features fast computing performance for a PUMA560-structured robot manipulator. By properties of the orthogonal matrix and block matrix, the complex IK matrix equations are transformed into eight pure algebraic equations that contain the six unknown joint angle variables, which makes the solving compact without computing the reverses of the 4*4 homogeneous transformation matrices. Moreover, the appropriate combination of related equations ensures that the solutions are free of extraneous roots in the solving process, and the wrist singularity problem of the robot is also addressed. Finally, a case study is given to show the effectiveness of the proposed algorithm.
Keywords Inverse Kinematics, PUMA560, Robot Manipulator
1. Introduction
Multi-degree of freedom serial robot manipulators with
revolute joints are greatly used currently in various
aspects of industrial applications, such as welding, painting, palletizing, transporting, CNC processing and so on. When the geometry structure of the robot manipulator satisfies the Pieper criterion [1], i.e., three consecutive joint axes of the robot are parallel or intersect at a common point, then a certain amount of closed-form solutions can be obtained for the inverse kinematics (IK) by the analytical method [2, 3], e.g., we can find eight closed-form solutions for a PUMA560-structured robot manipulator. The efficiency of the solving process directly affects the results on the motion control, especially in the online control situations.
The inverse transformation method, as a traditional IK algorithm, is widely adopted for its intuition, but it needs to work out the inverse of each 4*4 homogeneous transformation matrix, which results in a complex and time-consuming solving process [4]. More recently, some kinds of special techniques, such as vector-dot-product operations [5], product-of-exponentials (PoE) formulas [6], topological properties [7], double quaternions [8] and Bayesian network [9], are invoked in the IK algorithms to simplify the solving process.
www.intechopen.com
IntJAdvRoboticSy, 2013, Vol. 10,236:2013 1
Operating platform
Figure 1. QJ-I (a) and its D-H link coordinate (b)
Aiming at making further improvement on the real-time performance of the IK algorithm with closed-form analytical solutions, we propose a novel and efficient approach based on the orthogonality of rotation submatrix and multiplication properties of block matrix for a PUMA560-structured robot manipulator QJ-I. The rest of this paper is organized as follows. In Section 2 the kinematics of QJ-I is given. Section 3 presents some useful properties of the orthogonal matrix and block matrix. Section 4 deals with the reconstruction of the IK equations and the solving steps. Lastly, the case study is shown in Section 5 and conclusions are made in Section 6.
2. Kinematic Equations
QJ-I, a PUMA560 type robot, is as shown in Fig.la, with its D-H link coordinate shown in Fig.lb, and the D-H parameters as well as the joint angle interval in Table 1. The QJ-I's last three consecutive joint axes intersect at a common point - satisfying the Pieper criterion.
Joint No. di/mm a /mm ai/(°) 0i Angle interval/(°)
1 250 150 -90 01 [-90,270)
2 0 550 0 02 (-270,90]
3 0 160 -90 03 (-270,90]
4 594 0 90 04 (-180,180]
5 0 0 90 05 (-180,180]
6 0 0 0 06 (-270,90]
Table 1. D-H parameters of QJ-I
The direct kinematics (DK) of QJ-I can be described as
0T = /dk(6) = O1T(Û1)12T(d2)23T(d3)34T(04)tT(05)lT(Û6)
0 0 0 1
n o a p y y y ry n o a P R P
n o a p z z z r z 0 0 0 1 0 1
where R is the 3*3 rotation matrix, including three 3*1 vectors n, o and a, which respectively denote the normal vector, sliding vector and approach vector; p is the 3*1 position vector. The 4*4 homogeneous transformation matrix is:
"1T (6;) =
ci -Siri S a c ]
si ci r; -c i ai api Ri P
0 Ti d 0 1
0 0 0 1
where si = sin0^ ci = cos0, oi = sinai, Ti = cos«i, i = 1, 2, ..., 6. 0i, 02, ..., 06 are the six unknown variables to be solved.
Corresponding to (1), the IK of QJ-I can be written as
e = [0 e2 e3 0, 05 06] = /dK(6t) = /,k(6t) (3)
3. Some Properties of the Orthogonal Matrix and Block Matrix
i. If A is an orthogonal matrix, then A -1 is also an orthogonal matrix, and A -1 = AT.
ii. If matrix C = , then its inverse can be obtained
from C-1 =
- A-1B 1
, where C is a 4*4 matrix,
A is a 3*3 matrix, B is a 3*1 vector. Furthermore, we have
C X C2
" A1 B1 X " A2 B2 "
0 1 0 1
AjA2 AJB2 + B1 01
By property (i), we find that R, Ri, Ri -1, R -1, RT and Ri T in the kinematic equations above are all orthogonal matrices, and R -1 = R T, Ri -1 = Ri T. By property (ii), the
nx 0x ax Px
transformation matrix ¡T can be partitioned into four units (rotation matrix Ri, position vector pi, 0 and 1). Therefore, the complex operations of computing the inverse of each 4*4 transformation matrix are avoided.
4. Solving the IK Equations
4.1 Reconstruction of the Equations
In this part, by using the properties given above, we reconstruct the matrix equations described as (1) into relatively ordinary trigonometric function equations, thereby simplifying the solving process so as to enhance the real-time performance of the algorithm.
Considering that, the first three joints of QJ-I are intended to determine the three-dimensional coordinates of the end-effector, while with the last three joints to determine the orientation, and to balance the equation on both sides with the same quantity of the unknowns, we break up the 6R-chain of QJ-I as described in (1) into two 3R-chain[10], and then obtain
3T'4T'5^ — 2T"—1 1 0T"—1 0 ji 4T 5T 6T _ 3T 2T 1T 6T
By using the properties (i) and (ii) we get
«3T —RTP3
2T—1 =
0^-1 1
RT -RT P1
Then the left side of (4) can be written as
41 51 61 —
R4R5R6 R4R5P6 + R4 P5 + P4 0 1
In addition, the right side of (4) can be written as
2T—1 3^—14^—1 0T _ 3T 4T 5T 6T —
rTRTRTR RTRTRTp - RTRTRTP1 - RT RT Pi - RT P3"1(6) 0 1
Finally, from (4)~(6), we obtain
R4R5R6=R3TRiTRTR
R4R5P6 + R4P5 + P4 = R3 Ri R11 p-R3T R2TRT P1- R3T R2T Pi- R3T P3
By using the symbol processing software Maple©, equations (7) and (8) lead to
s1px-c1py — 0
c * s^ — c^o ca + s a s^^ a
4 5 23 1 x 23 1 y 23 z
s4s5 — s1ax c1ay
c5 — s23 c1ax s23 s1ay c23 az
s5c6 — s23 c1nx s23 c1ny c23 nz
s5s6 — s23 c10x s23 s10y c23 oz
where s23 = sin(02 +03), c23 = cos(02 +03).
Up to now we have obtained eight pure algebraic trigonometric equations (9)~(16), which is less than the algorithm proposed in [4] (with nine equations). Although the proposed algorithm has the same quantity of equations as the algorithm proposed in [5], all the equations are generated from (1) directly without constructing new equations as pp and Pa in [5], which may be a little abrupt.
4.2 Solving Steps
Here, the four-quadrant inverse tangent function atan2 will be involved frequently in the specific solving process to obtain the complete solutions as follows.
Step 1: By (9), we get two solutions of 01.
Step 2: Square both sides of (10) and (11), respectively, then add together. We get
(d1 - pz )s2 -(c1Fx + s1?y-a1)c2 —
[a32 + d42 - a22 - (c^x + s1Vy - a1)2 - (d1 - Vz)2 ] / 2a
For each 01 we can get two solutions of 02, i.e., four solutions of 02 in total.
Step 3: By (10), one solution of 03 corresponding to each 02, i.e., four solutions of 03 can be obtained.
Step 4: When we solve 04 by (12) and (13), we should judge whether the robot manipulator is singular or not, due to that singularities inherently limiting the capability of a manipulator to complete its task [11].
If 05 + 0°, then 04 = atan2(s4s5, c4s5) and atan2(-s4s5, - c4s5) for each set of 01, 02 and 03;
By (1) and (2), we can obtain
3^ _3t'4T'5T' _
6T —4T 5T 6T —
s23 (Vz + a2s2 -d1) - c23 (c1Vx + S1Vy - a2c2 - a1) — -a3 (10)
c4c5c6 + S4S6 c4c5S6 + s4c6 c4S5 0
s4c5c6 — c4c6 s4c5s6 c4c6 S4S5 0
s5c6 —S5S6 c5 d4
0 0 0 1
S23(c1Vx + S1Vy - a2c2 -a1) + c23(Vz + a2S2 -d1) — -d4 (11)
If 05 = 0°, i.e., QJ-I is in wrist singularity, (18) can be written as
www.intechopen.com
Huashan Liu, Wuneng Zhou, Xiaobo Lai and Shiqiang Zhu: 3 An Efficient Inverse Kinematic Algorithm for a PUMA560-Structured Robot Manipulator
cos(04 -e6) sin(04 -e6) 0
sin(04 -e6) -cos(04 -e6) o
o o -1 00
Now that the fourth and the sixth joint axes of QJ-I are overlapped, the value of 04 can be chosen arbitrarily in theory. It is worth noting that from (19) we find that the transformation matrix relates solely to (04-06), and on this condition, to keep away from the singularities and fix the pose of QJ-I in the meantime, we only need to rotate both 04 and 06 at an equal angle (which is small enough) in the same direction, to change the axis direction of the fifth joint, thereby avoiding the singular point.
Step 5: By (12) and (14), we can get one solution of 05 corresponding to each set of 01, 02, 03 and 04.
Step 6: By (15) and (16), one solution of 06 can be obtained corresponding to each set of 01, 02, 03 and 05. So far, we have obtained all eight closed-form solutions of 01, 02, 03, 04, 05 and 06. It is worth pointing out that benefiting from the proper combinations of the related equations, there generates no extraneous root in each solving step, which always occurs in the inverse transformation method in [4] and the vector-dot-product-based approach in [5]. This advantage excuses the IK solving from both verifying and matching the roots, which further enhances the efficiency of the algorithm.
5. Case Study
In this part, sample calculations are executed, together with simulations and tests to verify the effectiveness of the proposed IK algorithm.
The pose matrix of the end-effector relative to the bottom base is given as
-0.0188 0.4154 0.9095 206.7566
0.4810 0.8012 -0.3560 55.4003
-0.8765 0.4307 -0.2148 -418.0041 0 0 0 1.0000
According to the IK algorithm proposed and D-H parameters, as well as the joint angle interval shown in Table 1, we get eight closed-form solutions as shown in Table 2, and the corresponding orientation simulation in Fig.2.
To testify to the real-time performance of the proposed IK algorithm, we firstly sampled 681 discrete points from a closed-curve called QS Eagle referred to in [12], and recorded the three-dimensional coordinate of each point, then put them into the pose matrix of the end-effector, after getting these 681 samples of pose matrices. With the proposed method in this paper, (called "Ours"), the inverse transformation method in [4] (called "ITM") and the vector-dot-product-based approach in [5] (called "VDP"), we solved out the IK solutions of each given pose matrix respectively, in the VC++ compiling environment on a notebook computer platform (Windows XP 32-bit SP3 OS, Intel Core 2 Duo Dual 2.2GHz CPU, 2GB DDR3 800MHz RAM) for 500 times with an accuracy of 0.00000001°. The performance comparisons of the three schemes are shown in Table 3.
No. 01/(°) 02/(°) 0s/(°) 04/(°) 05/(°) 06/(°)
1 15.00000931 -215.95388774 -184.84918850 51.85808138 132.56569742 -5.57849644
2 15.00000931 -215.95388774 -184.84918850 -128.14191862 -132.56569742 -185.57849644
3 15.00000931 24.99999937 35.00000104 45.00289124 54.99852255 65.00379351
4 15.00000931 24.99999937 35.00000104 -134.99710876 -54.99852255 -114.99620649
5 195.00000931 -188.34210158 -173.60896143 -143.86066631 100.83006201 27.34981070
6 195.00000931 -188.34210158 -173.60896143 36.13933369 -100.83006201 -152.65018930
7 195.00000931 65.52127702 23.75977397 -70.51870198 142.09005479 -78.98705841
8 195.00000931 65.52127702 23.75977397 109.48129802 -142.09005479 -258.98705841
Table 2. IK solutions of QJ-I
Main means Extraneous root Average time cost
Ours matrix blocking has not 6.753 |is
ITM inverse transformation has 13.645|is
VDP vector dot product has 8.816|is
Table 3. Performance comparisons
As shown in Table 3, by our scheme, it just took an average time of 6.753 |is to figure out all eight solutions for one sample pose matrix, which is 49.5% of the average time cost by ITM and 76.6% of that by VDP, due to no calculating the inverse of the matrix and producing no extraneous root in the IK solving.
Figure 2. Orientation Simulation of QJ-I
6. Conclusions
This paper addresses the problem of an efficient IK algorithm for a PUMA560-structured robot manipulator. First, some properties of the orthogonal matrix and block matrix are applied to help simplify reconstructing the IK matrix equations into trigonometric function equations, without calculating the inverses of the homogeneous transformation matrices. As a second contribution, the proposed IK algorithm is free of producing extraneous roots by appropriately combining the related equations for a certain unknown, which allows the solving to avoid identifying the roots and matching the real root of one joint with those of the other joints. All of these qualify the proposed IK algorithm for high real-time control situations with efficiency computing performance as presented in the case study. Furthermore, although the wrist singularity, as the most common singular type for a PUMA560-structured robot manipulator, is discussed in the IK solving, the other singularities, such as boundary singularity or inner singularity, are not mentioned and remain to be further studied.
7. Acknowledgements
We express our sincere thanks to the editors, referees and all the members of our discussion group for their beneficial comments, as well as special thanks to Zhou Zhou for her work on the image processing. The presented work has been partially supported by the National Natural Science Foundation of China under grant nos. 61203337, 61075060, the Specialized Research Fund for the Doctoral Program of Higher Education under grant no. 20120075120009, the Natural Science Foundation of Shanghai under grant no. 12ZR1440200, the Zhejiang Province Natural Science Foundation of
China under grant no. LQ12F01004, the Fundamental Research Funds for the Central Universities under grant no. 12D10408, and the Young Teacher Training Program for the Shanghai Universities under grant no. DHU11035.
8. Reference
[1] Siciliano B., Khatib O. (2008), Springer Handbook of
Robotics, Springer.
[2] Chapelle F., Bidaud P. (2004), Closed form solutions
for inverse kinematics approximation of general 6R manipulators, Mechanism and Machine Theory, 39(3): 323-338.
[3] González-Palacios M. A. (2013), The unified
orthogonal architecture of industrial serial manipulators, Robotics and Computer-Integrated Manufacturing, 29(1): 257-271.
[4] Lenarcic J., Husty M. (2012), Latest Advances in Robot
Kinematics, Springer.
[5] Cheng Y. L., Zhu S. Q., Liu S. G. (2008), Inverse
kinematics of 6R robots based on the orthogonal character of rotation sub-matrix, Robot, 30(2): 487461.
[6] He C., Wang S. X., Xing Y., Wang X. F. (2013),
Kinematics analysis of the coupled tendon-driven robot based on the product-of-exponentials formula, Mechanism and Machine Theory, 60: 90-111.
[7] Tarokh M., Keerthi K., Lee M. (2010), Classification
and characterization of inverse kinematics solutions for anthropomorphic manipulators, Robotics and Autonomous Systems, 58(1): 115-120.
[8] Qiao S. G., Liao Q. Z., Wei S. M., Su H. J. (2010),
Inverse kinematic analysis of the general 6R serial manipulators based on double quaternions, Mechanism and Machine Theory, 45(2): 193-199.
[9] Artemiadis P. K., Katsiaris P. T., Kyriakopoulos K. J.
(2010), A biomimetic approach to inverse kinematics for a redundant robot arm, Autonomous Robots, 29(3-4): 293-308.
[10] Husty M. L., Pfurner M., Schröcker H. P. (2007), A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator, Mechanism and Machine Theory, 42(1): 66-81.
[11] Oetomo D., Ang Jr M. H. (2009), Singularity robust algorithm in serial manipulators, Robotics and Computer-Integrated Manufacturing, 25(1): 122-134.
[12] Liu H. S., Lai X. B., Wu W. X. (2013), Time-optimal and jerk-continuous trajectory planning for robot manipulators with kinematic constraints, Robotics and Computer-Integrated Manufacturing, 29(2): 309317.
www.intechopen.com
Huashan Liu, Wuneng Zhou, Xiaobo Lai and Shiqiang Zhu: 5 An Efficient Inverse Kinematic Algorithm for a PUMA560-Structured Robot Manipulator
