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

1 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 manipulators, 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 real-time 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 pseudo-inverse 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 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 time-consuming.

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

2 Inverse kinematics method based on joint perturbation

2.1 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 manipulator 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-\mathrm{1}}^{i}T$ can be attained.

$$\begin{array}{}\text{(1)}& {}_{i-\mathrm{1}}^{i}T=\left[\begin{array}{cccc}\cos {\mathit{\theta }}_i& -\sin {\mathit{\theta }}_i& \mathrm{0}& a_{i-\mathrm{1}}\\ \sin {\mathit{\theta }}_i\cos {\mathit{\alpha }}_{i-\mathrm{1}}& \cos {\mathit{\theta }}_i\cos {\mathit{\alpha }}_{i-\mathrm{1}}& -\sin {\mathit{\alpha }}_{i-\mathrm{1}}& -\sin {\mathit{\alpha }}_{i-\mathrm{1}}{d}_i\\ \sin {\mathit{\theta }}_i\sin {\mathit{\alpha }}_{i-\mathrm{1}}& \cos {\mathit{\theta }}_i\sin {\mathit{\alpha }}_{i-\mathrm{1}}& \cos {\mathit{\alpha }}_{i-\mathrm{1}}& \cos {\mathit{\alpha }}_{i-\mathrm{1}}{d}_i\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right],\end{array}$$

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 ${}_{\mathrm{0}}^{n}T$ of the manipulator.

$$\begin{array}{}\text{(2)}& {}_{\mathrm{0}}^{n}T{=}_{\mathrm{0}}^{\mathrm{1}}{T}_{\mathrm{1}}^{\mathrm{2}}T{\mathrm{\dots }}_{n-\mathrm{2}}^{n-\mathrm{1}}{T}_{n-\mathrm{1}}^{n}T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]\end{array}$$

Define $q={\left({\mathit{\theta }}_{\mathrm{1}}{\mathit{\theta }}_{\mathrm{2}}\mathrm{\dots }{\mathit{\theta }}_n\right)}^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_{\mathrm{0}}={\left({\mathit{\theta }}_{\mathrm{01}}{\mathit{\theta }}_{\mathrm{02}}\mathrm{\dots }{\mathit{\theta }}_{\mathrm{0}n}\right)}^T$, the current pose of the manipulator P₀ can be obtained by

$$\begin{array}{}\text{(3)}& P_{\mathrm{0}}=f\left(q_{\mathrm{0}}\right).\end{array}$$

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

2.2 Joint motion priority

First, define the motion priority of each joint as k₁, k₂, k₃…k_n−1, k_n, $\mathrm{0}\le k_i\le \mathrm{1}(i=\mathrm{1},\mathrm{2},\mathrm{3}\mathrm{\dots }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_{\mathrm{1}}\le k_{\mathrm{2}}\le k_{\mathrm{3}}\mathrm{\dots }{k}_{n-\mathrm{1}}\le k_n$. When a manipulator's motion is planned based on time minimization, the joint motion priorities can be set as $k_{\mathrm{1}}\ge k_{\mathrm{2}}\ge k_{\mathrm{3}}\mathrm{\dots }{k}_{n-\mathrm{1}}\ge 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_{\mathrm{1}}=k_{\mathrm{2}}=k_{\mathrm{3}}\mathrm{\dots }{k}_{n-\mathrm{1}}=k_n$ so that all the joints have the same motion priority.

2.3 Joint perturbation coefficient matrix

Suppose the initial joint variables of a manipulator are $q_{\mathrm{0}}={\left({\mathit{\theta }}_{\mathrm{01}}{\mathit{\theta }}_{\mathrm{02}}\mathrm{\dots }{\mathit{\theta }}_{\mathrm{0}n}\right)}^T$, which correspond to the initial pose P₀ 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 δθ.

$$\begin{array}{}\text{(4)}& \mathrm{\Delta }{\mathit{\theta }}_i=k_i\mathit{\delta }\mathit{\theta }\end{array}$$

A rotary joint has two types of possible motions: counter-clockwise or clockwise, so the coefficient of δθ can be 1 or −1. When the perturbation direction of joint i is counter-clockwise, Δθ_i=k_iδθ; when the perturbation direction is clockwise, $\mathrm{\Delta }{\mathit{\theta }}_i=-k_i\mathit{\delta }\mathit{\theta }$.

$$\begin{array}{}\text{(5)}& \mathrm{\Delta }q=\left(\begin{array}{c}\mathrm{\Delta }{\mathit{\theta }}_{\mathrm{1}}\\ \mathrm{\Delta }{\mathit{\theta }}_{\mathrm{2}}\\ \mathrm{⋮}\\ \mathrm{\Delta }{\mathit{\theta }}_n\end{array}\right)=\left(\begin{array}{c}\pm k_{\mathrm{1}}\mathit{\delta }\mathit{\theta }\\ \pm k_{\mathrm{2}}\mathit{\delta }\mathit{\theta }\\ \mathrm{⋮}\\ \pm k_n\mathit{\delta }\mathit{\theta }\end{array}\right)\end{array}$$

Hence, the perturbation variable Δq of a manipulator with n DOFs has 2ⁿ 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

$$\begin{array}{}\text{(6)}& K_{\mathrm{1}}=\left[\begin{array}{cc}\mathrm{1}& -\mathrm{1}\end{array}\right].\end{array}$$

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

$$\begin{array}{}\text{(7)}& K_{\mathrm{2}}=\left[\begin{array}{cccc}\mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}\\ \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}\end{array}\right]=\left[\begin{array}{cc}\mathrm{1}& -\mathrm{1}\\ K_{\mathrm{1}}& K_{\mathrm{1}}\end{array}\right].\end{array}$$

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

