A real-time inverse kinematics solution based on joint perturbation for redundant manipulators

Aiming at the problem that the calculation of the inverse kinematics solution of redundant manipulators is very time-consuming, this paper presents a real-time method based on joint perturbation and joint motion priority. The method first seeks the pose nearest to the target pose in the manipulator’s pose set through finetuning all the joints with different angle deviations at the same time and then regards this pose as the starting one to perform iterative calculations until the error between the current pose and the target pose is less than the predetermined error, thus obtaining the inverse kinematics solution corresponding to the target pose. This method can avoid the pseudo-inverse calculations of the Jacobian matrix and significantly reduce the solving complexity. Two types of manipulators are taken as examples to validate the proposed method. Under the premise that the manipulator motion trajectory is satisfied, the Jacobian pseudo-inverse method and the proposed method are both adopted to solve the inverse kinematics. Simulations and comparisons show that the proposed method has better real-time performance, and the joint motions can be flexibly controlled by setting different joint motion priorities. This method can make the work cycle faster and improve the production efficiency of redundant manipulators in real applications.


Introduction
Until the present, in the manufacturing industry, traditional industrial manipulators with 6 or fewer degrees of freedom (DOFs) have been widely used in such tasks as transporting, assembling, spraying, and welding. However, with the diversification of production environments and task requirements, manipulators are required to have more flexible motions and obstacle avoidance capability. Therefore, manipulators with redundant DOFs have gradually attracted people's favour for higher flexibility and adaptability compared to traditional manipulators (Aouache et al., 2019;Akbaripour and Masehian, 2017;Omrcen and Zlajpah, 2007). The additional DOFs have led to redundant manipulators being able to complete tasks such as obstacle avoidance, joint limit avoidance, and load distribution optimization while performing specific production tasks (Liu et al., 2017), such as welding and polishing. To speed up the work cycle and improve production efficiency, higher requirements are imposed on the real-time performance of manipulators (Kalra et al., 2006;Kuhlemann et al., 2016). Compared to non-redundant ma-nipulators, redundant manipulators have null space, and thus the inverse kinematics solution is not unique, and the process is more complicated and time-consuming. As a bottleneck problem of motion control and path planning, the high realtime inverse kinematics solution of redundant manipulators has caused much concern (Dasgupta et al., 2009;Kouabon et al., 2020).
Until the present, numerous research studies have focused on the inverse kinematics solution of redundant manipulators, and the methods can be divided into three types: geometric methods, numerical methods, and iterative methods (Jia et al., 2014). Transforming the inverse kinematics problem into a geometric one in a two-dimensional plane or a three-dimensional space, geometric methods (Tian et al., 2020;Faria et al., 2018;Zaplana et al., 2018) are suitable for redundant manipulators with a simple structure or special structure. Numerical methods include the Jacobian pseudoinverse method (Hayashibe et al., 2006;Park et al., 2018), the gradient projection method (Zu et al., 2005), the generalized Jacobian method (Kumar et al., 2010), and the weighted Published by Copernicus Publications. 222 Q. Xu and Q. Zhan: A real-time inverse kinematics solution minimum norm method (Maciejewski and Klein, 1985;Mohamed et al., 2009). However, these methods usually make equivalent approximations to the kinematics models of redundant manipulators during the solving process, inevitably leading to error accumulation. The high non-linearity and complex coupling of redundant manipulators have also resulted in the use of iterative algorithms to obtain inverse kinematics solutions. Korein and Badler (1982) first proposed the use of the iterative method to calculate the inverse kinematics solution of redundant manipulators. The limitation of this kind of method, however, is that the convergence of the solving results depends on the setting of the initial pose. Damas (2012) proposed an algorithm that can learn the input-output relationship of kinematics online and is suitable for solving forward and inverse kinematics of manipulators. Ayusawa and Nakamura (2012) proposed a fast inverse kinematics method that calculates the gradient vector with the Newton-Euler iterative algorithm to obtain the inverse solution. In addition, some methods based on genetic algorithms (Zeng et al., 2018), neural networks (Xia and Wang, 2001), particle swarm optimization (PSO) (Umar et al., 2019), and differential evolution (DE) (Lopezfranco et al., 2018) can also solve the inverse kinematics problem of redundant manipulators in an iterative manner. However, these iterative algorithms require extensive calculations and are very timeconsuming.
The inverse kinematics algorithms of redundant manipulators are mainly evaluated from indexes such as real-time, reliability, stability, and universality (Tolani et al., 2000). The geometric method is fast and accurate for solving inverse kinematics, but it depends on the specific structure and geometric characteristics of manipulators and lacks universality. The numerical method and iterative method have the advantages of strong universality as they can be applied to hyperredundant manipulators as well as constraints and objective functions in optimization problems, but they are not suitable for online real-time path planning because of extensive calculations, poor real-time performance, and low precision. This paper presents a real-time method based on joint perturbation to obtain the inverse kinematics solution of redundant manipulators, which has no constraints on the DOFs, sizes, and structures of redundant manipulators. The remainder of this paper is organized as follows. In Sect. 2, the inverse kinematics method based on joint perturbation is introduced. In Sect. 3, simulations and comparisons are provided to validate the proposed method. Finally, conclusions of the paper are given in Sect. 4.

