A General Approach Based on Newton’s Method and Cyclic Coordinate Descent Method for Solving the Inverse Kinematics

Featured Application

The new approach can be used to solve the inverse kinematics of revolute and prismatic joint robots with any number of degrees of freedom as well as any configurations.

Abstract

The inverse kinematics of robot manipulators is a crucial problem with respect to automatically controlling robots. In this work, a Newton-improved cyclic coordinate descent (NICCD) method is proposed, which is suitable for robots with revolute or prismatic joints with degrees of freedom of any arbitrary number. Firstly, the inverse kinematics problem is transformed into the objective function optimization problem, which is based on the least-squares form of the angle error and the position error expressed by the product-of-exponentials formula. Thereafter, the optimization problem is solved by combining Newton’s method with the improved cyclic coordinate descent (ICCD) method. The difference between the proposed ICCD method and the traditional cyclic coordinate descent method is that consecutive prismatic joints and consecutive parallel revolute joints are treated as a whole in the former for the purposes of optimization. The ICCD algorithm has a convenient iterative formula for these two cases. In order to illustrate the performance of the NICCD method, its simulation results are compared with the well-known Newton–Raphson method using six different robot manipulators. The results suggest that, overall, the NICCD method is effective, accurate, robust, and generalizable. Moreover, it has advantages for the inverse kinematics calculations of continuous trajectories.

1. Introduction

Solving typical robotics problems, such as trajectory planning [1] and motion control [2], requires that forward and inverse kinematics problems be addressed. The former involves calculating the position and orientation of a robot’s end-effector frame from its joint values, which can easily be solved by using the matrix method of analysis [3]. The latter involves determining the joint variables corresponding to a given end-effector position and orientation. Indeed, the inverse kinematics problem is more complex than the forward kinematics problem because of the existence of nonlinear and multiple solutions [4]. Generally, two types of solutions exist: analytical solutions and numerical solutions. Iterative numerical methods can be applied if the inverse kinematics problem in question does not have any closed-form solutions and if it does not satisfy the Pieper criterion [5]. Even in cases where closed-form solutions do exist, iterative numerical methods are often used to improve the accuracy of these solutions.

From the 1980s, the inverse kinematics problem has attracted the attention of a large number of experts and researchers, all of whom have made remarkable achievements with respect to providing an adequate solution. Tsai and Morgan equated the kinematics problem of the general six-degree-of-freedom robot manipulators to that of a system of eight second-degree equations with eight unknowns solved numerically by polynomial continuation [6]. The authors in [6] conjectured that this problem has at most 16 solutions, the first conclusive proof of which was given in [7]. In order to improve the real-time performance of the solution, Manocha and Canny presented an algorithm for efficient inverse kinematics for a general six revolute (6R) robot manipulator, which performs both symbolic preprocessing and matrix computations and reduces the problem in question to computing the eigendecomposition of a matrix [8]. The generality of this algorithm, however, is limited since it can only be applied to 6R robot manipulators. In order to improve the generality of the inverse kinematics algorithm, some general approaches are proposed. The representative algorithms are the Newton–Raphson (NR) method [9] and the damped least-squares approach [10,11], which are all based on the inverse Jacobian matrix. The common drawback of this kind of algorithm is that it is sensitive to the initial solution. In order to solve this problem, Goldenberg et al. proposed a combined optimization method, which is based on a combination of two nonlinear programming techniques [12]. More specifically, the proposed method uses the cyclic coordinate descent (CCD) method [13] in order to rapidly find a feasible point approximate to the true solution and then the Broyden–Fletcher–Shanno variable metric method [13] in order to obtain a solution accurate to the desired degree of precision. Recently, some novel algorithms have been proposed to solve the inverse kinematics problem of robot manipulators. Kelemen et al. presented an algorithm for the control of kinematically-redundant manipulators considering three secondary tasks: a joint limit avoidance task, a kinematic singularities avoidance task, and an obstacle avoidance task [14]. Indeed, this approach is practical and results in a significant decrease in computing time. Zaplana and Basanez proposed an approach for a closed-form solution for the inverse kinematics of redundant manipulators [15]. This approach involves transforming redundant manipulators into non-redundant ones by selecting a set of joints, identifying the redundant ones, and parametrizing the joint variables. Thereafter, several closed-form methods previously developed for non-redundant manipulators can be applied in order to obtain the requisite solutions. This approach, however, is only suitable for manipulators that conform to the Pieper criterion. Qiao et al. [16] and Menini et al. [17] solved the inverse kinematics problem using double quaternions and Lie symmetries, respectively. In addition, other methods based on heuristic search techniques have been developed to solve the inverse kinematics problem, such as neural networks [18,19], genetic algorithms [20,21], and particle swarm optimization [22], as well as a combination of these methods [23,24].

On the basis of prior research, there is rarely a general and effective method for solving inverse kinematics. Therefore, this paper transforms the inverse kinematics problem into the objective function optimization problem, which is based on the least-squares form of the angle error and the position error expressed by the product-of-exponentials (PoE) formula. Then the new general approach that is suitable for the inverse kinematics of revolute and prismatic joint robots with any number of degrees of freedom as well as any configurations is presented.

The rest of the paper is organized in the following manner. Section 2 provides basic and associated knowledge, including the kinematics description of robot manipulators, Newton’s method and the CCD method. Section 3 provides a definition of the objective function with respect to inverse kinematics and then the necessary formulas for Newton’s method and the iterative procedure for the improved cyclic coordinate descent (ICCD) method. At the end of this part, the Newton-improved cyclic coordinate descent (NICCD) method is presented to solve the inverse kinematics problem. Section 4 provides the simulation results, followed by the discussion. Finally, Section 5 provides the conclusions and future work.

2. Preliminaries

Generally, this section introduces the preliminary knowledge used in this article. Firstly, the kinematics description of the robot manipulator is introduced, which plays an important role in deriving the formulas. The iteration process for Newton’s method and the CCD method are then described, thereby laying the foundation for the proposed NICCD method.

2.1. Kinematics Description of Robot Manipulator

Generally speaking, two methods of describing the forward kinematics of open chains currently exist. The first method relies on the Denavit- Hartenberg parameters and the second relies on the PoE formula. Indeed, the latter method has advantages over the former (e.g., no link frames are necessary) [25], and it is, therefore, the preferred choice with respect to representing forward kinematics.

The PoE formula used to describe the forward kinematics $(\mathit{T}\left(\mathsf{\Theta }\right),\mathsf{\Theta }\in {\mathbb{R}}^n)$ of an open chain with n-degrees of freedom follows Equation (1)

$$\mathit{T}\left(\mathsf{\Theta }\right)=\left[\begin{array}{cc}\mathit{R}\left(\mathsf{\Theta }\right)& \mathit{P}\left(\mathsf{\Theta }\right)\\ \mathbf{0}& 1\end{array}\right]=e^{{\mathit{S}}_{\mathbf{1}}{\theta }_1}\cdots e^{{\mathit{S}}_{\mathbf{n}\mathbf{-}\mathbf{1}}{\theta }_{n-1}}{e}^{{\mathit{S}}_{\mathbf{n}}{\theta }_n}\mathit{M}.$$

(1)

Specifically, Equation (1) is the space form of the PoE formula, referring to the fact that the screw axes are expressed in the fixed space frame, $\mathit{R}\left(\mathsf{\Theta }\right)\in specialorthogonalgroup\left(SO\right(3\left)\right)$, $\mathit{P}\left(\mathsf{\Theta }\right)\in {\mathbb{R}}^{3\times 1}$, $\mathbf{0}$ denotes the ($1\times 3$) null vector.

To calculate the homogeneous transformation matrics $\left(\mathit{T}\right(\mathsf{\Theta }\left)\right)$, which represent both the position and orientation of the end-effector with respect to the base frame, the elements outlined below are required.

(a) The end-effector configuration $\mathit{M}\in$ Special Euclidean Group $\left(SE\right(3\left)\right)$ is defined as

$$\mathit{M}=\left[\begin{array}{cc}{\mathit{R}}_{\mathbf{m}}& {\mathit{P}}_{\mathbf{m}}\\ \mathbf{0}& 1\end{array}\right],$$

(2)

when the robot manipulator is at its home position.

(b) The screw axes are

$${\mathit{S}}_{\mathit{i}}=\left[\begin{array}{c}{\mathbf{\omega }}_{\mathit{i}}\\ {\mathit{v}}_{\mathit{i}}\end{array}\right]={[{\omega }_{i_1},{\omega }_{i_2},{\omega }_{i_3},v_{i_1},v_{i_2},v_{i_3}]}^T,(i=1\cdots n),$$

(3)

where

$${\mathit{v}}_{\mathit{i}}=-{\mathbf{\omega }}_{\mathit{i}}\times {\mathit{q}}_{\mathit{i}}$$

(4)

with ${\mathit{q}}_{\mathit{i}}$ (any arbitrary point on the joint axis i) written in coordinates in the fixed base frame, corresponding to the joint motions when the robot manipulator is at the home position. Since the screw axis ${\mathit{S}}_{\mathit{i}}$ is just a normalized twist, ${\mathit{S}}_{\mathit{i}}$ represents Equation (5)

$${\mathit{S}}_{\mathit{i}}=\left[\begin{array}{cc}{\mathsf{\Omega }}_{\mathit{i}}& {\mathit{v}}_{\mathit{i}}\\ \mathbf{0}& 0\end{array}\right],$$

(5)

where ${\mathsf{\Omega }}_{\mathit{i}}$ is a $3\times 3$ skew-symmetric matrix representation of ${\mathbf{\omega }}_{\mathit{i}}$ and is defined as

$${\mathsf{\Omega }}_{\mathit{i}}=\left[\begin{array}{ccc}0& -{\omega }_{i_3}& {\omega }_{i_2}\\ {\omega }_{i_3}& 0& -{\omega }_{i_1}\\ -{\omega }_{i_2}& {\omega }_{i_1}& 0\end{array}\right].$$

(6)

(c) The joint variables are represented as follows: $\mathsf{\Theta }={[{\theta }_1,{\theta }_2,\cdots ,{\theta }_n]}^T$.

(d) The matrix exponential $e^{{\mathit{S}}_{\mathit{i}}{\theta }_i}$ can be calculated using Equation (7)

$$\begin{array}{c}\hfill e^{{\mathit{S}}_{\mathit{i}}{\theta }_i}=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{i}}\left({\theta }_i\right)& {\mathit{P}}_{\mathit{i}}\left({\theta }_i\right)\\ \mathbf{0}& 1\end{array}\right],\end{array}$$

