An Efficient Inverse Kinematic Algorithm for a PUMA560-Structured Robot Manipulator

Abstract

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.

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 sub-matrix 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.1a, with its D-H link coordinate shown in Fig.1b, 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.

Table 1.

D-H parameters of QJ-I

Joint No.	d_i/mm	a_i/mm	α_i/(°)	θ _i	Angle interval/(°)
1	250	150	−90	θ ₁	[-90,270)
2	0	550	0	θ ₂	(−270,90]
3	0	160	−90	θ ₃	(−270,90]
4	594	0	90	θ ₄	(−180,180]
5	0	0	90	θ ₅	(−180,180]
6	0	0	0	θ ₆	(−270,90]

Figure 1.

QJ-I (a) and its D-H link coordinate (b)

The direct kinematics (DK) of QJ-I can be described as

\begin{array}{l} _{6}^{0} T = f_{D K} (θ) =_{1}^{0} T (θ_{1})_{2}^{1} T (θ_{2})_{3}^{2} T (θ_{3})_{4}^{3} T (θ_{4})_{5}^{4} T (θ_{5})_{6}^{5} T (θ_{6}) \\ = [\begin{matrix} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} n & o & a & p \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} R & p \\ 0 & 1 \end{matrix}] \end{array}

(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:

_{i}^{i - 1} T (θ_{i}) = [\begin{matrix} c_{i} & - s_{i} τ_{i} & s_{i} σ_{i} & a_{i} c_{i} \\ s_{i} & c_{i} τ_{i} & - c_{i} σ_{i} & a_{i} s_{i} \\ 0 & σ_{i} & τ_{i} & d_{i} \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} R_{i} & p_{i} \\ 0 & 1 \end{matrix}]

(2)

where s_i = sinθ_i, c_i = cosθ_i, σ_i = sinα_i, τ_i = cosα_i, i = 1, 2, …, 6. θ₁, θ₂, …, θ₆ are the six unknown variables to be solved.

Corresponding to (1), the IK of QJ-I can be written as

θ = [\begin{matrix} θ_{1} & θ_{2} & θ_{3} & θ_{4} & θ_{5} & θ_{6} \end{matrix}] = f_{D K}^{- 1} (_{6}^{0} T) = f_{I K} (_{6}^{0} T)

(3)

3. Some Properties of the Orthogonal Matrix and Block Matrix

iIf A is an orthogonal matrix, then A ⁻¹ is also an orthogonal matrix, and A ⁻¹ = A ^T.

If matrix $C = [\begin{matrix} A & B \\ 0 & 1 \end{matrix}]$ , then its inverse can be obtained from $C^{- 1} = [\begin{matrix} A^{- 1} & - A^{- 1} B \\ 0 & 1 \end{matrix}]$ , where C is a 4×4 matrix, A is a 3×3 matrix, B is a 3×1 vector.

Furthermore, we have

C_{1} \times C_{2} = [\begin{matrix} A_{1} & B_{1} \\ 0 & 1 \end{matrix}] \times [\begin{matrix} A_{2} & B_{2} \\ 0 & 1 \end{matrix}] = [\begin{matrix} A_{1} A_{2} & A_{1} B_{2} + B_{1} \\ 0 & 1 \end{matrix}] .

