A speedup method for solving the inverse kinematics problem of robotic manipulators

The inverse kinematics problem involves the study that the inverse kinematics solver needs to calculate the values of the joint variables given the desired pose of the end-effector of a robot. However, to apply to seven-degree-of-freedom robots with arbitrary configuration, analytical methods need to fix one joint and set an increment when the current value fails to solve the inverse kinematics problem. Although numerical methods based on inverse differential kinematics are efficient in solving the inverse kinematics problem of seven-degree-of-freedom robots with arbitrary geometric parameters, they are deficient in numerical stability and time-consuming for convergence to one solution governed by the initial guess. In order to reduce the execution time of an inverse kinematics solver, this article introduces a speedup method for analytical and numerical methods, which can improve their performance.


Introduction
In order to place its end-effector exactly at a desired position and orientation in Cartesian space, a serial-chain robot needs to possess no less than six degrees of freedom (DOFs). A 7-DOF robot has one more redundant DOF than it requires for positioning the end-effector in Cartesian space. A 7-DOF robot has more practical significance in applications as it has some advantages. For example, it can provide greater flexibility and reliability when executing tasks and avoid obstacles in complex and unknown work environments. But unfortunately, it is arduous to find the solution of inverse kinematics (IK) for a redundant robot in real time. How to solve the inverse kinematics problem (IKP) of a redundant serial-chain robot has been and remains quite challenging in robotics research and applications.
Generally speaking, there exist analytical methods and numerical methods to solve the IKP of redundant robots. 1 Because the IK solver must be pre-built, an analytical IK solution method cannot be generalized to the cases where a robot configuration changes and a tool is mounted at the end. Numerical methods are widely used to solve the IKP of various configurations. However, due to the limitation of the number of iterations and the setting of convergence conditions, these IK solution methods are sometimes not feasible. Most numerical IK solution methods use the Newton-Raphson (NR) algorithm 2 or similar to iterate until they find a solution. Compared with analytical methods, numerical methods are reasonable in theory, and they take a long time to find one IK solution. Sometimes, the precision of the found solution is low, and even the phenomenon occurs that the IKP cannot be solved correctly. Scholars have developed many methods to accelerate the calculation of Jacobian and matrix inversion and optimize numerical methods to improve solution efficiency. Many methods are developed according to the Denavit-Hartenberg (D-H) parameters of robots, but there are methods based on screw transforms 3 and exponential rotational matrices, 4 which arise from the explicit physical interpretation of the mechanism with Euler wrist and can be easily realized. The IKP becomes more complicated when obstacles in the workspace impose collision avoidance constraints. Convex Iteration for Distance-Geometric Inverse Kinematics algorithm can solve the convex feasibility problem whose low-rank feasible points provide exact IK solutions for complex workspace constraints. 5 For redundant robots, the analytical IK solution needs one joint fixed and then solves other joint rotation angles. Concerning the initial fixed joint angle value, the solution may fail. If it happens, an increment will add to the initial value until the solution succeeds within the time limit. Thus, more precisely to say, the fixed joint is free to change its value. For specific mechanisms, such as manipulators with Euler wrist, using dual quaternions can obtain a very compact formulation for closed-form solutions. 6 As to the numerical IK solution, a set of initial joint angles is required to compute the first Jacobian or as a seed state to start an iterative search. Prior to finishing the solving process, the number of iterations cannot be determined. To reach a particular precision, this kind of approach is computationally expensive in many cases, which can be numerically unstable. Although state-of-the-art algorithms have reduced inverse solution computation time to milliseconds or even microseconds, the challenge of computational efficiency in real-time robot motion planning still exists, especially for complex robot configurations. In this article, we propose a speedup solution to the IKP of a 7-DOF redundant robot, which can reduce the computational time when applying analytical and numerical methods for the IKP and improve the success rate of the analytical method.