(7)

where ${\mathit{R}}_{\mathit{i}}\left({\theta }_i\right)$ and ${\mathit{P}}_{\mathit{i}}\left({\theta }_i\right)$ are calculated as

$$\begin{array}{cc}\hfill {\mathit{R}}_{\mathit{i}}\left({\theta }_i\right)& =e^{{\mathsf{\Omega }}_{\mathit{i}}{\theta }_i}=\mathit{I}+{\mathsf{\Omega }}_{\mathit{i}}\sin {\theta }_i+{{\mathsf{\Omega }}_{\mathit{i}}}^2(1-\cos {\theta }_i),\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\mathit{P}}_{\mathit{i}}\left({\theta }_i\right)& =\left((\mathit{I}+{\mathsf{\Omega }}_{\mathit{i}}^2){\theta }_i+{\mathsf{\Omega }}_{\mathit{i}}(1-\cos {\theta }_i)-{\mathsf{\Omega }}_{\mathit{i}}^2\sin {\theta }_i\right){\mathit{v}}_{\mathit{i}},\hfill \end{array}$$

(9)

where $\mathit{I}$ denotes the ($3\times 3$) identity matrix.

The formulas listed in this section will be used for calculating the formulas in the NICCD method.

2.2. Newton’s Method

An effective way to solve nonlinear optimization problems is to use Newton’s method. This method has quadratic convergence, and it uses both the first-order and second-order partial derivatives of the objective function, thus taking into account gradient changes. Therefore, it can comprehensively determine the appropriate search direction, and it has a more rapid convergence rate than the gradient method [26]. In particular, some scholars have improved the truncation criterion of Newton’s method, which avoids “over-solving” of the Newton equation as much as possible. The representative method is the heuristic adaptive truncation criterion proposed by [27,28].

Although Newton’s method requires a Hessian matrix, it is an appropriate method for solving low-dimensional optimization problems, such as the inverse kinematics of robot manipulators. The iterative formula for finding the local minimum value of the objective function ($f\left(\mathsf{\Theta }\right),\mathsf{\Theta }\in {\mathbb{R}}^n$) using Newton’s method is given by Equation (10)

$${\mathsf{\Theta }}^{(k+1)}={\mathsf{\Theta }}^{\left(k\right)}-\mathrm{pinv}\left(\mathit{H}\left({\mathsf{\Theta }}^{\left(k\right)}\right)\right)▽f\left({\mathsf{\Theta }}^{\left(k\right)}\right),$$

(10)

where $\mathrm{pinv}\left(\right)$ is the pseudoinverse, and where $▽f\left({\mathsf{\Theta }}^{\left(k\right)}\right)$ is the gradient of the k-th iteration of the objective function, which is defined as

$$▽f\left({\mathsf{\Theta }}^{\left(k\right)}\right)={[\frac{\partial f\left({\mathsf{\Theta }}^{\left(k\right)}\right)}{\partial {\theta }_1},\frac{\partial f\left({\mathsf{\Theta }}^{\left(k\right)}\right)}{\partial {\theta }_2},\cdots ,\frac{\partial f\left({\mathsf{\Theta }}^{\left(k\right)}\right)}{\partial {\theta }_n}]}^T.$$

(11)

Moreover, $\mathit{H}\left({\mathsf{\Theta }}^{\left(k\right)}\right)$ must be positive definite matrix for the Hessian matrix of the k-th iteration of the objective function, which is expressed as Equation (12)

$$\mathit{H}\left({\mathsf{\Theta }}^{\left(k\right)}\right)={▽}^{2}f\left({\mathsf{\Theta }}^{\left(k\right)}\right)=\left[\begin{array}{cccc}\frac{{\partial }^{2}f}{\partial {\theta }_1^2}& \frac{{\partial }^{2}f}{\partial {\theta }_1\partial {\theta }_2}& \cdots & \frac{{\partial }^{2}f}{\partial {\theta }_1\partial {\theta }_n}\\ \frac{{\partial }^{2}f}{\partial {\theta }_2\partial {\theta }_1}& \frac{{\partial }^{2}f}{\partial {\theta }_2^2}& \cdots & \frac{{\partial }^{2}f}{\partial {\theta }_2\partial {\theta }_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{{\partial }^{2}f}{\partial {\theta }_n\partial {\theta }_1}& \frac{{\partial }^{2}f}{\partial {\theta }_n\partial {\theta }_2}& \cdots & \frac{{\partial }^{2}f}{\partial {\theta }_n^2}\end{array}\right].$$

(12)

Newton’s method for finding the optimal solution (${\mathsf{\Theta }}^{*}$) consists of the step-by-step procedure outlined below.

Step 1: determine the gradient $▽f\left({\mathsf{\Theta }}^{\left(k\right)}\right)$ (see Equation (11)).

Step 2: verify convergence with the final solution as follows,

$$∥▽f\left({\mathsf{\Theta }}^{\left(k\right)}\right)∥⩽ϵ,$$

(13)

where $ϵ$ is a threshold value supplied by the user. If Equation (13) holds, i.e., then ${\mathsf{\Theta }}^{*}={\mathsf{\Theta }}^{\left(k\right)}$ is the optimal solution, then terminate the process; otherwise, proceed to the next step.

Step 3: determine the Hessian matrix (see Equation (12)) and ${\mathsf{\Theta }}^{k+1}$ (see Equation (10)). Then, let $k=k+1$ and return to Step 1.

2.3. The CCD Method

The CCD method, also known as the univariate search technique, is a dimension reduction method for unconstrained optimization problems. Its iteration process requires that one searches alternately along different coordinate directions. Indeed, the CCD method is a simpler method of establishing the search direction than the objective function derivative.

Although the convergence rate of the CCD method is slow for high-dimensional optimization problems, it is permissible with respect to the inverse kinematics problem of robot manipulators, and it can reduce the objective function at each iteration.

The CCD method process requires finding the optimal solution for one variable at a time while keeping the remaining $n-1$ variable unchanged. The last point of the previous one-dimensional search is the starting point of this one-dimensional search. The iterative formula for finding the local minimum value of the objective function ($f\left(\mathsf{\Theta }\right),\mathsf{\Theta }\in {\mathbb{R}}^n$) by the CCD method is Equation (14)

$${\mathsf{\Theta }}_i^k={\mathsf{\Theta }}_{i-1}^k+{\alpha }_i^{\left(k\right)}{{\mathit{D}}_{\mathit{i}}}^{\left(k\right)},(i=1,2,\cdots ,n),$$

(14)

where ${{\mathit{D}}_{\mathit{i}}}^{\left(k\right)}$ is the search direction, defined as

$${{\mathit{D}}_{\mathit{i}}}^{\left(k\right)}={[0,\cdots ,1,\cdots ,0]}^T,$$

(15)

in which the component of the i-th coordinate direction is 1 and the remaining components are 0, Moreover, ${\alpha }_i^{\left(k\right)}$ is the optimal step length in the i-th iteration of the k-th round, which is written as Equation (16)

$${\alpha }_i^{\left(k\right)}=\arg \underset{{\alpha }_i^{\left(k\right)}}{\min }f({\mathsf{\Theta }}_{i-1}^k+{\alpha }_i^{\left(k\right)}{{\mathit{D}}_{\mathit{i}}}^{\left(k\right)}).$$

(16)

The CCD method for finding the optimal solution (${\mathsf{\Theta }}^{*}$) consists of the following step-by-step procedure.

Step 1: let i = 1.

Step 2: determine the optimal step length ${\alpha }_i^{\left(k\right)}$ (see Equation (16)) and ${\mathsf{\Theta }}_i^k$ (see Equation (14)).

Step 3: check whether the iteration round is over as follows:

$$i=n.$$

(17)

If Equation (17) holds, i.e., then proceed to the next step, else let $i=i+1$ and return to step 2.

Step 4: verify convergence with the final solution as follows,

$$∥{\mathsf{\Theta }}_n^{\left(k\right)}-{\mathsf{\Theta }}_0^{\left(k\right)}∥⩽ϵ,$$

(18)

where $ϵ$ is a threshold value supplied by the user. If Equation (18) holds, i.e., then ${\mathsf{\Theta }}^{*}={\mathsf{\Theta }}_n^{\left(k\right)}$ is the optimal solution, then terminate the process; otherwise, let ${{\mathit{D}}_{\mathit{i}}}^{(k+1)}={{\mathit{D}}_{\mathit{i}}}^{\left(k\right)}(i=1,2,\cdots ,n),{\mathsf{\Theta }}_0^{(k+1)}={\mathsf{\Theta }}_n^{\left(k\right)},k=k+1$ and return to Step 1.

The CCD method does not require the derivative of the objective function in the search process; rather, it only requires the objective function value information. Moreover, it can be used to analytically find the optimal value in each search direction, such as the single joint variable optimization for the objective function proposed in this paper.

3. Numerical Methods for Nonlinear Kinematic Equations

In this section, the inverse kinematics problem is first transformed into the problem of how to get the value of ${\mathsf{\Theta }}^{*}$ in order to minimize the objective function, which is based on the least-squares form of the angle error and the position error expressed by the PoE formula. Accordingly, the equivalence of these two problems is proven. Then, the necessary formulas for Newton’s method and the ICCD methods are given. Finally, for the NICCD method, the iterative procedure required to find the local minimum value of the objective function with respect to inverse kinematics is proposed.

3.1. Definition of Objective Function in Inverse Kinematics

For an open chain with n-degrees of freedom with respect to forward kinematics $(\mathit{T}\left(\mathsf{\Theta }\right),\mathsf{\Theta }\in {\mathbb{R}}^n)$, the inverse kinematics problem can be stated as follows: Given a homogeneous transformation matrix, $\mathit{E}\in SE\left(3\right)$ which is defined as follows

$$\mathit{E}=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{e}}& {\mathit{P}}_{\mathit{e}}\\ \mathbf{0}& 1\end{array}\right],$$

(19)

find solutions $\left({\mathsf{\Theta }}^{*}\right)$ that satisfy

$$T\left({\mathsf{\Theta }}^{*}\right)=\mathit{E}.$$

(20)

As previously mentioned, this problem can be transformed into the problem of how to get the value of ${\mathsf{\Theta }}^{*}$ in order to minimize the objective function ($f\left(\mathsf{\Theta }\right)$) of inverse kinematics. Formally, the problem can be defined as Equation (21)

$${\mathsf{\Theta }}^{*}=\arg \underset{\mathsf{\Theta }}{min}f\left(\mathsf{\Theta }\right),$$