Kinematics model of redundant manipulators
If the dimension of a manipulator's work space is m, and the dimension of its joint space is n, when m < n, the manipula-tor has n − m redundant DOFs. The kinematics model of the redundant manipulator can be established with the modified D-H method (Denavit-Hartenberg; Lv and Liu, 2016). First, the D-H coordinate systems of all the links are established. The z axis of coordinate system {i} is collinear with the axis of joint i, which is located at the beginning of link i. The x axis coincides with the common perpendicular of joint i and joint i + 1, pointing from joint i to joint i + 1, and the y axis can be determined by the right-hand rule. The D-H transformation parameters are determined according to the relations of neighbouring frames, and then the homogeneous transformation matrix of frame {i} relative to frame {i − 1} i i−1 T can be attained.
where a i−1 represents the length of the common perpendicular to the axes of joint i−1 and joint i, α i−1 represents the angle between the axes of joint i −1 and joint i, d i−1 represents the offset of link i relative to link i − 1, and θ i represents the rotation angle of link i relative to link i − 1 around the axis of joint i. Multiplying all the homogeneous transformation matrices in order results in the forward kinematics model n 0 T of the manipulator.
Define q = (θ 1 θ 2 . . .θ n ) T as the joint variables of the manipulator, and the pose of the manipulator is P = f (q). When the current joint angles of the manipulator are q 0 = (θ 01 θ 02 . . .θ 0n ) T , the current pose of the manipulator P 0 can be obtained by The target pose of the manipulator P t can be determined according to task requirements. The inverse kinematics solution of the manipulator is used to obtain the joint variables q t according to the initial pose P 0 and the target pose P t . A redundant manipulator with n DOFs has numerous inverse solutions, from which the optimal one should be selected in an actual motion control. This paper presents a real-time inverse kinematics method based on joint perturbation, in which two definitions including the joint motion priority and joint perturbation coefficient matrix are used.

Joint motion priority
First, define the motion priority of each joint as k 1 , k 2 , k 3 . . .k n−1 , k n , 0 ≤ k i ≤ 1(i = 1, 2, 3. . .n). The larger the k i is, the higher the motion priority joint i has. When k i = 0, the motion of joint i will be limited to zero. The priority of each joint can be determined according to task requirements. For example, when a manipulator's motion is planned based on energy optimization, according to the rule of "moving big joints less and small joints more", the joint motion priorities can be set as k 1 ≤ k 2 ≤ k 3 . . .k n−1 ≤ k n . When a manipulator's motion is planned based on time minimization, the joint motion priorities can be set as k 1 ≥ k 2 ≥ k 3 . . .k n−1 ≥ k n to follow the rule of "moving big joints more and small joints less" (Xu et al., 2010). In particular, when a manipulator's motion is not constrained by tasks, the joint motion priorities can be set as k 1 = k 2 = k 3 . . .k n−1 = k n so that all the joints have the same motion priority.