$$\begin{array}{}\text{(8)}& \begin{array}{rl}{K}_{\mathrm{3}}& =\left[\begin{array}{cccccccc}\mathrm{1}& \mathrm{1}& \mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}& -\mathrm{1}& -\mathrm{1}\\ \mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}& \mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}\\ \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}\end{array}\right]\\ & =\left[\begin{array}{cc}\mathrm{1}& -\mathrm{1}\\ K_{\mathrm{2}}& K_{\mathrm{2}}\end{array}\right].\end{array}\end{array}$$

Figure 1 Joint perturbation combinations of manipulator with n DOFs. (a) n=1. (b) n=2. (c) n=3.

Figure 2 Iterative calculation process of the joint perturbation method. (a) The first iteration. (b) The second iteration.

By this analogy, the joint perturbation coefficient matrix of a redundant manipulator with n DOFs is

$$\begin{array}{}\text{(9)}& \begin{array}{rl}{K}_n& =\left[\begin{array}{cc}\begin{array}{ccc}\mathrm{1}& \mathrm{\dots }& \mathrm{1}\end{array}& \begin{array}{ccc}-\mathrm{1}& \mathrm{\dots }& -\mathrm{1}\end{array}\\ K_{n-\mathrm{1}}& K_{n-\mathrm{1}}\end{array}\right]\\ & =\left[\begin{array}{cc}\mathrm{1}& -\mathrm{1}\\ K_{n-\mathrm{1}}& K_{n-\mathrm{1}}\end{array}\right].\end{array}\end{array}$$

In Eq. (9), $K_n\in R^{n\times {\mathrm{2}}^n}$, the elements of the first 2ⁿ⁻¹ columns and the first row are all 1, and the elements of the last 2ⁿ⁻¹ 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 $\mathrm{\Delta }d=-k_{\mathrm{p}}\mathit{\delta }d$, the perturbation direction is negative. Therefore, this algorithm can be used for manipulators with prismatic joints.

Figure 3 The flow chart of the algorithm based on joint perturbation.

2.4 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=\left[l_{\mathrm{1}},l_{\mathrm{2}}\mathrm{\dots }{l}_n\right]$, and then $l_{\mathrm{\Sigma }}=l_{\mathrm{1}}+l_{\mathrm{2}}+\mathrm{\dots }+l_n$, and the maximum displacement Δs of the manipulator position for one perturbation should satisfy

$$\begin{array}{}\text{(10)}& \mathrm{\Delta }s=l_{\mathrm{\Sigma }}\mathit{\delta }\mathit{\theta }+(l_{\mathrm{\Sigma }}-l_{\mathrm{1}})\mathit{\delta }\mathit{\theta }+\mathrm{\dots }+l_n\mathit{\delta }\mathit{\theta }\le e,\end{array}$$

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