(21)

where the objective function ($f\left(\mathsf{\Theta }\right)$) of inverse kinematics is written as

$$\begin{array}{cc}\hfill f\left(\mathsf{\Theta }\right)& =\mathrm{RErr}\left(\mathsf{\Theta }\right)+\lambda \mathrm{PErr}\left(\mathsf{\Theta }\right)\hfill \\ \hfill & =\mathrm{trace}\left({(\mathit{R}\left(\mathsf{\Theta }\right)-{\mathit{R}}_{\mathit{e}})}^T(\mathit{R}\left(\mathsf{\Theta }\right)-{\mathit{R}}_{\mathit{e}})\right)+\lambda {\left(\mathit{P}\left(\mathsf{\Theta }\right)-{\mathit{P}}_{\mathit{e}}\right)}^T\left(\mathit{P}\left(\mathsf{\Theta }\right)-{\mathit{P}}_{\mathit{e}}\right),\hfill \end{array}$$

(22)

where $\lambda >0$ is a scale factor, and $\mathit{R}\left(\mathsf{\Theta }\right)$ and $\mathit{P}\left(\mathsf{\Theta }\right)$ are expressed as

$$\begin{array}{cc}\hfill \mathit{R}\left(\mathsf{\Theta }\right)& =\prod _{i=1}^{n+1}{\mathit{R}}_{\mathit{i}}\left({\theta }_i\right),\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill \mathit{P}\left(\mathsf{\Theta }\right)& =\sum _{k=0}^n\left(\prod _{i=1}^k{\mathit{R}}_{\mathit{i}}\left({\theta }_i\right)\right){\mathit{P}}_{\mathit{k}+\mathbf{1}}\left({\theta }_{k+1}\right),\hfill \end{array}$$

(24)

where ${\mathit{R}}_{\mathit{i}}\left({\theta }_i\right),{\mathit{P}}_{\mathit{i}}\left({\theta }_i\right)(i=1,\cdots ,n)$ can be calculated using Equations (8) and (9), respectively, and where ${\mathit{P}}_{\mathbf{n}+\mathbf{1}}\left({\theta }_{n+1}\right),{\mathit{R}}_{\mathbf{n}+\mathbf{1}}\left({\theta }_{n+1}\right)$ are defined as

$$\begin{array}{cc}\hfill {\mathit{P}}_{\mathbf{n}+\mathbf{1}}\left({\theta }_{n+1}\right)& ={\mathit{P}}_{\mathbf{m}},\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\mathit{R}}_{\mathbf{n}+\mathbf{1}}\left({\theta }_{n+1}\right)& ={\mathit{R}}_{\mathbf{m}}.\hfill \end{array}$$

(26)

Equation (22) is based on the least-squares cost function. The first part of Equation (22) represents the sum of the squares of the errors of all elements of the current rotation matrix $\left(\mathit{R}\right(\mathsf{\Theta }\left)\right)$ and the target rotation matrix $\left({\mathit{R}}_{\mathit{e}}\right)$. The second part of Equation (22) represents $\lambda$ times the sum of the errors of all elements of the current displacement matrix $\left(\mathit{P}\right(\mathsf{\Theta }\left)\right)$ and the target displacement matrix $\left({\mathit{P}}_{\mathit{e}}\right)$. Therefore, if a solution does not exist for inverse kinematics, then ${\mathsf{\Theta }}^{*}$ satisfies the required conditions as closely as possible in a least-squares sense. However, if a solution exists for inverse kinematics, then ${\mathsf{\Theta }}^{*}$ exactly satisfies

$$f\left({\mathsf{\Theta }}^{*}\right)=\min f\left(\mathsf{\Theta }\right)=0.$$

(27)

A constructive proof of this claim is provided below.

Proof. 

If a solution of inverse kinematics $\left({\mathsf{\Theta }}^{*}\right)$ exists, then $\mathit{R}\left({\mathsf{\Theta }}^{*}\right)={\mathit{R}}_{\mathit{e}}$ and $\mathit{P}\left({\mathsf{\Theta }}^{*}\right)={\mathit{P}}_{\mathit{e}}$ must be satisfied. So Equation (27) must also be established. On the contrary, if there is ${\mathsf{\Theta }}^{*}$ so that Equation (27) holds, then $\mathit{R}\left({\mathsf{\Theta }}^{*}\right)={\mathit{R}}_{\mathit{e}}$ and $\mathit{P}\left({\mathsf{\Theta }}^{*}\right)={\mathit{P}}_{\mathit{e}}$ must also be satisfied. So Equation (20) must be established.

For the convenience of calculation, the objective function ($f\left(\mathsf{\Theta }\right),\mathsf{\Theta }\in {\mathbb{R}}^n$) of inverse kinematics can also be defined as

$$\begin{array}{cc}\hfill f\left(\mathsf{\Theta }\right)& =\mathrm{RErr}\left(\mathsf{\Theta }\right)+\lambda \mathrm{PErr}\left(\mathsf{\Theta }\right)\hfill \\ \hfill & =\mathrm{trace}\left({({\mathit{R}}_{\mathit{i}}\left({\theta }_i\right){\mathit{R}}_{\mathit{a}\mathit{i}}-{\mathit{R}}_{\mathit{t}\mathit{i}})}^T({\mathit{R}}_{\mathit{i}}\left({\theta }_i\right){\mathit{R}}_{\mathit{a}\mathit{i}}-{\mathit{R}}_{\mathit{t}\mathit{i}})\right)\hfill \\ \hfill & +\lambda {\left({\mathit{R}}_{\mathit{i}}\left({\theta }_i\right){\mathit{P}}_{\mathit{a}\mathit{i}}+{\mathit{P}}_{\mathit{i}}\left({\theta }_i\right)-{\mathit{P}}_{\mathit{t}\mathit{i}}\right)}^T\left({\mathit{R}}_{\mathit{i}}\left({\theta }_i\right){\mathit{P}}_{\mathit{a}\mathit{i}}+{\mathit{P}}_{\mathit{i}}\left({\theta }_i\right)-{\mathit{P}}_{\mathit{t}\mathit{i}}\right),(i=1,\cdots ,n),\hfill \end{array}$$

(28)

where ${\mathit{R}}_{\mathit{a}\mathit{i}}$, ${\mathit{P}}_{\mathit{a}\mathit{i}}$, ${\mathit{R}}_{\mathit{t}\mathit{i}}$ and ${\mathit{P}}_{\mathit{t}\mathit{i}}$ are expressed as follows:

$$\begin{array}{cc}\hfill {\mathit{R}}_{\mathit{a}\mathit{i}}& =\prod _{j=i+1}^{n+1}{\mathit{R}}_{\mathit{j}}\left({\theta }_j\right),\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\mathit{P}}_{\mathit{a}\mathit{i}}& =\sum _{k=i}^n\left(\prod _{j=i+1}^k{\mathit{R}}_{\mathit{j}}\left({\theta }_j\right)\right){\mathit{P}}_{\mathit{k}+\mathbf{1}}\left({\theta }_{k+1}\right),\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\mathit{R}}_{\mathit{t}\mathit{i}}& ={\left(\prod _{j=1}^{i-1}{\mathit{R}}_{\mathit{j}}\left({\theta }_j\right)\right)}^{-1}{\mathit{R}}_{\mathit{e}},\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill {\mathit{P}}_{\mathit{t}\mathit{i}}& ={\left(\prod _{j=1}^{i-1}{\mathit{R}}_{\mathit{j}}\left({\theta }_j\right)\right)}^{-1}\left({\mathit{P}}_{\mathit{e}}-\sum _{k=0}^{i-2}\left(\prod _{j=1}^k{\mathit{R}}_{\mathit{j}}\left({\theta }_j\right)\right){\mathit{P}}_{\mathit{k}+\mathbf{1}}\left({\theta }_{k+1}\right)\right),\hfill \end{array}$$

(32)

where ${\mathit{P}}_{\mathbf{n}+\mathbf{1}}\left({\theta }_{n+1}\right)$ and ${\mathit{R}}_{\mathbf{n}+\mathbf{1}}\left({\theta }_{n+1}\right)$ are defined as Equations (25) and (26), respectively. Equations (28) and (22) are completely equivalent.□

3.2. Necessary Formulas for Newton’s Method

Newton’s method involves calculating the gradient and Hessian matrix of the object function, and its iterative formula for finding the local minimum value of the objective function (Equation (28)) is Equation (10). The important formulas for Newton’s method are given below.

3.2.1. Determining the Gradient of Objective Function

For both prismatic and revolute joints, the gradient of the objective function of inverse kinematics is calculated using Equation (11), where

$$\frac{\partial f\left(\mathsf{\Theta }\right)}{\partial {\theta }_i}=r_{i1}\cos \left({\theta }_i\right)-r_{i2}\sin \left({\theta }_i\right)+r_{i3}{\theta }_i+r_{i4},$$

(33)

where

$$\begin{array}{cc}\hfill r_{i1}& =2\lambda \left({({\mathit{P}}_{\mathit{t}\mathit{i}}-{\mathit{P}}_{\mathit{a}\mathit{i}})}^T{\mathsf{\Omega }}_{\mathit{i}}^2{\mathit{v}}_{\mathit{i}}-{\mathit{P}}_{\mathit{t}\mathit{i}}^T{\mathsf{\Omega }}_{\mathit{i}}{\mathit{P}}_{\mathit{a}\mathit{i}}\right)-2\mathrm{trace}\left({\mathit{R}}_{\mathit{t}\mathit{i}}^T{\mathsf{\Omega }}_{\mathit{i}}{\mathit{R}}_{\mathit{a}\mathit{i}}\right),\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill r_{i2}& =2\lambda \left({({\mathit{P}}_{\mathit{a}\mathit{i}}+{\mathit{P}}_{\mathit{t}\mathit{i}})}^T{\mathsf{\Omega }}_{\mathit{i}}{\mathit{v}}_{\mathit{i}}-{\mathit{v}}_{\mathit{i}}^T{\mathsf{\Omega }}_{\mathit{i}}^T{\mathsf{\Omega }}_{\mathit{i}}{\mathit{v}}_{\mathit{i}}+{\mathit{P}}_{\mathit{t}\mathit{i}}^T{\mathsf{\Omega }}_{\mathit{i}}^2{\mathit{P}}_{\mathit{a}\mathit{i}}\right)+2\mathrm{trace}\left({\mathit{R}}_{\mathit{t}\mathit{i}}^T{\mathsf{\Omega }}_{\mathit{i}}^2{\mathit{R}}_{\mathit{a}\mathit{i}}\right),\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill r_{i3}& =2\lambda {\mathit{v}}_{\mathit{i}}^T(\mathit{I}+{\mathsf{\Omega }}_{\mathit{i}}^2){\mathit{v}}_{\mathit{i}},\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill r_{i4}& =2\lambda {({\mathit{P}}_{\mathit{a}\mathit{i}}-{\mathit{P}}_{\mathit{t}\mathit{i}})}^T(\mathit{I}+{\mathsf{\Omega }}_{\mathit{i}}^2){\mathit{v}}_{\mathit{i}},\hfill \end{array}$$