Joint perturbation coefficient matrix
Suppose the initial joint variables of a manipulator are q 0 = (θ 01 θ 02 . . .θ 0n ) T , which correspond to the initial pose P 0 of the manipulator. A very small angle δθ (δθ > 0) is set as the basic perturbation angle, and the perturbation angle of joint i is θ i . Thus, the perturbation angle of joint i is the product of the motion priority k i and the basic perturbation angle δθ.
A rotary joint has two types of possible motions: counterclockwise or clockwise, so the coefficient of δθ can be 1 or −1. When the perturbation direction of joint i is counterclockwise, θ i = k i δθ; when the perturbation direction is clockwise, Hence, the perturbation variable q of a manipulator with n DOFs has 2 n possible combinations. Each combination of the coefficients of the basic perturbation angle δθ of n joints constitutes a column vector, and all the column vectors constitute a matrix K, which is defined as the joint perturbation coefficient matrix. Figure 1 shows the joint perturbation combinations of three types of manipulators with 1 DOF, 2 DOFs, and 3 DOFs, and the construction of the joint perturbation coefficient matrix is introduced as follows.
When n = 1, the manipulator has only 1 DOF and two types of possible motions, as shown in Fig. 1a. Therefore, the joint perturbation coefficient matrix of the manipulator is When n = 2, the manipulator has 2 DOFs and four types of possible motions, as shown in Fig. 1b, and the joint perturbation coefficient matrix is When n = 3, the manipulator has 3 DOFs and eight types of possible motions, as shown in Fig. 1c, and the joint perturbation coefficient matrix is By this analogy, the joint perturbation coefficient matrix of a redundant manipulator with n DOFs is In Eq. (9), K n ∈ R n×2 n , the elements of the first 2 n−1 columns and the first row are all 1, and the elements of the last 2 n−1 columns and the first row are all −1. When the joint motion priorities are determined, the joint perturbation coefficient matrix considering the joint motion priority can be obtained by multiplying row i of matrix K n by k i . Similar to rotary joints, the motion priority of prismatic joints is set as k p , and the basic perturbation displacement as δd, and the coefficient of δd can be 1 or −1. When d = k p δd, the perturbation direction is positive, and when d = −k p δd, the perturbation direction is negative. Therefore, this algorithm can be used for manipulators with prismatic joints.

Calculation of basic perturbation angle
The basic perturbation angle not only determines the amplitude of joint perturbation in an iterative calculation process but also plays a decisive role in the solving speed and accuracy of the algorithm. The smaller the basic perturbation angle, the higher the accuracy of the solution. As the number of iterations increases, the solving speed will decrease. Therefore, it is necessary to take a bigger basic perturbation angle while satisfying the solution accuracy. For a manipulator with n DOFs, the link lengths are L = [l 1 , l 2 . . .l n ], and 224 Q. Xu and Q. Zhan: A real-time inverse kinematics solution   then l = l 1 + l 2 + . . . + l n , and the maximum displacement s of the manipulator position for one perturbation should satisfy where e is the maximum error allowed by the solution accuracy or predetermined. In this paper, the calculation formula of the basic perturbation angle is defined as

Inverse kinematics solution method
The joint angles after perturbation q ti (i =1, 2, 3. . . 2 n ) can be calculated according to the joint perturbation coefficient matrix and the basic perturbation angle δθ .
In Eq. (12), the vector K n (i) is the column i of the joint perturbation coefficient matrix K n , so the pose of the manipulator P i (i = 1, 2, 3 . . . 2 n ) can be calculated by 2 n different poses (P 1 , P 2 . . .P 2 n ) can be obtained through one single operation for a redundant manipulator with n DOFs, and then the distance D i = − − → P i P t between each pose P i and the target pose P t can be calculated. The minimum of D i can be determined by comparisons, D imin = min − − → P i P t . Set the pose nearest to the target pose P t as P imin , so the joint variable corresponding to P imin is q timin . Further, set the pose P imin as the starting pose and reassign P 0 and q 0 as P 0 = P imin , q 0 = q timin . The iterative calculations will continue for the target pose P t until min − − → P i P t ≤ e. When the iterative calculations end, the calculated joint variable q t is the inverse solution of the target pose P t . The process of the iterative calculations from the starting pose P 0 to the target pose P t is shown in Fig. 2.