$$\begin{array}{}\text{(11)}& \mathit{\delta }\mathit{\theta }={\displaystyle \frac{e}{l_{\mathrm{1}}+\mathrm{2}{l}_{\mathrm{2}}+\mathrm{\dots }+i{l}_i+\mathrm{\dots }+n{l}_n}}.\end{array}$$

Figure 4 The schematic diagram of a planar manipulator.

Figure 5 Parameters setting of a planar manipulator with 3 DOFs.

2.5 Inverse kinematics solution method

The joint angles after perturbation q_ti (i=1, 2, 3… 2ⁿ) can be calculated according to the joint perturbation coefficient matrix and the basic perturbation angle δθ.

$$\begin{array}{}\text{(12)}& q_{ti}=q_{\mathrm{0}}+\mathrm{\Delta }{q}_i=\left(\begin{array}{c}{\mathit{\theta }}_{\mathrm{01}}\\ {\mathit{\theta }}_{\mathrm{02}}\\ \mathrm{⋮}\\ {\mathit{\theta }}_{\mathrm{0}n}\end{array}\right)+\mathit{\delta }\mathit{\theta }\cdot K_n\left(i\right)\end{array}$$

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ⁿ) can be calculated by

$$\begin{array}{}\text{(13)}& P_i=f\left(q_{ti}\right)=f(q_{\mathrm{0}}+\mathrm{\Delta }{q}_i).\end{array}$$

2ⁿ different poses ($P_{\mathrm{1}},P_{\mathrm{2}}\mathrm{\dots }{P}_{{\mathrm{2}}^n})$ can be obtained through one single operation for a redundant manipulator with n DOFs, and then the distance $D_i=\left|\overrightarrow{P_i{P}_{\mathrm{t}}}\right|$ 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\left|\overrightarrow{P_i{P}_{\mathrm{t}}}\right|$. 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₀ and q₀ as $P_{\mathrm{0}}=P_{imin}$, $q_{\mathrm{0}}=q_{timin}$. The iterative calculations will continue for the target pose P_t until $min\left|\overrightarrow{P_i{P}_{\mathrm{t}}}\right|\le 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₀ to the target pose P_t is shown in Fig. 2.

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

2. Identify the current joint variables $q_{\mathrm{0}}={\left({\mathit{\theta }}_{\mathrm{01}}{\mathit{\theta }}_{\mathrm{02}}\mathrm{\dots }{\mathit{\theta }}_{\mathrm{0}n}\right)}^T$ and calculate the corresponding manipulator pose P₀.

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 $\mathrm{\Delta }q=k_i\cdot \mathit{\delta }\mathit{\theta }$.

5. Calculate the joint angles after perturbation $q_{ti}=q_{\mathrm{0}}+\mathrm{\Delta }{q}_i$ ($i=\mathrm{1},\mathrm{2},\mathrm{3}\mathrm{\dots }{\mathrm{2}}^n$).

6. Find all the poses P_i for the 2ⁿ kinds of possibilities by using the forward kinematics model of the manipulator.

7. Calculate the distance $D_i=\left|\overrightarrow{P_i{P}_{\mathrm{t}}}\right|$ between the current manipulator pose P_i and the target pose P_t.

8. Obtain D_imin by comparison and verify whether $D_{imin}\le e$ is satisfied. If $D_{imin}\le 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.

Figure 6 Motion curves of the joints and the manipulator end. (a) The first joint. (b) The second joint. (c) The third joint. (d) Trajectory of the manipulator end.

Figure 7 Motion curves of the three joints (k₁=0.2, k₂=0.6, k₃=1). (a) The first joint. (b) The second joint. (c) The third joint.

3 Simulations and analyses

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

