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

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.


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 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.

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. The direct kinematics (DK) of QJ-I can be described as 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: where s i = sinθ i , c i = cosθ i , σ i = sinα i , τ i = cosα i , i = 1, 2, …, 6. θ1, θ2, …, θ6 are the six unknown variables to be solved.
Corresponding to (1), the IK of QJ-I can be written as 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 , then its inverse can be obtained By property (i), we find that R, Ri, Ri -1 , R -1 , R T 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 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.

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

T T T T T T T (4)
By using the properties (i) and (ii) we get Then the left side of (4) can be written as

T T T T R R R R R R R p R R R p R R p R p
By using the symbol processing software Maple©, equations (7) and (8) where s 23 = sin(θ2 +θ3), c 23 = cos(θ2 +θ3).
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.

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 2: Square both sides of (10) and (11), respectively, then add together. We get For each θ1 we can get two solutions of θ2, i.e., four solutions of θ2 in total.
Step 3: By (10), one solution of θ3 corresponding to each θ2, i.e., four solutions of θ3 can be obtained.
Step 4: When we solve θ4 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 θ5 ≠ 0°, then θ4 = atan2(s 4 s 5 , c 4 s 5 ) and atan2(−s 4 s 5 , − c 4 s 5 ) for each set of θ1, θ2 and θ3; By (1) and (2), we can obtain If θ5 = 0°, i.e., QJ-I is in wrist singularity, (18) can be written as Now that the fourth and the sixth joint axes of QJ-I are overlapped, the value of θ4 can be chosen arbitrarily in theory. It is worth noting that from (19) we find that the transformation matrix relates solely to (θ4−θ6), 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 θ4 and θ6 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 6: By (15) and (16), one solution of θ6 can be obtained corresponding to each set of θ1, θ2, θ3 and θ5. So far, we have obtained all eight closed-form solutions of θ1, θ2, θ3, θ4, θ5 and θ6. 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-productbased approach in [5]. This advantage excuses the IK solving from both verifying and matching the roots, which further enhances the efficiency of the algorithm.

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  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 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.

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.