Methods
Solving the IKP of redundant robots is a problem of nonlinear mapping from robot operating space to joint space. It has to seek the numerical solution of nonlinear transcendental equations. 7 At present, methods to solve the IKP of redundant DOF robots are mainly numerical, such as pseudo-inverse of Jacobian methods, 8 extended Jacobian methods, 9 task-space augmentation methods, 10 gradient projection methods, 11 damped least square methods, 12 and weighted least-norm methods. 13 Besides, there are analytical methods to solve the IKP of redundant robots with specific geometric properties, such as solution algorithms based on screw theory, 14 geometric methods for robots with unique geometric features, 15,16 and the analytical approach by fixing a certain DOF. 17 We denote the configuration space of an n-joint, seriallink articulated robot as Q, which consists of n-tuples elementq ¼ ðq 1 ; q 2 ; . . . ; q n Þ T 2 R n , each q i representing the ith joint value of the robot, and its operating space as X, which consists of 6-tuplesx 2 R 6 with three translation variables p x , p y , and p z and three rotation angle variables a, b, and g. Then, a forward kinematic equation of this robot is represented as Function FK : Q ! X is differentiable. For eachx in operating space, according to the IK equation IKðxÞ 1 qjx ¼ FKðqÞ f g , there exist infinite sets of joint valuesq corresponding to one same pointx in operation space. In other words, the IK equation will have infinite solutions.

IKfast approach
We can use the D-H convention, kinematic chains, or unified robot description format (URDF) to represent the geometry of a robotic mechanism. An articulated joint has a revolute joint variable and other constant parameters. Regardless of how to represent the geometry of a robot, the coordinate system of one of its joints relative to the reference frame can be defined as A i ¼ T i J i , where T i is a constant affine transformation computed by the invariant geometrical parameter and J i is a transformation depending on variable q i , which is a rotation angle around an arbitrary In equation (2),! is a 4 Â 4 skew matrix defined bŷ Thus, the transformation of the end-effector can be formulated by continuous multiplication of the fundamental transformation: T ee ¼ A 1 A 2 . . . A n . With respect to the base, the position and orientation of the end-effector are represented as a 4 Â 4 transformation matrix, T ee , and it is represented as As a result, the IKP corresponds to computing the unknown variables, q i 's.
Diankov believed that there are four different analytical methods to solve the IK problem. 18 The first method is the algebraic representation of computational formulas, where the main problem is how to solve higher-order univariate polynomials. Univariate higher-order equations are liable to become ill-conditioned when looking for their roots. The second method is to analyze and solve the structure of the solution set of the polynomial system, but there is a numerical ill-conditioning. The third method is to apply linear algebra and formulate the solution process as an eigendecomposition. 19 Although the eigendecomposition method is the fastest of all the proposed methods, the eigendecomposition of the 48 Â 48 matrix still suffers from numerical instability. The fourth method we choose in this article, IKfast, is a solver plug-in developed by Diankov. 18 The IKfast method emphasizes the numerical stability of the solution process rather than the general applicability of the IK solution. The IKfast method can find the most robust closed-form solution for 6-DOF robots. For a robot with one DOF redundancy, a joint is taken as the free joint, and it is discretized between its upper and lower limit so that a similar closed-form solution can be obtained.
The IKfast method fundamentally changes the way researchers solve the IKP when doing kinematic planning for serial-chain manipulators. All researchers can easily and safely integrate analytical algorithms into their motion planners to obtain multiple inverse solutions, rather than relying on the gradient-based methods, which are plagued by initial values, numerical errors, and slow computing time, and even worse, by which only one approximate solution can be obtained. The IKfast method analyzes the structure of the whole equations derived from the forward kinematics and divides the functional relationship into four cases. Each entry of the matrix in the left side equations is constructed by the sine and cosine functions of the joint variables.
In equation (4), it is easy to see that there are 12 equations and they have scalar components on the right side. In the matrix, the rotation part is equivalent to the product of three elementary rotation matrices generated from a set of Euler angles or fixed angles, which represents the orientation of the end-effector. And the first three rows and three columns of T ee are orthonormal, plus three-position equations, only six equations are independent. Thus, for a robot with n joints, the IKP corresponds to solving six equations with n unknowns. Usually, solving these equations involves complicated trigonometry and generally does not yield a simple solution. When joints number n6, the number of independent equations is less than the DOF, thus it is called underconstrained problem.
When considering sinq i and cosq i as unknown variables, combined with trigonometric constraints sin 2 q i þ cos 2 q i ¼ 1 for all joint rotation angles, we can obtain a set of equations with up to 2n unknowns. In order to build a stable IK solver, our first task is to take advantage of as many simple structures as possible, starting with all variables that can be solved with low-degree polynomials. The solving process becomes more complicated when coupled pairs of joint variables exist. In this case, joint variables can be solved by finding the intersection of the quadratics. If there are more joint variable couplings, we seek a fast and robust algorithm for the IK of a manipulator with n joints. The solutions of polynomial equations will be roughly divided into the following four types: (1) The angle q can be solved from two simple linearly independent equations with the functions of sine and cosine. The equation sinq ¼ a has two solutions q ¼ Atan2ð ffiffiffiffiffiffiffiffiffiffiffiffiffi 1 À a 2 p ; aÞ and q ¼ ÀAtan2ð ; aÞ. Likewise, as to the equation (2) The little more complicated equation, such as the transcendental equation acosq þ bsinq ¼ 0, has two solutions q ¼ Atan2ðÀa; bÞ and q ¼ Atan2ða; ÀbÞ. When the right hand is a nonzero constant, the equation becomes acosq þ bsinq ¼ c, and it can be solved by q ¼ Atan2ðb; aÞ+ Atan2ð ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi a 2 þ b 2 À c 2 p ; cÞ. A final solution can be solved within p radians. The set of equations ðcf À bdÞ=ðaf À beÞ .
(3) Also resolve to equations of the form cos 2 q ¼ a, which can have up to four solutions: q ¼ +arccosð+ ffiffi ffi a p Þ.
(4) The most complex condition is to resolve into a high-degree multivariate polynomial. In this case, we have to solve the determinant Ax 2 þ Bx þ C ¼ 0 problem. Choosing one joint as the free joint and assigning a value by increment, the method developed by Manocha and Canny 20 can solve the solution to the IKP for the 7R serial manipulator of general geometry.
Failure may happen when the IKfast solver runs at the initial guess for the free joint. In this case, the value of the free joint can be set according to Algorithm 1. In this way, the success rate will improve markedly.