(37)

where ${\mathit{R}}_{\mathit{a}\mathit{i}}$, ${\mathit{P}}_{\mathit{a}\mathit{i}}$, ${\mathit{R}}_{\mathit{t}\mathit{i}}$, and ${\mathit{P}}_{\mathit{t}\mathit{i}}$ are defined as Equations (29)–(32), respectively.

If the robot manipulator joint is a prismatic joint (${\mathsf{\Omega }}_{\mathit{i}}=\mathbf{0}$), then the elements in Equation (11) are simplified to

$$\frac{\partial f\left(\mathsf{\Theta }\right)}{\partial {\theta }_i}=2\left({\theta }_i+\lambda {({\mathit{P}}_{\mathit{a}\mathit{i}}-{\mathit{P}}_{\mathit{t}\mathit{i}})}^T{\mathit{v}}_{\mathit{i}}\right).$$

(38)

Conversely, if the robot manipulator joint is a revolute joint ($(\mathit{I}+{{\mathsf{\Omega }}_{\mathit{i}}}^2){\mathit{v}}_{\mathit{i}}=\mathbf{0}$), then the elements in Equation (11) are simplified to

$$\frac{\partial f\left(\mathsf{\Theta }\right)}{\partial {\theta }_i}=r_{i1}\cos \left({\theta }_i\right)-r_{i2}\sin \left({\theta }_i\right),$$

(39)

where $r_{i1}$ and $r_{i2}$ are defined as Equations (34) and (35), respectively.

3.2.2. Determining the Hessian Matrix of Objective Function

The Hessian matrix of the objective function of inverse kinematics defined by Equation (12) is a symmetric matrix, so

$$\frac{{\partial }^{2}f}{\partial {\theta }_i\partial {\theta }_j}=\frac{{\partial }^{2}f}{\partial {\theta }_j\partial {\theta }_i}.$$

(40)

On the one hand, if the robot manipulator’s i-th joint is a prismatic joint, when $i=j$ is satisfied, then, according to Equation (38), the elements in the Hessian matrix can be defined as

$$\frac{{\partial }^{2}f}{\partial {\theta }_i^2}=2\lambda .$$

(41)

When $i<j$ is satisfied, the elements in the Hessian matrix can be calculated as

$$\frac{{\partial }^{2}f}{\partial {\theta }_i\partial {\theta }_j}=2\lambda {{\mathit{v}}_{\mathit{i}}}^T{\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{1}}\left({\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{2}}{\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{3}}+{\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{4}}\right),$$

(42)

where

$$\begin{array}{cc}\hfill {\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{1}}& =\prod _{k=i+1}^{j-1}{\mathit{R}}_{\mathit{k}},\hfill \end{array}$$

(43)

$$\begin{array}{cc}\hfill {\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{2}}& ={\mathsf{\Omega }}_{\mathit{j}}\cos \left({\theta }_j\right)+{\mathsf{\Omega }}_{\mathit{j}}^2\sin \left({\theta }_j\right),\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill {\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{3}}& =\sum _{e=j}^n\left(\prod _{k=j+1}^e{\mathit{R}}_{\mathit{k}}\right){\mathit{P}}_{\mathit{e}\mathbf{+}\mathbf{1}},\hfill \end{array}$$

(45)

$$\begin{array}{cc}\hfill {\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{4}}& ={\mathsf{\Omega }}_{\mathit{j}}{\mathit{v}}_{\mathit{j}}\sin \left({\theta }_j\right)-{\mathsf{\Omega }}_{\mathit{j}}^2{\mathit{v}}_{\mathit{j}}\cos \left({\theta }_j\right)+(\mathit{I}+{{\mathsf{\Omega }}_{\mathit{j}}}^2){\mathit{v}}_{\mathit{j}}.\hfill \end{array}$$

(46)

When $i>j$ is satisfied, the elements in the Hessian matrix can be calculated according to Equation (40).

On the other hand, if the robot manipulator’s i-th joint is a revolute joint, when $i=j$ is satisfied, the elements in the Hessian matrix can be defined as

$$\frac{{\partial }^{2}f}{\partial {\theta }_i^2}=-r_{i1}\sin \left({\theta }_i\right)-r_{i2}\cos \left({\theta }_i\right),$$

(47)

where $r_{i1}$ and $r_{i2}$ are defined as Equations (34) and (35), respectively. When $i<j$ is satisfied, the elements in the Hessian matrix can be expressed as

$$\begin{array}{cc}\hfill \frac{{\partial }^{2}f}{\partial {\theta }_i\partial {\theta }_j}& =-2\lambda \left({\mathit{v}}_{\mathit{i}}^T{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{1}}+{\mathit{P}}_{\mathit{t}\mathit{i}}^T{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{4}}\right){\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{1}}\left({\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{2}}{\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{3}}+{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{3}}\right)\hfill \\ \hfill & -2\mathrm{trace}\left({\mathit{R}}_{\mathit{t}\mathit{i}}^T{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{4}}{\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{1}}{\mathsf{\Omega }}_{\mathit{j}}{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{5}}\right)\cos \left({\theta }_j\right)-2\mathrm{trace}\left({\mathit{R}}_{\mathit{t}\mathit{i}}^T{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{4}}{\mathit{h}}_{\mathit{i}\mathit{j}\mathbf{1}}{{\mathsf{\Omega }}_{\mathit{j}}}^2{\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{5}}\right)\sin \left({\theta }_j\right),\hfill \end{array}$$

(48)

where

$$\begin{array}{cc}\hfill {\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{1}}& ={\mathsf{\Omega }}_{\mathit{i}}^2\cos \left({\theta }_i\right)-{\mathsf{\Omega }}_{\mathit{i}}\sin \left({\theta }_i\right),\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill {\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{2}}& ={\mathsf{\Omega }}_{\mathit{j}}\cos \left({\theta }_j\right)+{\mathsf{\Omega }}_{\mathit{j}}^2\sin \left({\theta }_j\right),\hfill \end{array}$$

(50)

$$\begin{array}{cc}\hfill {\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{3}}& ={\mathsf{\Omega }}_{\mathit{j}}{\mathit{v}}_{\mathit{j}}\sin \left({\theta }_j\right)-{\mathsf{\Omega }}_{\mathit{j}}^2{\mathit{v}}_{\mathit{j}}\cos \left({\theta }_j\right)+\left(\mathit{I}+{{\mathsf{\Omega }}_{\mathit{j}}}^2\right){\mathit{v}}_{\mathit{j}},\hfill \end{array}$$

(51)

$$\begin{array}{cc}\hfill {\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{4}}& ={\mathsf{\Omega }}_{\mathit{i}}\cos \left({\theta }_i\right)+{\mathsf{\Omega }}_{\mathit{i}}^2\sin \left({\theta }_i\right),\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill {\mathit{d}}_{\mathit{i}\mathit{j}\mathbf{5}}& =\prod _{k=j+1}^{n+1}{\mathit{R}}_{\mathit{k}}.\hfill \end{array}$$

(53)

When $i>j$ is satisfied, the elements in the Hessian matrix can be calculated according to Equation (40).

3.3. The ICCD Method

For the inverse kinematics problem, the CCD method has an analytical solution for the optimization of a single joint variable, but all joints are calculated independently, which makes the convergence slow. In order to improve this problem, this paper proposes the ICCD method in which multiple joint variables including consecutive prismatic joints and consecutive parallel revolute joints (Figure 1) can be adjusted concordantly. Although there are analytical solutions when the consecutive prismatic joints are perpendicular to each other, in order to improve the generality of the proposed method, the Newton’s method and the CCD method are combined to solve these two special cases. Associated formulas and the iterative procedure for the ICCD method are given below.

3.3.1. Necessary Formulas for Solving a Single Joint Variable

For a single joint variable, each iteration makes the objective function (Equation (28)) get a local minimum by adjusting only one joint variable, which has an analytical solution. If the joint is a prismatic joint, then the iteration formula for the ICCD method is calculated as

$${\theta }_i^{(k+1)}=\lambda {\left({\mathit{P}}_{\mathit{t}\mathit{i}}\left({\theta }_i^{\left(k\right)}\right)-{\mathit{P}}_{\mathit{a}\mathit{i}}\left({\theta }_i^{\left(k\right)}\right)\right)}^T{\mathit{v}}_{\mathit{i}},$$

(54)

where ${\mathit{P}}_{\mathit{t}\mathit{i}}$ and ${\mathit{P}}_{\mathit{a}\mathit{i}}$ are defined as Equations (32) and (30), respectively. If the joint is a revolute joint, then the iteration formula is defined as

$${\theta }_i^{(k+1)}=\mathrm{atan}2\left(-r_{i1}\left({\theta }_i^{\left(k\right)}\right),-r_{i2}\left({\theta }_i^{\left(k\right)}\right)\right),$$

(55)

where $r_{i1}$ and $r_{i2}$ are defined as Equations (34) and (35), respectively.

The proof for the iteration formula outlined above is shown in Appendix A.

3.3.2. Necessary Formulas for Solving Consecutive Prismatic Joints

When there are consecutive prismatic joints in a robot manipulator, they are regarded as a whole. Moreover, these joint variables can be adjusted simultaneously in order to minimize the objective function of inverse kinematics. Assuming that the robot manipulator in question has consecutive prismatic joints from the i-th joint to the h-th joint, then the following can be achieved:

$${\mathit{R}}_{\mathit{i}}={\mathit{R}}_{\mathit{i}+\mathbf{1}}=\cdots ={\mathit{R}}_{\mathit{h}}=\mathit{I},(1⩽i<h⩽n).$$

(56)

According to Equations (28), the objective function of inverse kinematics can be simplified to