$$\begin{array}{}\text{(14)}& \left\{\begin{array}{c}x=L_{\mathrm{1}}\cos {\mathit{\theta }}_{\mathrm{1}}+L_{\mathrm{2}}\cos ({\mathit{\theta }}_{\mathrm{1}}+{\mathit{\theta }}_{\mathrm{2}})+L_{\mathrm{3}}\cos ({\mathit{\theta }}_{\mathrm{1}}+{\mathit{\theta }}_{\mathrm{2}}+{\mathit{\theta }}_{\mathrm{3}})\\ y=L_{\mathrm{1}}\sin {\mathit{\theta }}_{\mathrm{1}}+L_{\mathrm{2}}\sin ({\mathit{\theta }}_{\mathrm{1}}+{\mathit{\theta }}_{\mathrm{2}})+L_{\mathrm{3}}\sin ({\mathit{\theta }}_{\mathrm{1}}+{\mathit{\theta }}_{\mathrm{2}}+{\mathit{\theta }}_{\mathrm{3}})\end{array}\right.\end{array}$$

The joint perturbation coefficient matrix of the manipulator is

$$\begin{array}{}\text{(15)}& K_{\mathrm{3}}=\left[\begin{array}{cccccccc}\mathrm{1}& \mathrm{1}& \mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}& -\mathrm{1}& -\mathrm{1}\\ \mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}& \mathrm{1}& \mathrm{1}& -\mathrm{1}& -\mathrm{1}\\ \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}& \mathrm{1}& -\mathrm{1}\end{array}\right].\end{array}$$

The motion priorities of the three joints are k₁, k₂, and k₃. The coefficient matrix considering the joint motion priorities is

$$\begin{array}{}\text{(16)}& K_{\mathrm{3}}=\left[\begin{array}{cccccccc}{k}_{\mathrm{1}}& k_{\mathrm{1}}& k_{\mathrm{1}}& k_{\mathrm{1}}& -k_{\mathrm{1}}& -k_{\mathrm{1}}& -k_{\mathrm{1}}& -k_{\mathrm{1}}\\ k_{\mathrm{2}}& k_{\mathrm{2}}& -k_{\mathrm{2}}& -k_{\mathrm{2}}& k_{\mathrm{2}}& k_{\mathrm{2}}& -k_{\mathrm{2}}& -k_{\mathrm{2}}\\ k_{\mathrm{3}}& -k_{\mathrm{3}}& k_{\mathrm{3}}& -k_{\mathrm{3}}& k_{\mathrm{3}}& -k_{\mathrm{3}}& k_{\mathrm{3}}& -k_{\mathrm{3}}\end{array}\right].\end{array}$$

Set the link lengths of the planar redundant manipulator as L₁=300, L₂=240, and L₃=180 mm, and the current joint angles are θ₀₁=60, ${\mathit{\theta }}_{\mathrm{02}}=-\mathrm{30}$, and ${\mathit{\theta }}_{\mathrm{03}}=-\mathrm{30}{}^{\circ }$. The current manipulator position coordinates P₀(x₀,y₀) can be calculated with the current joint angles, with x₀=537.8461 and y₀=379.8076 mm. The target position coordinates are set to be P_t(437.8461,179.8076), as shown in Fig. 5.

Figure 8 Motion curves of the three joints ($k_{\mathrm{1}}=k_{\mathrm{2}}=k_{\mathrm{3}}=\mathrm{1})$. (a) The first joint. (b) The second joint. (c) The third joint.

Figure 9 Motion curves of the three joints (k₁=0, $k_{\mathrm{2}}=k_{\mathrm{3}}=\mathrm{1}$). (a) The first joint. (b) The second joint. (c) The third joint.

Let the motion priorities of the three joints be k₁=0.6, k₂=0.8, and k₃=1.0, and set the allowable error e=0.01 mm and the basic perturbation angle as $\mathit{\delta }\mathit{\theta }=\mathrm{4.34}\times {\mathrm{10}}^{-\mathrm{4}}$^∘. When the joint motion priorities are considered, the coefficient matrix K₃ of the manipulator is

$$\begin{array}{}\text{(17)}& K_{\mathrm{3}}=\left[\begin{array}{cccccccc}\mathrm{0.6}& \mathrm{0.6}& \mathrm{0.6}& \mathrm{0.6}& -\mathrm{0.6}& -\mathrm{0.6}& -\mathrm{0.6}& -\mathrm{0.6}\\ \mathrm{0.8}& \mathrm{0.8}& -\mathrm{0.8}& -\mathrm{0.8}& \mathrm{0.8}& \mathrm{0.8}& -\mathrm{0.8}& -\mathrm{0.8}\\ \mathrm{1.0}& -\mathrm{1.0}& \mathrm{1.0}& -\mathrm{1.0}& \mathrm{1.0}& -\mathrm{1.0}& \mathrm{1.0}& -\mathrm{1.0}\end{array}\right].\end{array}$$