Newton-Raphson method
The NR method is a root-finding algorithm that produces successively better approximations to the true root for realvalued functions. In robotics, the NR method can solve the IKP of robots as a numerical method, and it needs to compute the Jacobian matrix and its pseudo-inverse. In the NR method, the forward kinematics of the initial configuration q temp is computed, and the result is expressed as a twist T temp . Given the desired pose, its corresponding twist is expressed as T d . Thus, we have the difference of the two twists: DT ¼ T d À T temp . Applying the eigendecomposition and the Moore-Penrose pseudo-inverse, we can compute joint increment Dq. For the new joint configuration q temp ¼ q temp þ Dq, an iteration process is executed until DT ! 0. However, there are two necessary conditions for the success of the NR method: one is that the joints of the robot do not pass through singularities during the movement from the initial configuration to the final configuration; the other is that the desired end-effector position is located in the working space of the robot. Due to these limitations, the NR method largely depends on the initial value and does not guarantee global convergence.

Levenberg-Marquardt algorithm
The Levenberg-Marquardt algorithm 21 (LMA) can solve nonlinear least-squares problems. The LMA, also known as the damped least-squares method, generally converges faster than first-order backpropagation methods. 22 In the LMA, the damping factor adopts the square norm of the residual of the original equation with a bit of deviation, which helps to prevent the algorithm from escaping local minima. By choosing an appropriate damping factor, the LMA becomes more robust. It can overcome both problems: singularity and solvability. As an alternative numerical method to solve the IKP, the LMA significantly improves the numerical stability and convergence performance.
For an n-DOF robot, all its joint variables form a vector q. Then, the kinematics of the robot is mathematically represented by the constraint relationship between a pose ðp; RÞ andq. Then, the positional constraint on the endeffector is represented as f p ðqÞ ¼p, where f p ðqÞ 2 R 3 is the current position of the end-effector. Meanwhile, an orientational constraint on the end-effector is represented as f R ðqÞ ¼ R, where f R ðqÞ 2 SOð3Þ is the rotation matrix of the current orientation of the end-effector. The desired pose of the end-effector in the operating space is expressed asp d 2 R 3 and R d 2 SOð3Þ. In general, the residualẽðqÞ can be expressed as where vðRÞ 2 R 3 , proposed by Luh et al. 23 to evaluate the orientational error, then modified by Sugihara. 21 For an arbitrary R 2 SOð3Þ, vðRÞ is equivalent to an angle-axis vector. Furthermore, the whole residual vectorẽðqÞ 2 R m is defined asẽ where m is the total amount of constraints. The LMA focuses on the following minimization objective where W e ¼ diag w e;i È É ðw e;i 0 for 8i ¼ 1*mÞ is a weighting matrix on the whole residual. NR is available to solve the above objective function and accepts minimization numerical results as approximate solutions. From the evaluation function EðqÞ, the following equations are acquired rE ¼ẽ T W e rẽ (8) where @rẽ @q i^À @J @q i is related to the Hessian matrix of the robot. Then, for this minimization objective, NR operates according to the following update convention q kþ1 ¼ q k À ðr 2 EÞ À1 ðrEÞ T However, the objective in IK solution is to find a descent direction of the evaluation function EðqÞ rather than to Algorithm 1. To generate a new guess for the free joint.