$$\begin{array}{cc}\hfill f\left({\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}\right)& =\mathrm{trace}\left({\left({\mathit{R}}_{\mathit{c}\mathit{p}}-{\mathit{R}}_{\mathit{t}\mathit{i}}\right)}^T\left({\mathit{R}}_{\mathit{c}\mathit{p}}-{\mathit{R}}_{\mathit{t}\mathit{i}}\right)\right)+\sum _{k=i}^h{\theta }_k^2+2\sum _{k=i}^{h-1}\sum _{j=k+1}^h{{\mathit{v}}_{\mathit{k}}}^T{\mathit{v}}_{\mathit{j}}{\theta }_k{\theta }_j\hfill \\ \hfill & +\lambda \left(2{\left({\mathit{P}}_{\mathit{c}\mathit{p}}-{\mathit{P}}_{\mathit{t}\mathit{i}}\right)}^T\left(\sum _{k=i}^h{\mathit{v}}_{\mathit{k}}{\theta }_k\right)+{\left({\mathit{P}}_{\mathit{c}\mathit{p}}-{\mathit{P}}_{\mathit{t}\mathit{i}}\right)}^T\left({\mathit{P}}_{\mathit{c}\mathit{p}}-{\mathit{P}}_{\mathit{t}\mathit{i}}\right)\right),\hfill \end{array}$$

(57)

where consecutive prismatic joints (${\theta }_i\cdots {\theta }_h$) are joint variables and other joints are constant, and where

$$\begin{array}{cc}\hfill {\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}& ={[{\theta }_i,\cdots ,{\theta }_h]}^T,\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill {\mathit{R}}_{\mathit{c}\mathit{p}}& =\prod _{j=h+1}^{n+1}{\mathit{R}}_{\mathit{j}},\hfill \end{array}$$

(59)

$$\begin{array}{cc}\hfill {\mathit{P}}_{\mathit{c}\mathit{p}}& =\sum _{k=h}^n\prod _{j=h+1}^k{\mathit{R}}_{\mathit{j}}{\mathit{P}}_{\mathit{k}+\mathbf{1}},\hfill \end{array}$$

(60)

${\mathit{R}}_{\mathit{t}\mathit{i}}$ and ${\mathit{P}}_{\mathit{t}\mathit{i}}$ are defined as Equations (31) and (32), respectively.

It’s not hard to conclude that when the joints are perpendicular to each other $({{\mathit{v}}_{\mathit{k}}}^T{\mathit{v}}_{\mathit{j}}=0)$, there is an analytical solution. In order to show the generality of the method, including the case that the joints are not perpendicular to each other $({{\mathit{v}}_{\mathit{k}}}^T{\mathit{v}}_{\mathit{j}}\ne 0)$, this paper uses the combination of Newton’s method and the CCD method to solve this problem. But note that this numerical solution is the same as the analytical solution when the joint satisfies the mutually perpendicular condition $({{\mathit{v}}_{\mathit{k}}}^T{\mathit{v}}_{\mathit{j}}=0)$. The gradient, the Hessian matrix, and the iteration formula of the CCD method with respect to the objective function (Equation (57)) are given below.

The gradient of the objective function (Equation (57)) is defined as

$$▽f\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)={\left[\frac{\partial f}{\partial {\theta }_i},\cdots ,\frac{\partial f}{\partial {\theta }_h}\right]}^T,$$

(61)

where

$$\frac{\partial f}{\partial {\theta }_j}=2\left({\theta }_j+{{\mathit{v}}_{\mathit{j}}}^T\left(\sum _{k=i,k\ne j}^h{\mathit{v}}_{\mathit{k}}{\theta }_k\right)+\lambda {\left({\mathit{P}}_{\mathit{c}\mathit{p}}-{\mathit{P}}_{\mathit{t}\mathit{i}}\right)}^T{\mathit{v}}_{\mathit{j}}\right),(j=i,\cdots ,h).$$

(62)

The Hessian matrix of the objective function (Equation (57)) is defined as

$$\mathit{H}\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)={▽}^{2}f\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)=\left[\begin{array}{cccc}\frac{{\partial }^{2}f}{\partial {\theta }_i^2}& \frac{{\partial }^{2}f}{\partial {\theta }_i\partial {\theta }_{i+1}}& \cdots & \frac{{\partial }^{2}f}{\partial {\theta }_i\partial {\theta }_h}\\ \frac{{\partial }^{2}f}{\partial {\theta }_{i+1}\partial {\theta }_i}& \frac{{\partial }^{2}f}{\partial {\theta }_{i+1}^2}& \cdots & \frac{{\partial }^{2}f}{\partial {\theta }_{i+1}\partial {\theta }_h}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{{\partial }^{2}f}{\partial {\theta }_h\partial {\theta }_i}& \frac{{\partial }^{2}f}{\partial {\theta }_h\partial {\theta }_{i+1}}& \cdots & \frac{{\partial }^{2}f}{\partial {\theta }_h^2}\end{array}\right],$$

(63)

where

$$\begin{array}{cc}\hfill \frac{{\partial }^{2}f}{\partial {\theta }_j^2}& =2,\hfill \end{array}$$

(64)

$$\begin{array}{cc}\hfill \frac{{\partial }^{2}f}{\partial {\theta }_j\partial {\theta }_k}& =2{{\mathit{v}}_{\mathit{j}}}^T{\mathit{v}}_{\mathit{k}},(j\ne k).\hfill \end{array}$$

(65)

It is not difficult to conclude that the Hessian matrix ($\mathit{H}\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)$) is always positive definite matrix, so when

$$▽f\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)=\mathbf{0}$$

(66)

is satisfied, the objective function (Equation (57)) obtains the local minimum value. Accordingly, the iteration formula of the CCD method of the objective function (Equation (57)) is calculated as

$${\theta }_j^{(k+1)}={\left(\lambda \left({\mathit{P}}_{\mathit{t}\mathit{i}}-{\mathit{P}}_{\mathit{c}\mathit{p}}\right)-\left(\sum _{l=i}^{j-1}{\mathit{v}}_{\mathit{l}}{\theta }_l^{(k+1)}+\sum _{l=j+1}^h{\mathit{v}}_{\mathit{l}}{\theta }_l^{\left(k\right)}\right)\right)}^T{\mathit{v}}_{\mathit{j}},(j=i,\cdots ,h).$$

(67)

3.3.3. Necessary Formulas for Solving Consecutive Parallel Revolute Joints

When there are consecutive parallel revolute joints in a robot manipulator, they can be regarded as a whole. Moreover, these joint variables can be adjusted simultaneously in order to minimize the objective function of inverse kinematics. Assuming that the robot manipulator in question has consecutive parallel revolute joints from the i-th joint to the h-th joint,

$${\mathsf{\Omega }}_{\mathit{i}}={\mathsf{\Omega }}_{\mathit{i}+\mathbf{1}}=\cdots ={\mathsf{\Omega }}_{\mathit{h}},(1⩽i<h⩽n),$$

(68)

then the rotation matrix satisfies the following property:

$${\mathit{R}}_{\mathit{i}}{\mathit{R}}_{\mathit{i}+\mathbf{1}}\cdots {\mathit{R}}_{\mathit{h}}=e^{{\mathsf{\Omega }}_{\mathit{i}}{\theta }_i}{e}^{{\mathsf{\Omega }}_{\mathit{i}+\mathbf{1}}{\theta }_{i+1}}\cdots e^{{\mathsf{\Omega }}_{\mathit{h}}{\theta }_h}=e^{{\mathsf{\Omega }}_{\mathit{i}}({\theta }_i+{\theta }_{i+1}+\cdots +{\theta }_h)}.$$

(69)

When there are several consecutive parallel revolute joints $({\theta }_i,\cdots ,{\theta }_h)$ in the robot manipulator configuration, the several joints are regarded as variables, and the others as constants. Accordingly, the gradient of the objective function (Equation (28)) is defined as Equation (61) The Hessian matrix of the objective function(Equation (28)) is also defined as Equation (63).

In order to simplify the calculation, only the main diagonal elements of the Hessian matrix ($\mathit{H}\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)$) are considered here. When

$$\frac{{\partial }^{2}f}{\partial {\theta }_i^2}⩾0,\frac{{\partial }^{2}f}{\partial {\theta }_{i+1}^2}⩾0,\cdots ,\frac{{\partial }^{2}f}{\partial {\theta }_h^2}⩾0$$

(70)

and

$$\frac{\partial f}{\partial {\theta }_i}=\frac{\partial f}{\partial {\theta }_{i+1}}=\cdots =\frac{\partial f}{\partial {\theta }_h}=0$$

(71)

are satisfied at the same time, the objective function (Equation (28)) obtains the local minimum. Thereafter, the iterative formulas of the CCD method for joint angles $\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)$ can be calculated as

$${\theta }_j^{(k+1)}=\mathrm{atan}2\left(g\left({\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}^{\left(k_{j-i+1}\right)}\right),h\left({\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}^{\left(k_{j-i+1}\right)}\right)\right),(j=i,i+1,\cdots ,h),$$

(72)

where $g\left(\right)$ and $h\left(\right)$ are functions of ${\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}^{\left(k_t\right)}$ which means that the t-th calculation in the k-round iteration, and where ${\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}^{\left(k_{j-i+1}\right)}$ are defined as

$${\mathsf{\Theta }}_{\mathit{i}\mathbf{\sim }\mathit{h}}^{\left(k_{j-i+1}\right)}={[{\theta }_i^{(k+1)},\cdots ,{\theta }_{j-1}^{(k+1)},{\theta }_j^{\left(k\right)},\cdots ,{\theta }_h^{\left(k\right)}]}^T.$$

(73)

The configuration of more than three consecutive parallel revolute joints is rare in robot manipulators, Accordingly, necessary formulas with respect to minimizing the objective function by adjusting two or three consecutive parallel revolute joints at the same time are given in Appendix B.

3.3.4. ICCD Method of the Iterative Procedure