Therefore, the perturbation angle of each joint can be calculated as

$$\begin{array}{}\text{(18)}& \begin{array}{rl}\mathrm{\Delta }q& =\left(\begin{array}{c}\mathrm{\Delta }{\mathit{\theta }}_{\mathrm{1}}\\ \mathrm{\Delta }{\mathit{\theta }}_{\mathrm{2}}\\ \mathrm{\Delta }{\mathit{\theta }}_{\mathrm{3}}\end{array}\right)=\left(\begin{array}{c}\pm k_{\mathrm{1}}\mathit{\delta }\mathit{\theta }\\ \pm k_{\mathrm{2}}\mathit{\delta }\mathit{\theta }\\ \pm k_{\mathrm{3}}\mathit{\delta }\mathit{\theta }\end{array}\right)\\ & =\left(\begin{array}{c}\pm \mathrm{2.60}\times {\mathrm{10}}^{-\mathrm{4}}{}^{\circ }\\ \pm \mathrm{3.47}\times {\mathrm{10}}^{-\mathrm{4}}{}^{\circ }\\ \pm \mathrm{4.34}\times {\mathrm{10}}^{-\mathrm{4}}{}^{\circ }\end{array}\right).\end{array}\end{array}$$

Suppose that the manipulator takes t=10s to move from P₀(x₀,y₀) to the target position P_t(x_t,y_t) along a straight line at a constant speed. The position changes of the manipulator $\mathrm{\Delta }x=x_{\mathrm{t}}-x_{\mathrm{0}}=-\mathrm{100}$ and $\mathrm{\Delta }y=y_{\mathrm{t}}-y_{\mathrm{0}}=-\mathrm{200}$ mm are given, and thus, the distance between the initial position and the target position is

$$\begin{array}{}\text{(19)}& l=\sqrt{(x_{\mathrm{t}}-x_{\mathrm{0}}{)}^{\mathrm{2}}+(y_{\mathrm{t}}-y_{\mathrm{0}}{)}^{\mathrm{2}}}=\mathrm{223.6068}\mathrm{mm}.\end{array}$$

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

$$\begin{array}{}\text{(20)}& h={\displaystyle \frac{t}{\mathrm{\Delta }t}}=\mathrm{1000}.\end{array}$$

Therefore, in an interpolation period, the displacement changes in x and y directions are as follows:

$$\begin{array}{}\text{(21)}& \mathrm{d}x={\displaystyle \frac{\mathrm{\Delta }x}{h}}={\displaystyle \frac{x_{\mathrm{t}}-x_{\mathrm{0}}}{h}}=-\mathrm{0.1}\mathrm{mm}\end{array}$$

$$\begin{array}{}\text{(22)}& \mathrm{d}y={\displaystyle \frac{\mathrm{\Delta }y}{h}}={\displaystyle \frac{y_{\mathrm{t}}-y_{\mathrm{0}}}{h}}=-\mathrm{0.2}\mathrm{mm}.\end{array}$$

Calculate the path point P_b(x_b,y_b) of the manipulator from the initial position P₀(x₀,y₀) to the target position P_t(x_t,y_t).