Require:
q ini , the initial guess for the free joint q upper , the upper limit of the free joint q lower , the lower limit of the free joint Dq, the search discretization increment Ensure: q new , a new guess; 1: Initializing:q new 0, counter 0 2: n pos FLOOR ðq upper À q ini Þ=Dq , getting the step number from the initial guess to the upper limit 3: n neg CEIL ðq lower À q ini Þ=Dq , getting the step number from the lower limit to the initial guess 4: counter GETCOUNTðn pos ; n neg ; counterÞ, to generate a new counter between n pos and n neg 5: q new q ini þ counter Ã Dq, to generate a new guess for the free joint 6: return q new obtain its exact curvature. In order to simplify the computation, it is preferable to generate a positive-definite matrix to substitute r 2 E in equation (10). If the S summation term of equation (9) is omitted, it becomes the same as the Gauss-Newton method, and equation (10) turns to where J k J ðq k Þ, e k eðq k Þ. The iteration can continue in equation (11), as long as J k is non-singular. But, if J k is singular, the whole method becomes numerically ill-posed. However, the LMA can fix this problem by adding a damping factor and turning equation (11) to where W n is the damping factor, and W n ¼ diag w n;i È É ðw n;i 0 for 8i ¼ 1*mÞ. In equation (13), H k must be guaranteed to be proper and positive-definite. Under this circumstance, the incremental term necessarily varies toward a descent direction. As Nakamura and Hanafusa 24 pointed out, the iteration in equation (12) is also equivalent to solving the mixed minimization problem as shown as below where Dq k q kþ1 À q k and r k e k À J k Dq k . At last, after a number of iterations, equation (14) converges to a certain configuration that has the minimum joint distance from the initial one.

Sequential quadratic programming method
If the initial guess is close to the right solution, the LMA optimization method can make the solution algorithm converge faster. However, it does not handle arbitrary initial guesses well. The third numerical method for IK solving is the sequential quadratic programming (SQP) method, 25 which uses the quasi-Newton method and the Broyden-Fletcher-Goldfarb-Shanno (BFGS) 26 gradient projection algorithm to find the IK solution. As an iterative algorithm for nonlinear optimization, the SQP method is adequate for solving the problems when the objective function with constraints is twice consecutively differentiable. The SQP uses the BFGS algorithm as its iterative search method. The BFGS is a Newtonian approximation method and has performed well even for non-smooth optimizations. Compared to the conjugate gradient method, this algorithm requires more computation and storage space per iteration, but it usually converges in fewer iterations. Although the BFGS improves the performance of non-smooth optimization to a limited extent, it can still get stuck in local minima. Concerning the above facts, in specific applications, measures such as local minimum detection and random restarts are employed to avoid the algorithm getting stuck.
In the SQP method, the Euclidean distance error between the Cartesian pose of the current configuration and desired Cartesian pose is computed. Moreover, its square is taken as the optimization objective function, subject to some bounding constraints. The 6-element twist vector T err is used as the metric to measure the distance error and angular error in Cartesian space, and the sum of squares of each element in T err is minimized in the SQP method. The objective function is minimized as where T cur ¼ FKðq cur Þ; s:t:b l q c ur b u , b l is the lower joint displacement limit, and b u is the upper joint displacement limit. For the optimization process, the SQP method uses a BFGS update of the B-matrix and an L1-test function in the step-length to iteratively search the IK solution. 27 The SQP method is more robust than the LMA in finding IK solutions and is more effective when the configurations are close to joint limits or when the initial guess is far from the solution. It utilizes the gradient of the cost function from previous iterations to compute approximate second-derivative values. This value determines the steps to take in the current iteration. The gradient projection method is used to deal with the boundary constraints of the cost function constructed from the upper and lower joint limits of the robot model. By constantly correcting the calculated direction, the search direction is always valid. If the initial guess is close to the solution, the SQP method can quickly find the IK solution.