By property (i), we find that R, R _i, R _i ⁻¹, R ⁻¹, R ^T and R _i ^T in the kinematic equations above are all orthogonal matrices, and R ⁻¹ = R ^T, R _i ⁻¹ = R _i ^T. By property (ii), the transformation matrix $_{i}^{i - 1} T$ can be partitioned into four units (rotation matrix R _i, position vector p_i, 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

_{4}^{3} T_{5}^{4} T_{6}^{5} T =_{3}^{2} T ​^{- 1}_{2}^{1} T ​^{- 1}_{1}^{0} T ​^{- 1}_{6}^{0} T

(4)

By using the properties (i) and (ii) we get

\begin{matrix} _{3}^{2} T^{- 1} = [\begin{array}{l} R_{3}^{T} & - R_{3}^{T} p_{3} \\ 0 & 1 \end{array}],_{2}^{1} T^{- 1} = [\begin{array}{l} R_{2}^{T} & - R_{2}^{T} p_{2} \\ 0 & 1 \end{array}], \\ _{1}^{0} T^{- 1} = [\begin{array}{l} R_{1}^{T} & - R_{1}^{T} p_{1} \\ 0 & 1 \end{array}] \end{matrix}

Then the left side of (4) can be written as

_{4}^{3} T_{5}^{4} T_{6}^{5} T = [\begin{matrix} R_{4} R_{5} R_{6} & R_{4} R_{5} p_{6} + R_{4} p_{5} + p_{4} \\ 0 & 1 \end{matrix}]

(5)

In addition, the right side of (4) can be written as

\begin{array}{l} _{3}^{2} T ​^{- 1}_{4}^{3} T ​^{- 1}_{5}^{4} T ​^{- 1}_{6}^{0} T = \\ [\begin{matrix} R_{3}^{T} R_{2}^{T} R_{1}^{T} R & R_{3}^{T} R_{2}^{T} R_{1}^{T} p - R_{3}^{T} R_{2}^{T} R_{1}^{T} p_{1} - R_{3}^{T} R_{2}^{T} p_{2} - R_{3}^{T} p_{3} \\ 0 & 1 \end{matrix}] \end{array}

(6)

Finally, from (4)˜(6), we obtain

R_{4} R_{5} R_{6} = R_{3}^{T} R_{2}^{T} R_{1}^{T} R

(7)

\begin{array}{l} R_{4} R_{5} p_{6} + R_{4} p_{5} + p_{4} = R_{3}^{T} R_{2}^{T} R_{1}^{T} p - \\ R_{3}^{T} R_{2}^{T} R_{1}^{T} p_{1} - R_{3}^{T} R_{2}^{T} p_{2} - R_{3}^{T} p_{3} \end{array}

(8)

By using the symbol processing software Maple©, equations (7) and (8) lead to

s_{1} p_{x} - c_{1} p_{y} = 0

(9)

s_{23} (p_{z} + a_{2} s_{2} - d_{1}) - c_{23} (c_{1} p_{x} + s_{1} p_{y} - a_{2} c_{2} - a_{1}) = - a_{3}

(10)

s_{23} (c_{1} p_{x} + s_{1} p_{y} - a_{2} c_{2} - a_{1}) + c_{23} (p_{z} + a_{2} s_{2} - d_{1}) = - d_{4}

(11)

c_{4} s_{5} = c_{23} c_{1} a_{x} + c_{23} s_{1} a_{y} - s_{23} a_{z}

(12)

s_{4} s_{5} = s_{1} a_{x} - c_{1} a_{y}

(13)

- c_{5} = - s_{23} c_{1} a_{x} - s_{23} s_{1} a_{y} - c_{23} a_{z}

(14)

s_{5} c_{6} = - s_{23} c_{1} n_{x} - s_{23} c_{1} n_{y} - c_{23} n_{z}

(15)

- s_{5} s_{6} = - s_{23} c_{1} o_{x} - s_{23} s_{1} o_{y} - c_{23} o_{z}

(16)

where s₂₃ = sin(θ₂ +θ₃), c₂₃ = cos(θ₂ +θ₃).

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 p·p and p·a 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 θ₁.

Step 2:

Square both sides of (10) and (11), respectively, then add together. We get

\begin{array}{l} (d_{1} - p_{z}) s_{2} - (c_{1} p_{x} + s_{1} p_{y} - a_{1}) c_{2} = \\ [a_{3} ​^{2} + d_{4} ​^{2} - a_{2} ​^{2} - {(c_{1} p_{x} + s_{1} p_{y} - a_{1})}^{2} - {(d_{1} - p_{z})}^{2}] / 2 a_{2} \end{array}

(17)

For each θ₁ we can get two solutions of θ₂, i.e., four solutions of θ₂ in total.

Step 3:

By (10), one solution of θ₃ corresponding to each θ₂, i.e., four solutions of θ₃ can be obtained.

Step 4:

When we solve θ₄ 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 θ₅ ≠ 0°, then θ₄ = atan2(s₄s₅, c₄s₅) and atan2(–s₄s₅, – c₄s₅) for each set of θ₁, θ₂ and θ₃;

By (1) and (2), we can obtain

_{6}^{3} T =_{4}^{3} T_{5}^{4} T_{6}^{5} T = [\begin{matrix} c_{4} c_{5} c_{6} + s_{4} s_{6} & - c_{4} c_{5} s_{6} + s_{4} c_{6} & c_{4} s_{5} & 0 \\ s_{4} c_{5} c_{6} - c_{4} c_{6} & - s_{4} c_{5} s_{6} - c_{4} c_{6} & s_{4} s_{5} & 0 \\ s_{5} c_{6} & - s_{5} s_{6} & - c_{5} & d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}]