The ICCD method proposed in this section is responsible for finding ${\mathsf{\Theta }}^{(k+1)}$ from ${\mathsf{\Theta }}^{\left(k\right)}$ as shown in Algorithm 1. In line 1 groups are determined to facilitate subsequent computation. Specifically, consider a single revolute joint, a single prismatic joint, consecutive parallel revolute joints and consecutive prismatic joints as the groups, e.g., the UR robot with three consecutive parallel revolute joints(${\theta }_2,{\theta }_3,{\theta }_4$) is grouped into $\mathit{G}=\{{\mathit{g}}_{\mathbf{1}},{\mathit{g}}_{\mathbf{2}},{\mathit{g}}_{\mathbf{3}},{\mathit{g}}_{\mathbf{4}}\}=\left\{\left\{{\theta }_1\right\},\left\{{\theta }_2,{\theta }_3,{\theta }_4\right\},\left\{{\theta }_5\right\},\left\{{\theta }_6\right\}\right\}$. In lines $3\sim 44$, taking $g_i$ as a variable and all others as constants, the goal is to find $g_i$ such that the objective function obtains a local minimum value. In lines $3\sim 6$, the single revolute joint and prismatic joint are calculated separately. In lines $8\sim 44$ consecutive parallel revolute joints and consecutive prismatic joints are calculated in order to find the minimum value of the objective function. ${ϵ}_{ca}$ is a threshold value to verify convergence supplied by the user. If the Hessian matrix $\left(\mathit{H}\left({\mathsf{\Theta }}_{\mathit{i}\sim \mathit{h}}\right)\right)$ of either the consecutive parallel revolute joints or the consecutive prismatic joints is positive definite matrix, then Newton’s method should be used (let ${\mathrm{flag}}_{ca2}=1$). Otherwise, Newton’s method and the CCD method are used simultaneously, and the more effective result of the two methods is adopted (let ${\mathrm{flag}}_{ca2}=2$). The variable ${\mathrm{flag}}_{ca2}$ is a flag used to record which method is used in this iteration. If ${\mathrm{flag}}_{ca2}=1$, it means the former. On the contrary, if ${\mathrm{flag}}_{ca2}=2$, it means the latter. In order to ensure that the Newton’s method and the CCD method converge to the local minimum value at the same time, ${\mathrm{flag}}_{ca1}=0$ is used, so that Newton’s method and CCD method are simultaneously calculated in the case where the Hessian matrix is positive definite matrix. To be specific, the ICCD method consists of the step-by-step procedure shown in Algorithm 1.

3.4. The NICCD Method for The Inverse Kinematics Problem

The CCD method has great robustness and simple calculation process [13], but the convergence rate is slow when the iteration point approaches the optimal point. The ICCD method has the same problem. Conversely, Newton’s method (with a quadratic rate of convergence) converges quickly when the iteration point approaches the optimal point. However, Newton’s method often fails to converge when the initial estimate is not sufficiently close, and cannot search the descent direction when the second-order Hessian matrix is not positive definite matrix. In order to inherit the advantages of the two algorithms and discard their disadvantages, the NICCD method is proposed to solve the inverse kinematics problem. The determination of the scale factor and the iterative procedure for the NICCD method are given below.

Algorithm 1 The improved cyclic coordinate descent (ICCD) method.

1: Initialization: Set ${\mathrm{Err}}_g=+\infty$, ${\mathrm{flag}}_{ca2}=0$, and determine groups ${\mathit{G}}^{\left(k\right)}=\left\{{\mathit{g}}_{\mathbf{1}}^{\left(k\right)},{\mathit{g}}_{\mathbf{2}}^{\left(k\right)},\cdots ,{\mathit{g}}_{\mathit{l}}^{\left(k\right)}\right\}$ 2: for each $i\in [1,l]$ do 3:  if ${\mathit{g}}_{\mathit{i}}^{\left(k\right)}$ is a group of single revolute joint then 4:   Determine ${\mathit{g}}_{\mathit{i}}^{(k+1)}$ (see Equation (55)) 5:  else if${\mathit{g}}_{\mathit{i}}^{\left(k\right)}$ is a group of single prismatic joint then 6:   Determine ${\mathit{g}}_{\mathit{i}}^{(k+1)}$ (see Equation (54)) 7:  else 8:   Set ${\mathrm{flag}}_{ca1}=1$ 9:   while${\mathrm{Err}}_g>{ϵ}_{ca}$ or ${\mathrm{flag}}_{ca2}\ne 2$do 10:    if$$ is a group of consecutive parallel revolute joints then 11:     Calculate $▽f\left({\mathit{g}}_{\mathit{i}}^{\left(k\right)}\right)$ and $\mathit{H}\left({\mathit{g}}_{\mathit{i}}^{\left(k\right)}\right)$ (see Equations (61), (63), and Appendix B) 12:     else 13:     Calculate $▽f\left({\mathit{g}}_{\mathit{i}}^{\left(k\right)}\right)$ and $\mathit{H}\left({\mathit{g}}_{\mathit{i}}^{\left(k\right)}\right)$ (see Equations (61)∼(65))) 14:     end if 15:    if $\mathit{H}\left({\mathit{g}}_{\mathit{i}}^{\left(k\right)}\right)$ is a positive definite matrix and ${\mathrm{flag}}_{ca1}=1$ then 16:     Set ${\mathrm{flag}}_{ca2}=1$ 17:     Determine ${\mathit{g}}_{\mathit{i}}^{(k+1)}$ by Newton’s method (see Equation (10)) 18:     else 19:     Set ${\mathrm{flag}}_{ca2}=2$ 20:     Determine ${\mathit{g}}_{\mathit{N}\mathit{i}}^{(k+1)}$ by Newton’s method (see Equation (10)) 21:     if ${\mathit{g}}_{\mathit{i}}^{\left(k\right)}$ is a group of consecutive parallel revolute joints then 22:      Calculate ${\mathit{g}}_{\mathit{C}\mathit{i}}^{(k+1)}$ by CCD method (see Equation (72) and Appendix B) 23:      else 24:      Calculate ${\mathit{g}}_{\mathit{C}\mathit{i}}^{(k+1)}$ by CCD method (see Equation (67)) 25:      end if 26:     Set ${\mathit{G}}_{\mathit{C}}^{(k+1)}=\left\{{\mathit{g}}_{\mathbf{1}}^{(k+1)},\cdots ,{\mathit{g}}_{\mathit{C}\mathit{i}}^{(k+1)},{\mathit{g}}_{\mathit{i}+\mathbf{1}}^{(k+1)},\cdots ,{\mathit{g}}_{\mathit{l}}^{(k+1)}\right\}$ and ${\mathit{G}}_{\mathit{N}}^{(k+1)}=\left\{{\mathit{g}}_{\mathbf{1}}^{(k+1)},\cdots ,{\mathit{g}}_{\mathit{N}\mathit{i}}^{(k+1)},{\mathit{g}}_{\mathit{i}+\mathbf{1}}^{(k+1)},\cdots ,{\mathit{g}}_{\mathit{l}}^{(k+1)}\right\}$ 27:     Calculate $f_{caC}=f\left({\mathit{G}}_{\mathit{C}}^{(k+1)}\right)$ and $f_{caN}=f\left({\mathit{G}}_{\mathit{N}}^{(k+1)}\right)$ (see Equation (28)) 28:      if$f_{caC}<f_{caN}$then 29:      Set ${\mathit{g}}_{\mathit{i}}^{(k+1)}={\mathit{g}}_{\mathit{C}\mathit{i}}^{(k+1)}$ 30:      else 31:      Set ${\mathit{g}}_{\mathit{i}}^{(k+1)}={\mathit{g}}_{\mathit{N}\mathit{i}}^{(k+1)}$ 32:      end if 33:     end if 34:    Set ${\mathrm{Err}}_g=∥{\mathit{g}}_{\mathit{i}}^{(k+1)}-{\mathit{g}}_{\mathit{i}}^{\left(k\right)}∥$ 35:     if${\mathrm{Err}}_g⩽{ϵ}_{ca}$then 36:      if${\mathrm{flag}}_{ca2}\ne 2$then 37:      Set ${\mathrm{flag}}_{ca1}=0$ 38:      end if 39:     else 40:     Set ${\mathrm{flag}}_{ca1}=1$ 41:     end if 42:    Set ${\mathit{g}}_{\mathit{i}}^{\left(k\right)}={\mathit{g}}_{\mathit{i}}^{(k+1)}$ 43:    end while 44:   end if 45: end for 46: Set ${\mathsf{\Theta }}^{(k+1)}={\mathit{G}}^{(k+1)}$

3.4.1. The Scale Factor

There is a scale factor ($\lambda$) in the objective function (Equation (22)), which is used to scale the position error so that the difference between the position error and the angle error is not very large. This scale factor has an impact on the convergence speed of the proposed approach and the ability to avoid the local minimum.

From the demonstration in Appendix C, it can be concluded that

$$\underset{\mathsf{\Theta }}{max}\mathrm{RErr}\left(\mathsf{\Theta }\right)=\underset{\mathsf{\Theta }}{max}\mathrm{trace}\left({(\mathit{R}\left(\mathsf{\Theta }\right)-{\mathit{R}}_{\mathit{e}})}^T(\mathit{R}\left(\mathsf{\Theta }\right)-{\mathit{R}}_{\mathit{e}})\right)=8.$$

(74)

Therefore, the scale factor ($\lambda$) can be defined as

$$\lambda =\frac{{max}_{\mathsf{\Theta }}\mathrm{RErr}\left(\mathsf{\Theta }\right)}{{max}_{\mathsf{\Theta }}\mathrm{PErr}\left(\mathsf{\Theta }\right)}=\frac{8}{{max}_{\mathsf{\Theta }}\mathrm{PErr}\left(\mathsf{\Theta }\right)},$$

(75)

where the $\mathrm{PErr}\left(\mathsf{\Theta }\right)$ is defined as Equation (22) and ${max}_{\mathsf{\Theta }}\mathrm{PErr}\left(\mathsf{\Theta }\right)$ can be calculated by the robot manipulator configuration in advance.

3.4.2. The NICCD Approach of the Iterative Procedure

The method proposed in this section seeks a solution ${\mathsf{\Theta }}^{*}$ to the problem defined by Equation (27) as shown in Algorithm 2. Some important parameters in Algorithm 2 are introduced as follows. Parameters t and ${\max }_k$ represent the coefficient of the scale factor ($\lambda$) and the maximum number of iterations respectively. Parameters ${ϵ}_1$ and ${ϵ}_2$ represent the convergence threshold of objective function (f) and independent variable ($\mathsf{\Theta }$) respectively. The roles of ${\mathrm{flag}}_1$ and ${\mathrm{flag}}_2$ can be analogized to ${\mathrm{flag}}_{ca1}$ and ${\mathrm{flag}}_{ca2}$ in Algorithm 1, respectively. In lines $3\sim 17$, if the Hessian matrix is positive definite matrix and ${\mathrm{flag}}_1=1$, then ${\mathsf{\Theta }}^{(k+1)}$ can be calculated using Newton’s method (let ${\mathrm{flag}}_2=1$); otherwise, Newton’s method and the ICCD method are executed simultaneously, with the more efficient method being used to calculate ${\mathsf{\Theta }}^{(k+1)}$ (let ${\mathrm{flag}}_2=2$). Lines 18 and 19 involve the global and local convergence judgments of the method, respectively. In lines $23\sim 30$, the method of jumping out of the local optimal solution is used. The NICCD method consists of the step-by-step procedure shown in Algorithm 2.