$$\begin{array}{}\text{(23)}& \left\{\begin{array}{c}{x}_b=x_{\mathrm{0}}+b\cdot \mathrm{d}x\\ y_b=y_{\mathrm{0}}+b\cdot \mathrm{d}y\end{array}\right.(b=\mathrm{1},\mathrm{2},\mathrm{3},\mathrm{\dots },h)\end{array}$$

The first path point P₁(x₁,y₁) is determined, where $x_{\mathrm{1}}=x_{\mathrm{0}}+\mathrm{d}x$ and $y_{\mathrm{1}}=y_{\mathrm{0}}+\mathrm{d}y$. Next, $q_m=({\mathit{\theta }}_{m\mathrm{1}},{\mathit{\theta }}_{m\mathrm{2}},{\mathit{\theta }}_{m\mathrm{3}})$ and P_m(x_m,y_m) can be obtained through iterative calculations from the initial position P₀(x₀,y₀) to the first path point P₁(x₁,y₁). Further, q_m and P_m(x_m,y_m) are assigned to q₀ and P₀(x₀,y₀), respectively, and then iterated to the second path point P₂(x₂,y₂), where $x_{\mathrm{2}}=x_{\mathrm{1}}+\mathrm{d}x$, $y_{\mathrm{2}}=y_{\mathrm{1}}+\mathrm{d}y$. The iterative calculations and solutions will be repeated until $D=\sqrt{(x_m-x_{\mathrm{t}}{)}^{\mathrm{2}}+(y_m-y_{\mathrm{t}}{)}^{\mathrm{2}}}\le e$.

Figure 10 Variations of joint motions with different motion priorities.

In the process of iteration from the initial position P₀(x₀,y₀) 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.

Table 1 Comparisons of joint motions with different motion priorities.

Figure 11 Motion trajectories of the manipulator with different motion priorities. (a) k₁=0.6, k₂=0.8, k₃=1.0. (b) k₁=0.2, k₂=0.6, k₃=1.0. (c) $k_{\mathrm{1}}=k_{\mathrm{2}}=k_{\mathrm{3}}=\mathrm{1}$. (d) k₁=0, $k_{\mathrm{2}}=k_{\mathrm{3}}=\mathrm{1}$.

Figure 6 shows that the angle variations of the three joints are Δθ₁=8.96, Δθ₂=26.39, and $\mathrm{\Delta }{\mathit{\theta }}_{\mathrm{3}}=\mathrm{40.33}{}^{\circ }$, 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₁=0.2, k₂=0.6, k₃=1, $k_{\mathrm{1}}=k_{\mathrm{2}}=k_{\mathrm{3}}=\mathrm{1}$, and k₁=0, $k_{\mathrm{2}}=k_{\mathrm{3}}=\mathrm{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, θ₀₁, θ₀₂, and θ₀₃ represent the initial joint angles; θ_t1, θ_t2, and θ_t3 represent the joint angles corresponding to the target position; and Δθ₁, Δθ₂, and Δθ₃ represent the angle variations of three joints, $\mathrm{\Delta }{\mathit{\theta }}_i=\left|{\mathit{\theta }}_{ti}-{\mathit{\theta }}_{\mathrm{0}i}\right|$ ($i=\mathrm{1},\mathrm{2},\mathrm{3}$).

Figure 12 D–H model of a manipulator with 7 DOFs.

Table 2 D–H parameters of a manipulator with 7 DOFs.

Figure 13 Initial position of iiwa manipulator.

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₁=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₂=0.2, and the corresponding variation of joint motion changes from 26.39 to 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₁=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₂=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₁=0), the first joint will remain stationary, and the corresponding motion is completed by the second joint and the third joint.

3.2 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 d₁=340, d₂=400, d₃=400, and d₄=126.6 mm. The D–H parameters are shown in Table 2.

Set the initial joint values as $q_{\mathrm{0}}=\left(\mathrm{0},\mathit{\pi }/\mathrm{6},\mathrm{0},-\mathit{\pi }/\mathrm{3},\mathrm{0},\mathrm{0},\mathrm{0}\right)$, and the coordinates of the manipulator end can be obtained by the forward kinematics, P₀=[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_{\mathrm{t}}=\left[\mathrm{263.3}-\mathrm{400542.5}\right]$. Set the allowable error as e=0.01 mm, and calculate the basic perturbation angle $\mathit{\delta }\mathit{\theta }=\mathrm{3.92}\times {\mathrm{10}}^{-\mathrm{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, $\mathrm{d}y=-\mathrm{0.4}$, and $\mathrm{d}z=-\mathrm{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.

Table 3 Setting of joint motion priorities.

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.

Figure 14 Motion curves of the joints and the manipulator ($k_{\mathrm{1}}=k_{\mathrm{2}}=k_{\mathrm{3}}=k_{\mathrm{4}}=k_{\mathrm{5}}=k_{\mathrm{6}}=k_{\mathrm{7}}=\mathrm{1.0})$. (a) Joint motion curves. (b) Trajectory of the manipulator.

Figure 15 Motion curves of the joints and the manipulator (k₁=0.4, k₂=0.5, k₃=0.6, k₄=0.7, k₅=0.8, k₆=0.9, k₇=1.0). (a) Joint motion curves. (b) Trajectory of the manipulator.

Figure 16 Motion curves of the joints and the manipulator (k₂=0.5, $k_{\mathrm{1}}=k_{\mathrm{3}}=k_{\mathrm{4}}=k_{\mathrm{5}}=k_{\mathrm{6}}=k_{\mathrm{7}}=\mathrm{1.0}$). (a) Joint motion curves. (b) Trajectory of the manipulator.

Figure 17 Motion curves of the joints and the manipulator (k₁=0, $k_{\mathrm{2}}=k_{\mathrm{3}}=k_{\mathrm{4}}=k_{\mathrm{5}}=k_{\mathrm{6}}=k_{\mathrm{7}}=\mathrm{1.0}$). (a) Joint motion curves. (b) Trajectory of the manipulator.

Figure 18 Motion curves of the joints and the manipulator (k₃=0, $k_{\mathrm{1}}=k_{\mathrm{2}}=k_{\mathrm{4}}=k_{\mathrm{5}}=k_{\mathrm{6}}=k_{\mathrm{7}}=\mathrm{1.0}$). (a) Joint motion curves. (b) Trajectory of the manipulator.

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, $\mathrm{\Delta }{\mathit{\theta }}_i=\left|{\mathit{\theta }}_{ti}-{\mathit{\theta }}_{\mathrm{0}i}\right|(i=\mathrm{1},\mathrm{2}\mathrm{\dots }\mathrm{7})$.

Table 4 Comparisons of joint motions with different motion priorities.

Table 5 The joint motion ranges of the manipulator.

Figure 19 Joint motion curves and simulation process. (a) Joint motion curves. (b) Initial and target poses.

Figure 20 Motion curves of the joints and the manipulator (3 DOFs). (a) The first joint. (b) The second joint. (c) The third joint. (d) Trajectory of the manipulator.

Figure 21 Motion curves of the joints and the manipulator (7 DOFs). (a) Joint motion curves. (b) Trajectory of the manipulator.

Table 6 Solving time for the Jacobian pseudo-inverse method and the perturbation method.

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 $\left[q_{min,i},q_{max,i}\right]$, then the joint angles q_i satisfy the following conditions:

$$\begin{array}{}\text{(24)}& q_{min,i}\le q_i\le q_{max,i}.\end{array}$$

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

$$\begin{array}{}\text{(25)}& k_i={\displaystyle \frac{min\left\{\left|q_{max,i}-q_i\right|,\left|q_i-q_{min,i}\right|\right\}}{\left(\frac{q_{max,i}-q_{min,i}}{\mathrm{2}}\right)}}.\end{array}$$

Therefore, k_i approaches 0 when q_i approaches the joint limit; k_i=1 when $q_i=\frac{q_{max,i}+q_{min,i}}{\mathrm{2}}$.

3.3 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 real-time 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.

3.4 Real-time characteristic analyses

The Jacobian pseudo-inverse method is widely used to solve the inverse kinematics of redundant manipulators. The real-time characteristic comparisons will be conducted between it and the proposed method. The inverse kinematics solution of the redundant manipulator with n DOFs is

$$\begin{array}{}\text{(26)}& \dot{q}=J^{+}\dot{p}+\mathit{\alpha }(I-J^{+}J)z,\end{array}$$

where $\dot{q}$ is the joint velocity, $\dot{p}$ is the velocity of the manipulator, J⁺ is the generalized inverse matrix of J, α is the scalar coefficient, $z\in R^{n\times \mathrm{1}}$ is an arbitrary vector, and $\mathit{\alpha }(I-J^{+}J)z$ is the null space vector which represents the self-motion of the manipulator in the null space and satisfies $\mathit{\alpha }J(I-J^{+}J)z=\mathrm{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, $\dot{q}=J^{+}\dot{p}$, 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 pseudo-inverse method has a downward trend.

4 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 pseudo-inverse 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 exponentially 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.