The main steps of the method
The main steps of the joint perturbation method are summarized as follows: 1. Establish the kinematics model of redundant manipulators with n DOFs.
3. Determine the target pose P t and the priority of each joint motion k i according to actual task requirements.
4. Construct the joint perturbation coefficient matrix K n , determine the basic perturbation angle δθ, and obtain the perturbation angle of each joint q = k i · δθ .
6. Find all the poses P i for the 2 n kinds of possibilities by using the forward kinematics model of the manipulator.
7. Calculate the distance D i = − − → P i P t between the current manipulator pose P i and the target pose P t .

Obtain D imin by comparison and verify whether
D imin ≤ e is satisfied. If D imin ≤ e, the current joint angles are the joint variables q t corresponding to the target pose P t . If D imin > e, set the pose corresponding to joint variables q timin as the starting pose and repeat steps 5-7.
A flow chart of the method is shown in Fig. 3.

Simulation verification of a manipulator with 3 DOFs
A planar manipulator with 3 DOFs is used to verify the inverse kinematics method based on joint perturbation. The schematic diagram of the manipulator mechanism is shown in Fig. 4. If only the position of the manipulator is considered, the mechanism is a planar manipulator with 1 redundant DOF. The manipulator's position coordinates (x, y) can be obtained with the forward kinematics model.
Let the motion priorities of the three joints be k 1 = 0.6, k 2 = 0.8, and k 3 = 1.0, and set the allowable error e = 0.01 mm and the basic perturbation angle as δθ = 4.34 × 10 −4• . When the joint motion priorities are considered, the coefficient matrix K 3 of the manipulator is Suppose that the manipulator takes t = 10s to move from P 0 (x 0 , y 0 ) to the target position P t (x t , y t ) along a straight line at a constant speed. The position changes of the manipulator x = x t − x 0 = −100 and y = y t − y 0 = −200 mm are given, and thus, the distance between the initial position and the target position is Therefore, the velocity of the manipulator end is v = l/t. The linear interpolation is performed in the motion path, and the interpolation period is taken as t = 10 ms. So, the number of interpolations h is Therefore, in an interpolation period, the displacement changes in x and y directions are as follows: Calculate the path point P b (x b , y b ) of the manipulator from the initial position P 0 (x 0 , y 0 ) to the target position P t (x t , y t ).
The first path point P 1 (x 1 , y 1 ) is determined, where x 1 = x 0 + dx and y 1 = y 0 + dy. Next, q m = (θ m1 , θ m2 , θ m3 ) and P m (x m , y m ) can be obtained through iterative calculations from the initial position P 0 (x 0 , y 0 ) to the first path point P 1 (x 1 , y 1 ). Further, q m and P m (x m , y m ) are assigned to q 0 and P 0 (x 0 , y 0 ), respectively, and then iterated to the second path point P 2 (x 2 , y 2 ), where x 2 = x 1 + dx, y 2 = y 1 + dy. The iterative calculations and solutions will be repeated until D = (x m − x t ) 2 + (y m − y t ) 2 ≤ e.
In the process of iteration from the initial position P 0 (x 0 , y 0 ) to the target position P t (x t , y t ), the joint angles corresponding to each path point can be calculated. The joint   motion curves of the entire process are obtained after the iteration is completed, as shown in Fig. 6, where the abscissa represents the motion time and the ordinate represents the corresponding joint angles. Figure 6 shows that the angle variations of the three joints are θ 1 = 8.96, θ 2 = 26.39, and θ 3 = 40.33 • , which comply with the setting of the motion priorities of the three joints, and the manipulator can move smoothly in the joint space.
Change the motion priority of each joint, by setting k 1 = 0.2, k 2 = 0.6, k 3 = 1, k 1 = k 2 = k 3 = 1, and k 1 = 0, k 2 = k 3 = 1 while keeping other parameters unchanged. The effect of the joint motion priorities on joint motions is obtained through simulations. The joint motion curves under different motion priorities are shown in Figs. 7,8,and 9. After analysing the joint motion curves in the four cases, the angle variation of each joint under different motion priorities can be obtained, as shown in Fig. 10, where the abscissa represents the three joint motions of the manipulator and the ordinate represents the magnitude of the angle variation of each joint. The specific values are shown in Table 1. In this  Table, θ 01 , θ 02 , and θ 03 represent the initial joint angles; θ t1 , θ t2 , and θ t3 represent the joint angles corresponding to the target position; and θ 1 , θ 2 , and θ 3 represent the angle variations of three joints, θ i = |θ ti − θ 0i | (i = 1, 2, 3).
The effect of the joint motion priority on joint motions can be obtained according to Table 1 and Fig. 11. Figure 11a and b show that the motion priority of the first joint decreases by k 1 = 0.4, and the variation of joint motion changes from 8.96 to 1.65 • with a decrease of 81.58 %; the motion priority of the second joint decreases by k 2 = 0.2, and the corresponding variation of joint motion changes from 26.39 to  Figure 11. Motion trajectories of the manipulator with different motion priorities. (a) k 1 = 0.6, k 2 = 0.8, π/2 0 0 θ 6 7 −π/2 0 d 4 θ 7 5.19 • with a decrease of 80.33 %, while the variation of the third joint motion changes from 40.33 to 63.73 • with an increase of 58.02 %. Figure 11a and c show that the motion priority of the first joint increases by k 1 = 0.4, and the variation of joint motion changes from 8.96 to 13.81 • with an increase of 54.13 %; the motion priority of the second joint increases by k 2 = 0.2, and the corresponding variation of joint motion changes from 26.39 to 36.71 • with an increase of 39.11 %, while the variation of the third joint motion changes from 40.33 to 26.86 • with a decrease of 33.34 %.  Figure 11d shows that when the motion priority of the first joint is 0 (k 1 = 0), the first joint will remain stationary, and the corresponding motion is completed by the second joint and the third joint.