4. Simulation and Discussion

In order to illustrate the effectiveness, accuracy, robustness, and generality of the inverse kinematics approach proposed in this article, five simulations of six robots—a three-link planar arm (3R), a selective compliance assembly robot arm (SCARA), a Cartesian manipulator (3P), Universal Robot’s UR5, a Stanford Arm, and Barrett Technology’s WAM7R robot manipulator—are conducted using the NICCD method, the results of which are compared with the NR method using the Jacobian matrix. The NR method is described in section 6.2.2 of [25]. The PoE parameters of the six robots with the current configurations relative to the base frame are shown in

Table 1. The product-of-exponentials (PoE) parameters of three-link planar arm (3R), a selective compliance assembly robot arm (SCARA), a Cartesian manipulator (3P).

i	3R		SCARA		3P
	${{\mathbf{\omega }}_{\mathit{i}}}^{\mathit{T}}$	${{\mathit{v}}_{\mathit{i}}}^{\mathit{T}}$	${{\mathbf{\omega }}_{\mathit{i}}}^{\mathit{T}}$	${{\mathit{v}}_{\mathit{i}}}^{\mathit{T}}$	${{\mathbf{\omega }}_{\mathit{i}}}^{\mathit{T}}$	${{\mathit{v}}_{\mathit{i}}}^{\mathit{T}}$
1	$[0,0,1]$	$[0,0,0]$	$[0,0,1]$	$[0,0,0]$	$[0,0,0]$	$[0,0,1]$
2	$[0,0,1]$	$[0,-400,0]$	$[0,0,1]$	$[0,-450,0]$	$[0,0,0]$	$[0,1,0]$
3	$[0,0,1]$	$[0,-800,0]$	$[0,0,0]$	$[0,0,-1]$	$[0,0,0]$	$[1,0,0]$
4			$[0,0,1]$	$[0,-850,0]$
$\mathit{M}$	$\left[\begin{array}{cccc}1& 0& 0& 1200\\ 0& 1& 0& 0\\ 0& 0& 1& 1\\ 0& 0& 0& 1\end{array}\right]$		$\left[\begin{array}{cccc}1& 0& 0& 850\\ 0& 1& 0& 0\\ 0& 0& 1& -420\\ 0& 0& 0& 1\end{array}\right]$		$\left[\begin{array}{cccc}1& 0& 0& 400\\ 0& 1& 0& 400\\ 0& 0& 1& 400\\ 0& 0& 0& 1\end{array}\right]$

and

Table 2. The PoE parameters of UR5, Stanford Arm, and WAM7R.

i	UR5		Stanford Arm		WAM7R
	${{\mathbf{\omega }}_{\mathit{i}}}^{\mathit{T}}$	${{\mathit{v}}_{\mathit{i}}}^{\mathit{T}}$	${{\mathbf{\omega }}_{\mathit{i}}}^{\mathit{T}}$	${{\mathit{v}}_{\mathit{i}}}^{\mathit{T}}$	${{\mathbf{\omega }}_{\mathit{i}}}^{\mathit{T}}$	${{\mathit{v}}_{\mathit{i}}}^{\mathit{T}}$
1	$[0,0,1]$	$[0,0,0]$	$[0,0,1]$	$[0,0,0]$	$[0,0,1]$	$[0,0,0]$
2	$[0,1,0]$	$[-89,0,0]$	$[1,0,0]$	$[0,0,0]$	$[0,1,0]$	$[0,0,0]$
3	$[0,1,0]$	$[-89,0,425]$	$[0,0,0]$	$[0,0,1]$	$[0,0,1]$	$[0,0,0]$
4	$[0,1,0]$	$[-89,0,817]$	$[0,0,1]$	$[0,-200,0]$	$[0,1,0]$	$[-550,0,45]$
5	$[0,0,-1]$	$[-109,817,0]$	$[0,1,0]$	$[0,0,200]$	$[0,0,1]$	$[0,0,0]$
6	$[0,1,0]$	$[6,0,817]$	$[1,0,0]$	$[0,400,0]$	$[0,1,0]$	$[-850,0,0]$
7					$[0,0,1]$	$[0,0,0]$
$\mathit{M}$	$\left[\begin{array}{cccc}-1& 0& 0& 817\\ 0& 0& 1& 191\\ 0& 1& 0& -6\\ 0& 0& 0& 1\end{array}\right]$		$\left[\begin{array}{cccc}1& 0& 0& 692\\ 0& 1& 0& 0\\ 0& 0& 1& 400\\ 0& 0& 0& 1\end{array}\right]$		$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 910\\ 0& 0& 0& 1\end{array}\right]$

. The last simulation verifies the improvement of ICCD compared with CCD method and the evolution process of NICCD method. All simulations are performed on a desktop computer (Core i5 3.40 GHz, 16 GB RAM, MATLAB 2015b software program).

Algorithm 2 The Newton-improved cyclic coordinate descent (NICCD) method.

1: Initialization: Set $t=1$, ${\mathrm{flag}}_1=1$ and determine the scale factor $\lambda$ (see Equation (75)) 2: for each $k\in [1,{\max }_k]$ do 3:  Calculate $▽f\left({\mathsf{\Theta }}^{\left(k\right)}\right)$ and $\mathit{H}\left({\mathsf{\Theta }}^{\left(k\right)}\right)$ (see Equations (11), (12), and (33)∼(39)) 4:  if $\mathit{H}\left({\mathsf{\Theta }}^{\left(k\right)}\right)$ is positive definite matrix and ${\mathrm{flag}}_1=1$ then 5:   Set ${\mathrm{flag}}_2=1$ 6:   Determine ${\mathsf{\Theta }}^{(k+1)}$ by Newton’s method (see Equation (10)) 7:  else 8:   Set ${\mathrm{flag}}_2=2$ 9:   Determine ${\mathsf{\Theta }}_N^{(k+1)}$ by Newton’s method (see Equation (10)) 10:   Determine ${\mathsf{\Theta }}_C^{(k+1)}$ by ICCD method (see Algorithm 1 ) 11:   Calculate $f_N=f\left({\mathsf{\Theta }}_N^{(k+1)}\right)$ and $f_C=f\left({\mathsf{\Theta }}_C^{(k+1)}\right)$ (see Equation (28)) 12:   if$f_C<f_N$then 13:    Set ${\mathsf{\Theta }}^{(k+1)}={\mathsf{\Theta }}_C^{(k+1)}$ 14:   else 15:    ${\mathsf{\Theta }}^{(k+1)}={\mathsf{\Theta }}_N^{(k+1)}$ 16:   end if 17:  end if 18:  if$∥f\left({\mathsf{\Theta }}^{(k+1)}\right)∥>{ϵ}_1$then 19:   if$∥{\mathsf{\Theta }}^{(k+1)}-{\mathsf{\Theta }}^{\left(k\right)}∥⩽{ϵ}_2$then 20:    if${\mathrm{flag}}_2\ne 2$then 21:     Set $t=1,{\mathrm{flag}}_1=0$ 22:    else 23:     if$\mathrm{RErr}\left({\mathsf{\Theta }}^{(k+1)}\right)<\mathrm{PErr}\left({\mathsf{\Theta }}^{(k+1)}\right)$then 24:      Set $t=\frac{t}{2}$ 25:     else 26:      Set $t=2t$ 27:     end if 28:     Set $\lambda =t\lambda$ 29:     Determine ${\mathsf{\Theta }}^{(k+1)}$ by ICCD method (see Algorithm 1 ) 30:     Set $\lambda =\frac{\lambda }{t}$ 31:    end if 32:   else 33:    Set ${\mathrm{flag}}_1=1$ 34:   end if 35:  else 36:   Set ${\mathsf{\Theta }}^{*}={\mathsf{\Theta }}^{(k+1)}$ 37:   break 38:  end if 39: end for

4.1. Simulation I