(18)

If θ₅ = 0°, i.e., QJ-I is in wrist singularity, (18) can be written as

_{6}^{3} T = [\begin{matrix} \cos (θ_{4} - θ_{6}) & \sin (θ_{4} - θ_{6}) & 0 & 0 \\ \sin (θ_{4} - θ_{6}) & - \cos (θ_{4} - θ_{6}) & 0 & 0 \\ 0 & 0 & - 1 & d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}]

(19)

Now that the fourth and the sixth joint axes of QJ-I are overlapped, the value of θ₄ can be chosen arbitrarily in theory. It is worth noting that from (19) we find that the transformation matrix relates solely to (θ₄–θ₆), 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 θ₄ and θ₆ 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 θ₅ corresponding to each set of θ₁, θ₂, θ₃ and θ₄.

Step 6: By (15) and (16), one solution of θ₆ can be obtained corresponding to each set of θ₁, θ₂, θ₃ and θ₅. So far, we have obtained all eight closed-form solutions of θ₁, θ₂, θ₃, θ₄, θ₅ and θ₆. 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

_{6}^{0} T = [\begin{matrix} - 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 \end{matrix}]

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.

Table 2.

IK solutions of QJ-I

No.	θ₁/(°)	θ₂/(°)	θ₃/(°)	θ₄/(°)	θ₅/(°)	θ₆/(°)
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

Figure 2.

Orientation Simulation of QJ-I

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.

Table 3.

Performance comparisons

	Main means	Extraneous root	Average time cost
Ours	matrix blocking	has not	6.753μs
ITM	inverse transformation	has	13.645μs
VDP	vector dot product	has	8.816μs

As shown in Table 3, by our scheme, it just took an average time of 6.753 μs 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.

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.

References

Siciliano

Khatib

(2008), Springer Handbook of Robotics, Springer.

Chapelle

Bidaud

(2004), Closed form solutions for inverse kinematics approximation of general 6R manipulators, Mechanism and Machine Theory, 39(3): 323–338.

González-Palacios

M. A.

(2013), The unified orthogonal architecture of industrial serial manipulators, Robotics and Computer-Integrated Manufacturing, 29(1): 257–271.

Lenarcic

Husty

(2012), Latest Advances in Robot Kinematics, Springer.

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): 487–461.

Wang

S. X.

Xing

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.

Tarokh

Keerthi

Lee

(2010), Classification and characterization of inverse kinematics solutions for anthropomorphic manipulators, Robotics and Autonomous Systems, 58(1): 115–120.

Qiao

S. G.

Liao

Q. Z.

Wei

S. M.

H. J.

(2010), Inverse kinematic analysis of the general 6R serial manipulators based on double quaternions, Mechanism and Machine Theory, 45(2): 193–199.

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

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

Ang

M. H.

Jr (2009), Singularity robust algorithm in serial manipulators, Robotics and Computer-Integrated Manufacturing, 25(1): 122–134.

12.

Liu

H. S.

Lai

X. B.

W. X.

(2013), Time-optimal and jerk-continuous trajectory planning for robot manipulators with kinematic constraints, Robotics and Computer-Integrated Manufacturing, 29(2): 309–317.