Simulation verification of a manipulator with 7 DOFs
To verify the universality of the algorithm, a redundant manipulator with 7 DOFs (KUKA iiwa) is used for simulation verification. First, the model of the redundant manipulator is obtained using the D-H method, as shown in Fig. 12, where  Table 2. Set the initial joint values as q 0 = (0, π/6, 0, −π/3, 0, 0, 0), and the coordinates of the manipulator end can be obtained by the forward kinematics, P 0 = [63.301142.5] mm, as shown in Fig. 13.
The manipulator will take t = 10 s to move from the initial position to the target position P t = [263.3 − 400542.5]. Set the allowable error as e = 0.01 mm, and calculate the basic perturbation angle δθ = 3.92 × 10 −5• .
The interpolation period is set as t = 10 ms, and thus the number of interpolations is h = 1000. Therefore, the displacement changes in x, y, and z directions are dx = 0.2, dy = −0.4, and dz = −0.6 mm, respectively. The simulations are divided into five groups for comparisons, and the joint motion priorities of each group are set as shown in Table 3.
Using the motion priorities of the five groups while keeping other parameters unchanged, the joint motion curves under different motion priorities are shown in Figs. 14, 15, 16, 17, and 18.
The angle variation of each joint under different motion priorities can be obtained, as shown in Table 4. In this table, θ 0i represents the initial joint angles, θ ti represents the joint angles corresponding to the target position, and θ i represents the angle variations of seven joints, θ i = |θ ti − θ 0i | (i = 1, 2. . .7).
Because the joint motion priorities are the same in Group 1, the motion angle of each joint is relatively averaged. In Group 2, the motion priorities of big joints are relatively low, while the motion priorities of small joints are relatively high. Then, the motion amplitudes of the first and the second joints decrease significantly. Comparing Groups 1 and 3 shows that the motion priority of the second joint is reduced to 0.5, and the corresponding angle variation is decreased from 35.47 to 7.15 • . In Groups 4 and 5, the motion priorities of the first joint and the third joint are set as 0, so the corresponding joints remain stationary.
Solving inverse kinematics of redundant manipulators with 3 DOFs and 7 DOFs shows consistent simulation results. The higher the relative joint motion priority, the bigger the joint motion angle, and in contrast, the lower the relative joint motion priority, the smaller the joint motion angle. When the joints have the same priority, the motion angle of each joint will be relatively averaged. In particular, when the motion priority of a joint is zero, the joint motion angle is zero, too. Therefore, the motion range of a joint can be controlled by changing the motion priority. Hence, the joint perturbation method can be used to deal with joint limit avoidance when calculating the inverse kinematics solution of a redundant manipulator. For a manipulator with n DOFs, assume that the motion range limit of joint i is q min,i , q max,i , then the joint angles q i satisfy the following conditions: Normally, the constraint interval of a joint angle is symmetrical; that is q max,i = −q min,i . Then, the motion priority of joint i can be defined as Therefore, k i approaches 0 when q i approaches the joint limit; k i = 1 when q i = q max,i +q min,i 2 .