This simulation aims to verify the effectiveness of the proposed approach using SCARA and UR5. The specific values of the target joint variables (${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$) and the initial joint variables (${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{1}},{\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{2}}$) with respect to SCARA and UR5 are shown in

Table 3. The specific values of the target and initial joint variables of SCARA and UR5.

	SCARA				UR5
	${\mathbf{\theta }}_{\mathbf{1}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{2}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathit{d}}_{\mathbf{3}}\left(\mathbf{mm}\right)$	${\mathbf{\theta }}_{\mathbf{4}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{1}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{2}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{3}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{4}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{5}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{6}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$
${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$	0.2169	2.1269	100.0000	0.2391	3.0076	1.3364	0.0030	−0.1817	−2.7670	1.1434
${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{1}}$	−2.1142	2.6458	−11.9929	0.4863	−1.9350	−2.2690	1.2332	−2.5521	0.1596	0.1907
${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{2}}$	−1.5218	−0.6484	−20.0000	1.1567	−1.0585	0.4914	2.2274	0.8946	−0.5020	0.2885

. According to Equation (1), the goal configurations of the end-effector frames of the two robot manipulators relative to ${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$ are described as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathit{T}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}& =\left[\begin{array}{cccc}-0.8479& -0.5301& 0& 160.1408\\ 0.5301& -0.8479& 0& 383.1681\\ 0& 0& 1.0000& -520.0000\\ 0& 0& 0& 1.0000\end{array}\right],\left(\mathrm{SCARA}\right),\hfill \\ \hfill {\mathit{T}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}& =\left[\begin{array}{cccc}-0.9592& -0.0838& 0.2699& -93.1191\\ 0.2823& -0.3247& 0.9027& -20.4293\\ 0.0120& 0.9421& 0.3351& -716.5883\\ 0& 0& 0& 1.0000\end{array}\right],\left(\mathrm{UR}5\right).\hfill \end{array}\end{array}$$

(76)

The inverse kinematics are calculated using the NICCD and the NR method, with ${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{1}}$ and ${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{2}}$ as initial values. In order to illustrate the effectiveness of the proposed method, the angle error, and position error are defined as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathrm{Err}}_R& =\sqrt{\mathrm{trace}\left({({\mathit{R}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}-{\mathit{R}}_{\mathbf{o}\mathbf{u}\mathbf{t}})}^T({\mathit{R}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}-{\mathit{R}}_{\mathbf{o}\mathbf{u}\mathbf{t}})\right)},\hfill \\ \hfill {\mathrm{Err}}_P& =\sqrt{{({\mathit{P}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}-{\mathit{P}}_{\mathbf{o}\mathbf{u}\mathbf{t}})}^T({\mathit{P}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}-{\mathit{P}}_{\mathbf{o}\mathbf{u}\mathbf{t}})},\hfill \end{array}\end{array}$$

(77)

where ${\mathit{R}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$ and ${\mathit{P}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$ are the elements in ${\mathit{T}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$, and ${\mathit{R}}_{\mathbf{o}\mathbf{u}\mathbf{t}}$ and ${\mathit{P}}_{\mathbf{o}\mathbf{u}\mathbf{t}}$ are the elements in ${\mathit{T}}_{\mathbf{o}\mathbf{u}\mathbf{t}}$, which are obtained by the forward kinematics calculated by either the NICCD or the NR method with respect to ${\mathsf{\Theta }}_{\mathbf{o}\mathbf{u}\mathbf{t}}$. The position and angle errors of the initial selected joint angles of the SCARA and the UR5 versus the convergence times of the NICCD and the NR methods are illustrated in Figure 2.

From Figure 2, when the initial value is ${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{1}}$, the NICCD method converges more rapidly than the NR method with respect to SCARA. However, it is slower with respect to UR5. When the initial value is ${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{2}}$, the NICCD method still converges, but the NR method diverges with respect to the inverse kinematics of SCARA and UR5. In addition, the NICCD method has less fluctuation in both the angle error and the position error during the iteration process. Therefore, it can be concluded that the proposed approach is more stable and effective than the NR method.

4.2. Simulation II

This second simulation involves testing the accuracy of the proposed approach using the Stanford Arm and Barrett Technology’s WAM7R robot manipulator. The specific values of the target joint variables (${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$) and the initial joint variables (${\mathsf{\Theta }}_{\mathrm{in}}$) of the Stanford Arm and the WAM7R manipulator are shown in

Table 4. The specific values of the target and initial joint variables of Stanford Arm and WAM7R manipulator.

	Stanford Arm						WAM7R
	${\mathbf{\theta }}_{\mathbf{1}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{2}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathit{d}}_{\mathbf{3}}\left(\mathbf{mm}\right)$	${\mathbf{\theta }}_{\mathbf{4}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{5}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{6}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{1}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{2}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{3}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{4}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{5}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{6}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$	${\mathbf{\theta }}_{\mathbf{7}}\left(\mathbf{r}\mathit{a}\mathbf{d}\right)$
${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$	−0.1290	0.8754	100.0000	0.9256	0.2757	1.3889	0.4088	2.0346	−2.3493	−1.2559	−3.1283	2.8344	1.6732
${\mathsf{\Theta }}_{\mathrm{in}}$	−1.0520	2.2184	−0.3619	2.5406	−2.9331	0.2037	−0.5576	2.2993	2.6431	1.8048	−0.9740	−2.7061	1.6552

. According to Equation (1), the goal configurations of the end-effector frames of the two robot manipulators relative to ${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$ are described as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathit{T}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}& =\left[\begin{array}{cccc}0.6641& -0.0505& 0.7459& 549.2836\\ 0.6213& -0.5177& -0.5882& -42.3986\\ 0.4159& 0.8541& -0.3124& 582.0788\\ 0& 0& 0& 1.0000\end{array}\right],(\mathrm{Stanford}\mathrm{Arm}),\hfill \\ \hfill {\mathit{T}}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}& =\left[\begin{array}{cccc}-0.0765& -0.9970& -0.0139& 346.4601\\ 0.7721& -0.0504& -0.6335& 309.6627\\ 0.6309& -0.0592& 0.7736& -381.6887\\ 0& 0& 0& 1.0000\end{array}\right],\left(\mathrm{WAM}7\mathrm{R}\right).\hfill \end{array}\end{array}$$

(78)

In the case of closing the convergence judgment, 150 iterations of the NICCD and NR methods were conducted. The position and angle errors of the initial selected joint angles of the Stanford Arm and the WAM7R manipulator versus the number of iterations of the NICCD and NR methods are given in Figure 3, where the error is represented on the logarithmic axis. Indeed, from Figure 3, it is clear that the approach proposed in this paper is more accurate than the NR method with respect to the position error and angle error.

4.3. Simulation III

This simulation is conducted in order to validate the robustness of the proposed approach by using the 3P, 3R, SCARA, UR5, and Barrett Technology’s WAM7R robot manipulator. The difference between this simulation and the prior two is that 500 sets of target joint variables and 500 sets of initial joint variables are generated randomly. The end-effector configurations of the four robots are calculated by forward kinematics relative to the target joint variables. Thereafter, the inverse kinematics of the four robots are solved using the NICCD and NR method. The correct rate of solving the inverse kinematics of these four robots is shown in Figure 4. Indeed, from Figure 4, for the WAM7R robot manipulator, it is clear that the approach proposed in this paper has a correct rate that is smaller than that of the NR method. Moreover, from Figure 4, for the remaining four robots, it is clear that the approach proposed in this paper has a correct rate that is larger than that of the NR method.

4.4. Simulation IV

This simulation aims to illustrate the fact that the proposed approach has offline programming abilities by using the UR5. The goal here is to create a trajectory that will allow the UR5 to draw the letter ‘F’ (the red trajectory in Figure 5) in 20 s. The orientation of the end-effector is constant, with the $\mathit{y}$-axis parallel to that of the base frame and the $\mathit{x}$- and $\mathit{z}$-axes opposite to that of the base frame. Specifically, the desired trajectory of UR5 is shown in the left figure of Figure 5, and the actual trajectory is shown in the right figure. The latter is obtained after discrete and smooth processing of the former. The inverse kinematics solution for each sample point is calculated. and the position, velocity, and acceleration of the joints are given in Figure 6.

The proposed approach takes an average of $0.0057$ s with respect to the calculation process, which demonstrates how efficient it is. By comparing Figure 6 of Simulation IV and Figure 2 of Simulation I, the average calculation time of the former is shorter than the latter. This is because Simulation IV follows a continuous trajectory, which means that the solution of the last step is taken as the initial value of the next step. In other words, the difference between the initial joint variables and the target joint variables is smaller in Simulation IV than in Simulation I. Accordingly, it can be concluded that the proposed approach is appropriate for offline programming and that it is advantageous with respect to the inverse kinematics of continuous trajectories.

4.5. Simulation V

This simulation aims to verify the improvement of ICCD method (consider consecutive prismatic joints or consecutive parallel revolute joints as the group) compared with the traditional CCD method (each joint is adjusted individually) and the evolution process of NICCD method. UR5 manipulator with consecutive parallel revolute joints meets the configuration requirements. The specific values of its target and initial angles of UR5 are the same as the data of ${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$ and ${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{2}}$ in Simulation I (see Table 3). According to Equation (1), the goal configuration of the end-effector frame of UR5 relative to ${\mathsf{\Theta }}_{\mathbf{g}\mathbf{o}\mathit{a}\mathbf{l}}$ is described as Equation (76).

The inverse kinematics are calculated using the CCD, ICCD, and NICCD methods, with ${\mathsf{\Theta }}_{\mathbf{i}\mathbf{n}\mathbf{2}}$ as initial value. The position and angle errors of the initial selected joint angles of the UR5 versus the convergence times of the CCD, ICCD, and NICCD methods are illustrated in Figure 7 and versus the number of iterations are illustrated in Figure 8, where the error is represented on the logarithmic axis.

From Figure 7 and Figure 8, ICCD method is better than CCD method in convergence time and iteration times. Especially at the initial iteration, the ICCD method drops much more rapidly than the CCD method in terms of positional error. Specifically, the ICCD method has a position error of $5.8159$ after 2 iterations ($0.0032$ s), while the CCD method needs to iteration 29 times ($0.0364$ s) to achieve the same error. A similar conclusion can be drawn for angle error.

It is not difficult to conclude from Figure 7 and Figure 8 that the convergence rate of the NICCD method is significantly more rapidly than that of the other two methods. To analyze this reason, Figure 9 shows the evolution process of the NICCD method. From Figure 9, the NICCD method converges after four iterations among which the ICCD method is used for the first two iterations and Newton’s method is used for the last two iterations. NICCD method proposed in this paper uses the ICCD method to rapidly find a feasible point that is near to the true solution and then uses Newton’s method to obtain a solution that achieves the desired precision. This avoids the common disadvantage of CCD and ICCD methods, that is, the convergence rate is slow when the iteration point approaches the optimal point.

5. Conclusions and Future Work

This article presented an efficient approach for inverse kinematics that is suitable for any configurations of robots with either revolute or prismatic joints with any arbitrary number of degrees of freedom. Compared with the traditional method, this paper transformed the inverse kinematics problem into the objective function optimization problem, which is based on the least-squares form of the angle error and the position error expressed by the PoE formula. Moreover, the necessary gradient and Hessian matrix required to solve the formula are given. Finally, the NICCD method was proposed to solve the proposed objective function optimization problem.

Five simulations were given in order to illustrate the effectiveness, accuracy, robustness, and generality of the proposed method. Simulation I verified the effectiveness of the proposed method, the results of which suggest that it has less fluctuation and is more effective than the NR method. Simulation II tests the accuracy of the proposed method. The results of which suggest that the approach proposed in this paper can accurately calculate the error of position and the angle to reach ${10}^{-13}$ and ${10}^{-15}$ respectively, but the NR method provides the error of the angle within ${10}^{-11}$. This also proves the validity of the objective function constructed in Section 3.1. Simulation III demonstrated the fact that the proposed approach is not sensitive to the initial joint variables. Indeed, the correct rate for inverse kinematics calculations with respect to 3P, 3R, SCARA, UR5, and WAM7R are approximately $100\%$, $100\%$, $100\%$, $94.5\%$, and $93.1\%$. Simulation IV illustrated the fact that the proposed approach has offline programming abilities and that it is advantageous with respect to the inverse kinematics of continuous trajectories since each iteration only takes $0.0057$ s to calculate. Simulation V verified that the ICCD method drops much more rapidly than the CCD method in positional and angle errors in the initial iteration, and the NICCD method combines the respective advantages of the ICCD and Newton’s methods while trying to avoid their shortcomings.

The future work will mainly focus on the following aspects. First of all, we will introduce the heuristic adaptive truncation criterion [27] into the NICCD method to improve the convergence rate. Next, we will improve the NICCD method to become a more effective method with respect to avoiding a singularity location and local minimum, which, in turn, will enhance the accuracy and robustness of the method. Last but not least, the NICCD method will be applied to practical applications such as trajectory planning and motion control of the robot.