Speedup method based on sampled data
For 7-DOF redundant robots, the IKfast approach needs one joint fixed and to be free to change value according to Algorithm 1 when failure happens, then to solve the displacements of the other joints. With respect to the initial joint value of the fixed joint, the IKfast approach may fail. If it happens, an increment will add to the initial value until the IK solution succeeds in the running time limit. As to the three numerical IK solutions, a set of initial joint values, such as current or nominal joint values, is taken as a seed state to compute Jacobian or the pose error. IK solvers require several iterations before they find the IK solution or fail because of singular configurations or the arrival of end conditions. To reach a particular precision, this kind of approach is computationally expensive in many cases, which can be numerically unstable. In view of the situation mentioned above, we propose a method to speed up solving the IKP based on sampled data which are computed using the forward kinematics equation under certain conditions.
After computing by the forward kinematics formula, the end-effector has a unique position and orientation corresponding to a given robot configuration. By a kind of random number generation method, a large number of random joint coordinates can be generated in the joint space of the robot. After mapping them to the operating space, each group of joint values and the corresponding pose of the end-effector are recorded according to a specific filtering mechanism. In this way, we can obtain a large amount of sampled data. Choosing data among them as a seed state can improve the analytical method or numerical methods for IK solutions. These sampled data can be obtained by following the steps below.
(1) To set several parameters: Maximum of samples-it will have an effect on the performance of this proposed method.
Minimum distance of configuration-it decides the sampling density; if a newly generated random configuration is very close to the one already existing in the sample data, it will be discarded.
Minimum distance of pose-it also decides the sampling density; if the distance between the computed pose of the end-effector and each one in the sample data is less than the parameter, it will be discarded.
(2) Generating a random joint configuration, if it is self-collision-free and satisfies the conditions in step (1), its corresponding pose will be computed. If the result satisfies the conditions in step (1), the joint configuration and its corresponding pose will be recorded as an entry.
(3) Repeating step (2) until the number of sampled data gets to the limit.
After obtaining enough sample data, the next move is how to employ them. No matter which IK solver is used, these sampled data are read at first. Then, according to the desired task pose, the speedup method searches for the nearest pose in the sampled data. When the nearest entry is found, the IK solver carries on using the joint values in the searched entry as the seed state. As a result, the IK solver can perform better than the conventional way. An example of employing the speedup method for a random joint state is shown in Appendix 1.
In rare cases, our speedup method would fail to solve the IKP. If it happens, one measure is to randomly generate a set of angle values within the range of all joints. Then taking them as a new initial guess for the four original methods, the IK solvers start a new iteration.