Simulation experiment based on CoppeliaSim
Take the manipulator with 7 DOFs, KUKA iiwa, to perform the same motion task of Sect. 3.2 in CoppeliaSim, of which simulation results are widely acknowledged to have a high consistency with actual experiments. The joint motion ranges of the manipulator are shown in Table 5, and the realtime joint motion priorities are calculated according to formula (25). The joint curves are obtained as shown in Fig. 19a. All joints do not exceed their motion ranges. The joint curves were imported into CoppeliaSim as the driving function to complete the experimental tasks, as shown in Fig. 19b. During the movement, the manipulator ran smoothly and completed the task of a linear trajectory.

Real-time characteristic analyses
The Jacobian pseudo-inverse method is widely used to solve the inverse kinematics of redundant manipulators. The realtime characteristic comparisons will be conducted between it and the proposed method. The inverse kinematics solution of the redundant manipulator with n DOFs iṡ whereq is the joint velocity,ṗ is the velocity of the manipulator, J + is the generalized inverse matrix of J , α is the scalar coefficient, z ∈ R n×1 is an arbitrary vector, and α(I − J + J )z is the null space vector which represents the self-motion of the manipulator in the null space and satisfies αJ (I − J + J )z = 0. All the conditions and motion tasks of the two manipulators (3 DOFs and 7 DOFs) are the same as those of the two in Sect. 3.1 and 3.2 respectively. In this paper, the minimum  norm solution of joint velocity,q = J +ṗ , is adopted, and the joint motion curves are obtained as shown in Figs. 20 and 21.
For the planar manipulator with 3 DOFs and the space manipulator with 7 DOFs, different solution accuracy values (e = 0.01 and e = 0.001 mm, respectively) are set. The solving times of the two methods are calculated with MATLAB, and the results are shown in Table 6. According to Table 6, both methods can reach the microsecond level, and the solving time of the perturbation method t j is shorter than that of the Jacobian pseudo-inverse method t m . So, the joint perturbation method has better efficiency and real-time performance than the Jacobian pseudo-inverse method. However, with the improvement of the solution accuracy and the increase of the degrees of freedom, the calculation times of the both methods increase significantly, and the preponderance of the joint perturbation method over the Jacobian pseudoinverse method has a downward trend.

Conclusions
The inverse kinematics solution of redundant manipulators usually takes a considerable amount of time to calculate. Aiming at improving the solving efficiency of inverse kinematics, this paper first defines the concepts of joint motion priority and joint perturbation coefficient matrix, then proposes an algorithm based on joint perturbation to solve the inverse solution of redundant manipulators. The feasibility and real-time characteristics of the algorithm were verified through simulations of two types of redundant manipulators with 3 DOFs and 7 DOFs, respectively. The following conclusions are obtained: 1. The magnitude of motion priority determines the level of joint motion priority. When a redundant manipulator performs a specific task, a higher joint motion priority leads to a bigger joint motion angle, while a lower joint motion priority results in a smaller joint motion angle.  In particular, when the joint motion priority is zero, the joint motion angle is also zero. Therefore, the motion of a joint can be controlled by setting different joint motion priorities, which can be used in obstacle avoidance and joint limit avoidance.
2. The proposed algorithm has universality and has no restrictions on the structure and DOFs of manipulators. Moreover, the solving process of this algorithm only involves the forward kinematics, which can avoid the pseudo-inverse calculations of the Jacobian matrix and significantly reduce the solving complexity, so it has a better real-time performance than the Jacobian pseudoinverse method. The method can be used in real applications to speed up the work cycle and improve production efficiency.
3. However, the proposed algorithm also has certain limitations, and the calculation load will increase exponen-tially with the increase of the degrees of freedom of a manipulator. In the future, we will continue to study the algorithm in the optimization of joint motion priorities and experimental verifications.