Results and discussion
In order to evaluate the performance of our IK solution speedup method, a simulation test is designed to solve the IKP for the Baxter robot, which has two 7-DOF manipulators. The test is executed on an Ubuntu Linux PC with Intel ® Core TM i7-4712MQ@2.3GHz and 16G memory.
After analyzing the URDF files in the robot's control system, the D-H parameters are worked out. When taking the upper shoulder link as the base link, the left arm and right arm of the robot have the same D-H parameters, and their installation positions have the same coordinate transformation, translð0:055695; 0; 0:011038Þrotzð0Þ rotyð0Þrotxð0Þ. In this test, the SI unit of angle is rad and the SI unit of length is m. The left arm is taken as the test object, and its D-H parameters by Craig's D-H parameter convention 28 are listed in Table 1, and Figure 1 shows its corresponding D-H parameter motion model.   The primitive analytical and numerical IK solvers are IKfast, NR, LMA, and SQP. The speedup versions are IKfast-SP, NR-SP, LMA-SP, and SQP-SP. Between the joint limit ranges, 100,000 random configurations of the left arm are generated, and their corresponding poses are computed by forward kinematics. These IK solvers search the IK solution for the 100,000 poses. In the IKfast solver, the free joint is the sixth joint, and the initial value is zero. The initial value is zero for all joints in the three numerical solvers: NR, LMA, and SQP. The sampled data size used for the speedup method is 1000, 5000, 10,000, and 50,000 in the four speedup IK solvers: IKfast-SP, NR-SP, LMA-SP, and SQP-SP. The value of timeout is 0.01s for each IK solver. Average cost time and success rate per IK solver call are measured, and the results are shown in Tables 2 and  3. We visualize the summary statistics of the running time with a box plot, as shown in Figures 2 to 5. In Figures 2 to 5, when the sampled data size is zero, it means that the IK solvers use the zero configuration or a random joint state as the initial guess.
In Tables 2 and 3, the data size is the number of candidate seed states for the IK solvers. The last line is the average cost time of the primitive IK solvers in Table 2. The IKfast solver has the longest running time, up to 14:1271 ms. When the sampled data size is 10,000, the IKfast-SP, NR-SP, and LMA-SP solvers have the best performance. However, for the SQP-SP solver, the average cost time is the smallest when the sampled data size is 1000. In terms of the success rate, there is no significant     change in the NR-SP, LMA-SP, and SQP-SP and their primitive versions. However, the IKfast-SP solver promotes the percentage compared to the IKfast. The zero configuration is taken as the seed state for the four primitive IK solvers in the simulation. If the joint distance is far from the desired configuration for a random robot configuration, the numerical IK solvers will execute more iterations before they find the IK solutions. In addition, using the default initial guess, the NR and LMA solvers may fail, and in this case, a new joint state will be generated at random and taken as the seed state. However, the speedup method provides a closer configuration as the seed state and results in fewer iterations, as shown in Table 4. As for the IKfast solver, the initial value assigned to the free joint is not possibly suitable or is far from the theoretical value. The IKfast solver spends more time with several iterations to change the value of the free joint before solving the IKP, but in most cases, the speedup method chooses a better initial value. Due to the timeout being set to a short time, for example, 0:01 s, the primitive IK solvers will fail before the convergence. In Tables 2 and 4, it is obvious that the IKfast-SP, NR-SP, LMA-SP, and SQP-SP solvers are more efficient in solving the IKP than the primitive ones.

Conclusion
To improve analytical and numerical IK solutions, we propose a speedup method. The speedup method performs random sampling many times in the configuration space, saves those configurations that satisfy the given constraints and their corresponding pose data, and generates a database file with a designed data structure. For a desired endeffector pose, the method searches the pose data loaded into memory according to the proximity principle until the nearest pose is found. Then, it uses the corresponding joint values of the nearest pose as the initial guess.
We have three conclusions: (1) if the original IK solver is slow, significant improvements can be obtained with the speedup method; (2) if the original IK solver is fast, then the speedup method does not improve performance much and can make it worse; and (3) for the analytical method, IKfast, the speedup method can acquire better success rate. In addition, performance can potentially be improved by tuning sampled data sizes. The result in the third section verifies that the proposed method promotes the performance of the primitive IK solvers.
Our speedup method is to choose a better initial guess for four IK methods to solve the IKP of 7-DOF robots, and we do not consider the singular problem for the time being. Of course, the speedup method is applicable to modify the numerical approach 29 for 6-DOF robots, but research on whether it has the advantage over the closed-form methods 30 needs to be done in the future. In addition, we do not test whether the speedup method can be applied to parallel 31 and hybrid robots. 32,33 Much more work should be done before the method can solve the IKP of parallel and hybrid robots.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the National Key Research and Development Program of China under Grant 2019YFB1310201.