# Two Optimized General Methods for Inverse Kinematics of 6R Robots Based on Machine Learning

Authors: Xiaoqi Wang; Jianfu Cao; Lerui Chen; Heyu Hu
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

For the 6R robot, there is no analytical solution for some configurations, so it is necessary to analyse inverse kinematics (IK) by the general solution method, which cannot achieve high precision and high speed as the analytical solution. With the expansion of application fields and the complexity of application scenarios, some robots with special configuration have become the research hotspot, and more high-speed and high-precision general algorithms are still being explored and studied. The present paper optimized two general solutions. Elimination is a numerical solution, which has high accuracy, but the solution process is complex and time-consuming. The present paper optimized the elimination method, derived the final matrix expression directly through complex coefficient extraction and simplifying operation, and realized one-step solution. The solving speed was reduced to 15% of the original, and the integrity of the method was supplemented. This paper proposed a new optimization method for the Gaussian damped least-squares method, in which the variable step-size coefficient is introduced and the machine learning method is used for the research. It was proved that, on the basis of guaranteeing the stability of motion, the average number of iterations can be effectively reduced and was only 4-5 times, effectively improving the solving speed.


---

## Body

## 1. Introduction

For 6R robots, most configurations satisfy the Pieper criterion; that is, the adjacent three axes are parallel or intersect at one point, and there is analytical solution, which can be solved with high speed and high precision. Part of the configuration does not meet the Pieper criterion and has no analytical solution due to special operating requirements, so it needs to be solved by the general solution method. To meet the requirements of smoothness, higher speed, and higher precision, some new general methods and optimization methods are still being explored and studied. Among numerous methods, the elimination method is time-consuming and complex, but it has the highest precision and can get all the solutions under the same terminal posture. This paper optimized the elimination method to improve the solving speed and make the algorithm more complete. On the basis of the Gaussian damped least-squares method, this paper introduced the variable step-size coefficient, studied the influencing factors of the coefficient, and determined the coefficient through the regression algorithm of machine learning. The speed is superior to other iterative optimization methods. The average number of iterations is only 4 to 5 times, which effectively improves the solving speed and basically approximates the speed of analytical solutions. For inverse kinematic problems, there are three main categories of algorithms: iterative algorithm, numerical and geometric methods, and soft computing methods.The Jacobian matrix pseudoinverse method is proposed in [1], and it is the most commonly used method of robot inverse kinematics. The algorithm is unstable when the robot approaches the singular configuration. Four Jacobian-based methods of solving the inverse kinematics are evaluated in [2]. A novel formula for calculating the pseudoinverse is introduced for a class of redundant robots in [3]. The optimal approximation of the Jacobian pseudoinverse algorithm by the extended Jacobian algorithm is described in [4]. On the basis of the pseudoinverse method, the damped least-squares (DLS) method [5, 6] introduces the damping factor to balance the solution accuracy and the joint velocity near the singular configuration, so as to make the motion more stable. The DLS method is used to analyse the inverse kinematics of the 7R 6-DOF robots with the hollow nonspherical wrist in [7]. Based on different damping factor selection strategies, the selectively damped least-squares (SDLS) method is proposed in [8]. The Gaussian damped least-squares (GDLS) method is described in [9]. Based on the Gaussian distribution of the damping factor, the Gaussian function is applied to determine the damping factor, which can continuously transition from undamped to damped, making the motion more stable and reducing errors. A deeply learnt damped least-squares method is proposed for solving IK of the spatial snake-like robot in [10]. A deep network is built for prediction of the unique damping factor required for each target point. A hierarchical iterative inverse kinematic algorithm is proposed in [11].An elimination method for inverse kinematics of a 6R robot is proposed in [12, 13], which simplifies the inverse kinematics into a 16-degree polynomial problem. Then, the matrix eigenvalue decomposition is used to replace the solving equation of higher degree. Inverse iterations of the Jacobian matrix and the elimination method are combined in [14] to study the 6-DOF manipulator and polymers. Literature [15] optimized the elimination method and verified it with an example. A new method for inverse kinematics while considering jerk limits is proposed in [16] to efficiently handle acceleration and jerk constraints. A generalized solution for a subproblem of inverse kinematics based on an exponential formula product is proposed in [16]. Conformal geometric algebra is used to develop analytical inverse kinematic solutions for the KUKA Agilus robot and the UR5 robot in [17]. In [18], to avoid the problem of singularity, the product of exponentials method based on screw theory is employed for kinematic modeling. In addition, the inverse kinematics is solved by adopting analytical, geometric, and algebraic methods combined with the Paden–Kahan subproblem. Various optimized numerical and iterative methods failed to achieve the same performance as analytical solutions.Various soft computing methods [19–26] based on the artificial neural network and genetic algorithm are also the research hotspots in recent years. A comparative study between different soft computing-based methods (artificial neural network, adaptive neuro-fuzzy inference system, and genetic algorithms) applied to the problem of inverse kinematics is presented in [19]. A genetic algorithm based on extreme learning machine and sequential mutation is presented in [20]. A hybrid Taguchi deoxyribonucleic acid (DNA) swarm intelligence for solving the inverse kinematic redundancy problem is presented in [21]. In [22], an online adaptive strategy based on the Lyapunov stability theory is presented to solve the inverse kinematics of redundant manipulators. The neural network and genetic algorithms are used together to solve the inverse kinematics of a six-joint manipulator to minimize the error at the end effector in [23]. A neural-network committee machine (NNCM) is designed in [24] to solve the inverse kinematics of a 6R redundant manipulator to improve the precision of the solution. An approach for solving the inverse kinematics of manipulator robots based on soft computing algorithms is introduced in [25]. In [26], the inclusion of the current configuration of joint angles in the ANN significantly increased the accuracy of ANN estimation of the joint angle output. Various soft computing methods can achieve accuracy up to the micron level, but the solution speed is still a problem.Some machine learning regression models are used in this paper. The decision tree (DT) [27] is a prediction model to establish a mapping between object attributes and object values. It has fast speed and easily visualizes the model. The K-nearest neighbor (KNN) [28] algorithm is based on the limit theorem. Decision-making depends on a very small number of nearest samples. The random forest (RF) [29] integrates the set of decision trees with control variance by using the idea of bagging. The gradient boosting decision tree (GBDT) [30] generates a weak classifier in each iteration through multiple iterations. Each classifier trains on the basis of the residual of the previous one. The bagging method constructs a series of predictive functions and combines them into a predictive function. ExtraTreeRegressor (ETR) is similar to the RF and consists of many decision trees. Each decision tree is obtained by using all training samples, and the bifurcation values are obtained completely randomly.


## 2. Optimized Gaussian Damped Least-Squares Method

### 2.1. Gaussian Damped Least-Squares Method

The Jacobian matrix pseudoinverse method, which was proposed by Whiteney [1], is most frequently used to solve inverse kinematics. Based on the known speed of the end effector, the robot joint speed is determined as(1)θ˙=J+X˙,where J+ denotes the pseudoinversion of the Jacobian matrix. It can minimize the joint speed norm θ˙2 and tracking error X˙−Jθ˙2 by adopting the Jacobian matrix pseudoinverse method. However, this method is unstable near the singular points. The Jacobian matrix is SVD decomposed as follows:(2)J=USVT,where S is a diagonal matrix comprising singular values of J, which are arranged in the descending order as σ1≥σ2≥…σm, and then the joint speed is(3)θ˙=J+X˙=∑i=161σiVUTX˙.When the robot is close to the singular configuration, the minimum singular value is approximate to 0 and joint speed is infinitely great, which is the reason why the pseudoinverse method is unstable. The damped least-squares (DLS) method incorporates the damping factorλ on this basis to minimize X˙−Jθ˙2+λ2θ˙∗2, and as a result, as the robot approaches the singular configuration, the solving accuracy and joint speed are balanced, and then the joint speed is(4)θ˙∗=∑i=16σiσi2+λ2VUTX˙.Based on the DLS method, according to different strategies for damping factor selection, many methods have been developed. The Gaussian damped least-squares (GDLS) method allows for Gaussian distribution characteristic of the damping factor; the damping factor is determined through the following Gaussian function:(5)λi=λmax⋅e−σi/ε2,where λmax is the maximum value of the damping factor and ε is a scalar quantity indicating the region of singularities. In comparison with other methods which take constant or piecewise function as the damping factor, the GDLS method includes a better strategy for selecting the damping factor. On the one hand, when the robot approaches the singular configuration, the joint speed transitions from the undamped into the damped state in a continuous way so that the motion is steadier. On the other hand, each singular value corresponds to a damping factor, so the damping only acts on the singular vectors, thus avoiding unnecessary damping and reducing the error. λmax,ε can be determined through the genetic algorithm, the details of which are given in literature [9].


### 2.2. Variable Step-Size Coefficient

#### 2.2.1. Concept and Distribution of Variable Step-Size Coefficient

In order to elevate the solving speed, the variable step-size coefficientk and optimal variable step-size coefficient k_optimal are introduced based on the GDLS method.Definition.
In the iteration process,θcur=θcur+dθ is changed into θcur=θcur+kdθ so that the iteration proceeds rapidly towards convergence and k is called the variable step-size coefficient. As shown in Figure 1, the number of iterations changes with k. When k is a certain value, the number of iterations is minimized, and the k value is defined as the optimal variable step-size coefficient k_optimal.
It can be seen from Figure1 that when k is close to k_optimal, the number of iterations presents a continuous k-dependent change and gradually decreases. Hence, the number of iterations will approach the minimum value as long as k is close to k_optimal.
In order to investigatek_optimal distribution and the range of the minimum number of iterations, 104 poses are randomly selected, and they satisfy the following: D-H parameters are arbitrary, the iterative convergence error is Err=10−4, and initial and target joint angles are controlled within θini−θend≤5°. The GDLS method is used to solve inverse solution, and k_optimal is obtained each time. Based on the statistics of data, the probability distribution of k_optimal and minimum number of iterations are shown in Figure 2.
As shown in Figure2, k_optimal is mainly distributed within [20, 120]. Statistical data show that, for 95.4% of the poses, the minimum number of iterations corresponding to k_optimal can be controlled under 20.Figure 1
Number of iterations varying withk.Figure 2
Distribution ofk_optimal (a) and minimum iteration number (b).
(a)
(b)


#### 2.2.2. Influence Factors ofk_optimal

Through the analysis of the iterative process, it is estimated that the possible influence factors ofk_optimal include the iterative convergence error Err, the difference e between the current pose Tcur and the target pose Tend, the initial iterative value θi, and singularity.The difference between the current poseTcur and the target pose Tend is e=fex,ey,ez,eϕ,eθ,eψ, where ex,ey,ez belong to position errors, eϕ,eθ,eψ belong to Euler angular direction errors, and e=∑i=16ei.Generally, the minimum singular valueσm of the Jacobian matrix or the operand w=detJJT=σ1σ1…σm is adopted to characterize the singularity.When the other conditions remain unchanged, the changes ofk_optimal and minimum number of iterations with e,Err,θi,σm,andw are worked out, as shown in Figure 3.Figure 3
Influencing factors ofk_optimal.
(a)
(b)
(c)
(d)It can be seen from Figure3 that when Err≤0.01, Err does not affect k_optimal. When e is small, the effect on k_optimal is minor. Within a certain range, the effect on k_optimal can be great. When e exceeds a certain value, that is, when the current pose and target pose are quite different, the iteration will not converge. The initial iteration value θi has a great influence on k_optimal, and different joint angles can exert different effects on k_optimal. The minimum singular value and operand have no direct influence on k_optimal. On the whole, the main factors influencing k_optimal are θi and e.


### 2.3. Determination ofk_optimal Based on Machine Learning

The analysis of influence factors ofk_optimal shows that the goal is to determine k_optimal through θi and e. Though all kinds of methods are tried, it is very difficult to directly obtain their function expressions. Therefore, with θ1∼θ6 and e as the input and k_optimal as the output, the machine learning regression method is selected.10
5 poses are randomly selected. When other conditions are unchanged, θ1∼θ6 and e are changed to obtain the corresponding k_optimal which acts as original data for training and testing. Three frequently used statistical indicators, that is, root mean square error (RMSE), coefficient of determination (R2), and mean absolute error (MAE), are used to evaluate the model performance:(6)RMSE=1n∑i=1nYi,m−Yi,e2,(7)R2=∑i=1nYi,m−Yi,e2∑i=1nYi,m−Y¯i,m2,(8)MAE=1n∑i=1nYi,m−Yi,e.The regression models are frequently adopted in machine learning; 6 effective models are selected and compared, and the performance analysis is shown in Table1. A total of 1,000 groups of data are selected by adopting four methods to generate the regressive fitting curves in Figure 4.Table 1
Performance comparison of regression models.
Model
Training
Testing
R
2
RMSE
MAE
R
2
RMSE
MAE
Decision tree
1.000
0.000
0.000
0.931
0.721
0.463
K-nearest neighbor
0.848
1.100
0.803
0.796
1.237
0.919
Random forest
0.990
0.280
0.172
0.977
0.412
0.216
Gradient boosting
0.950
0.631
0.480
0.951
0.604
0.431
Bagging
0.988
0.309
0.185
0.975
0.432
0.245
ExtraTree
1.000
0.000
0.000
0.818
1.168
0.766Figure 4
Regression fitting curves of four methods.
(a)
(b)
(c)
(d)As shown in Table1, training effects of the decision tree and ExtraTree on the training samples are good. The effects of the random forest and bagging on test samples are better. It can be intuitively seen from Figure 4 that the random forest and bagging generate better fitting results. Through a comprehensive comparison, the random forest brings the best result. Meanwhile, the trained random forest model is used to predict k_optimal, and the average prediction time is only 0.9735μs, which can be neglected in comparison with solution time.


### 2.4. Flowchart of Optimized Method

The flowchart of the optimized GDLS method is shown in Figure5.Figure 5
Flowchart of the optimized algorithm.


### 2.5. Case of Optimized GDLS Method

A randomly selected 6R robot is taken as an example for the test, and D-H parameters are listed in Table2. The optimized method is named “E-GDLS,” and inverse solution is solved according to the flow in Figure 5. λmax=0.09,ε=0.05, and the iterative convergence error is set as Err=0.0001.Table 2
D-H parameters of a 6R robot in the case of the optimized GDLS method.
a
i
−
1
m
α
i
−
1
°
d
i
m
θ
i
°
1 (S)
0.78
30
0.61
θ
1
2 (L)
0.38
40
0.72
θ
2
3 (U)
0.62
10
0.93
θ
3
4 (R)
0.53
60
0.79
θ
4
5 (B)
0.46
20
0.39
θ
5
6 (T)
0.12
80
0.17
θ
6In Table3, IK results of 4 random data points of this method in the robot working space are summarized, and the number of iterations of the method before and after optimization is compared, as shown in Figure 6.Table 3
IK results of the optimized method based on 4 arbitrary data points.
ID
Target position (m)
Joint angles (deg)
Predictedk_optimal
e (mm)
Iterations
X
Y
Z
θ
1
θ
2
θ
3
θ
4
θ
5
θ
6
1
−0.569
−1.219
2.848
−17
−90
−170
66
−32
9
59
0.044
4
2
2.265
−0.364
2.044
72
−53
173
12
140
−97
65
0.035
5
3
1.522
−1.296
−0.424
54
−70
76
175
19
122
79
0.043
9
4
−2.397
0.168
1.698
−130
17
168
−126
−94
160
53
0.042
4Figure 6
Comparison of the number of iterations.The test results of four arbitrary data points show that the E-GDLS method can effectively predictk_optimal; the solving accuracy meets the requirements; the number of iterations is all within 10. The GDLS method needs about 200 iterations to converge to the error threshold in the same situation. This method can greatly reduce the number of iterations to improve the solving speed.Three trajectories are randomly selected in this study. Trajectory 1 only displays the position increment, while trajectory 2 displays increments of both the position and the Euler angular direction. Firstly, the number of iterations before and after the optimization is compared, and accuracy and stability of the optimized method are analysed through the changes of the spatial joint angle and trajectory tracking error. Trajectory 3 represents the status when the initial pose is approximate to a singular configuration, and it is mainly adopted to analyse the controlling effect of the optimized method on the joint speed relative to the Jacobian matrix pseudoinverse method.Trajectory 1: the initial configuration isθ=0.715,−2.914,1.919,2.897,−2.007,2.059Trad, and the increment is Δp=0.31,−0.72,0.43Tm.Trajectory 2. The initial configuration isθ=−1.658,0.244,2.705,−0.925,0.803,−2.251Trad, and the increment is Δp=−0.12,0.57,0.79,1.8675,0.5236,2.304Tm.Trajectory 3. The initial configuration isθ=0.734,3.1708,0.028,−3.184,0.059,−0.044Trad, and the increment is Δp=0.19,−0.45,0.33Tm.It can be seen from Figures7 and 8 that the number of iterations of the optimized method in this paper is obviously reduced compared with that of the GDLS method, so it can effectively improve solving speed. The joint angle in the joint space changes steadily with a minor trajectory tracking error, so stability and accuracy of the motion are guaranteed. As shown in Figure 9, when the robot is approaching the singular configuration, compared with the Jacobian matrix pseudoinverse method, the optimized method can still effectively control joint speed just as other damping methods.Figure 7
Comparison of the number of iterations (a), joint space trajectory (b), and tracking error of the optimized method (c).
(a)
(b)
(c)Figure 8
Comparison of the number of iterations (a), joint space trajectory (b), and tracking error of the optimized method (c).
(a)
(b)
(c)Figure 9
Joint velocities of the two methods near the singular configuration.
(a)
(b)In order to further analyse the solving performances of different methods,106 poses are randomly selected, the iterative convergence error is set as Err=0.0001, the maximum number of iterations is ilimit=500, and the difference value between the initial pose and the target pose is controlled within a certain range. The test environment is 3.2 GHz CPU, 8 GB RAM, and python. The Jacobian matrix pseudoinverse methods JT, DLS, and GDLS and the optimized method E-GDLS in this paper are adopted to solve the inverse solution, respectively. The average number of iterations, the average solving time, and the proportion of failed convergence due to exceeding maximum number are calculated for each method. The results are listed in Table 4.Table 4
Performance comparison of four methods.
Method
Average number of iterations
Mean solution time (ms)
Proportion of nonconvergence beyond the maximum number of iterations (%)
J
T
161.63
5.254
3.9
DLS
187.34
6.918
5.3
GDLS
182.53
6.546
3.3
E-GDLS
4.23
0.229
0.17From Table4, it can be found that the optimized method in this paper can effectively reduce the number of iterations and the average solving time and make iterative convergence faster so as to improve the method’s convergence performance.


## 2.1. Gaussian Damped Least-Squares Method

The Jacobian matrix pseudoinverse method, which was proposed by Whiteney [1], is most frequently used to solve inverse kinematics. Based on the known speed of the end effector, the robot joint speed is determined as(1)θ˙=J+X˙,where J+ denotes the pseudoinversion of the Jacobian matrix. It can minimize the joint speed norm θ˙2 and tracking error X˙−Jθ˙2 by adopting the Jacobian matrix pseudoinverse method. However, this method is unstable near the singular points. The Jacobian matrix is SVD decomposed as follows:(2)J=USVT,where S is a diagonal matrix comprising singular values of J, which are arranged in the descending order as σ1≥σ2≥…σm, and then the joint speed is(3)θ˙=J+X˙=∑i=161σiVUTX˙.When the robot is close to the singular configuration, the minimum singular value is approximate to 0 and joint speed is infinitely great, which is the reason why the pseudoinverse method is unstable. The damped least-squares (DLS) method incorporates the damping factorλ on this basis to minimize X˙−Jθ˙2+λ2θ˙∗2, and as a result, as the robot approaches the singular configuration, the solving accuracy and joint speed are balanced, and then the joint speed is(4)θ˙∗=∑i=16σiσi2+λ2VUTX˙.Based on the DLS method, according to different strategies for damping factor selection, many methods have been developed. The Gaussian damped least-squares (GDLS) method allows for Gaussian distribution characteristic of the damping factor; the damping factor is determined through the following Gaussian function:(5)λi=λmax⋅e−σi/ε2,where λmax is the maximum value of the damping factor and ε is a scalar quantity indicating the region of singularities. In comparison with other methods which take constant or piecewise function as the damping factor, the GDLS method includes a better strategy for selecting the damping factor. On the one hand, when the robot approaches the singular configuration, the joint speed transitions from the undamped into the damped state in a continuous way so that the motion is steadier. On the other hand, each singular value corresponds to a damping factor, so the damping only acts on the singular vectors, thus avoiding unnecessary damping and reducing the error. λmax,ε can be determined through the genetic algorithm, the details of which are given in literature [9].


## 2.2. Variable Step-Size Coefficient

### 2.2.1. Concept and Distribution of Variable Step-Size Coefficient

In order to elevate the solving speed, the variable step-size coefficientk and optimal variable step-size coefficient k_optimal are introduced based on the GDLS method.Definition.
In the iteration process,θcur=θcur+dθ is changed into θcur=θcur+kdθ so that the iteration proceeds rapidly towards convergence and k is called the variable step-size coefficient. As shown in Figure 1, the number of iterations changes with k. When k is a certain value, the number of iterations is minimized, and the k value is defined as the optimal variable step-size coefficient k_optimal.
It can be seen from Figure1 that when k is close to k_optimal, the number of iterations presents a continuous k-dependent change and gradually decreases. Hence, the number of iterations will approach the minimum value as long as k is close to k_optimal.
In order to investigatek_optimal distribution and the range of the minimum number of iterations, 104 poses are randomly selected, and they satisfy the following: D-H parameters are arbitrary, the iterative convergence error is Err=10−4, and initial and target joint angles are controlled within θini−θend≤5°. The GDLS method is used to solve inverse solution, and k_optimal is obtained each time. Based on the statistics of data, the probability distribution of k_optimal and minimum number of iterations are shown in Figure 2.
As shown in Figure2, k_optimal is mainly distributed within [20, 120]. Statistical data show that, for 95.4% of the poses, the minimum number of iterations corresponding to k_optimal can be controlled under 20.Figure 1
Number of iterations varying withk.Figure 2
Distribution ofk_optimal (a) and minimum iteration number (b).
(a)
(b)


### 2.2.2. Influence Factors ofk_optimal

Through the analysis of the iterative process, it is estimated that the possible influence factors ofk_optimal include the iterative convergence error Err, the difference e between the current pose Tcur and the target pose Tend, the initial iterative value θi, and singularity.The difference between the current poseTcur and the target pose Tend is e=fex,ey,ez,eϕ,eθ,eψ, where ex,ey,ez belong to position errors, eϕ,eθ,eψ belong to Euler angular direction errors, and e=∑i=16ei.Generally, the minimum singular valueσm of the Jacobian matrix or the operand w=detJJT=σ1σ1…σm is adopted to characterize the singularity.When the other conditions remain unchanged, the changes ofk_optimal and minimum number of iterations with e,Err,θi,σm,andw are worked out, as shown in Figure 3.Figure 3
Influencing factors ofk_optimal.
(a)
(b)
(c)
(d)It can be seen from Figure3 that when Err≤0.01, Err does not affect k_optimal. When e is small, the effect on k_optimal is minor. Within a certain range, the effect on k_optimal can be great. When e exceeds a certain value, that is, when the current pose and target pose are quite different, the iteration will not converge. The initial iteration value θi has a great influence on k_optimal, and different joint angles can exert different effects on k_optimal. The minimum singular value and operand have no direct influence on k_optimal. On the whole, the main factors influencing k_optimal are θi and e.


## 2.2.1. Concept and Distribution of Variable Step-Size Coefficient

In order to elevate the solving speed, the variable step-size coefficientk and optimal variable step-size coefficient k_optimal are introduced based on the GDLS method.Definition.
In the iteration process,θcur=θcur+dθ is changed into θcur=θcur+kdθ so that the iteration proceeds rapidly towards convergence and k is called the variable step-size coefficient. As shown in Figure 1, the number of iterations changes with k. When k is a certain value, the number of iterations is minimized, and the k value is defined as the optimal variable step-size coefficient k_optimal.
It can be seen from Figure1 that when k is close to k_optimal, the number of iterations presents a continuous k-dependent change and gradually decreases. Hence, the number of iterations will approach the minimum value as long as k is close to k_optimal.
In order to investigatek_optimal distribution and the range of the minimum number of iterations, 104 poses are randomly selected, and they satisfy the following: D-H parameters are arbitrary, the iterative convergence error is Err=10−4, and initial and target joint angles are controlled within θini−θend≤5°. The GDLS method is used to solve inverse solution, and k_optimal is obtained each time. Based on the statistics of data, the probability distribution of k_optimal and minimum number of iterations are shown in Figure 2.
As shown in Figure2, k_optimal is mainly distributed within [20, 120]. Statistical data show that, for 95.4% of the poses, the minimum number of iterations corresponding to k_optimal can be controlled under 20.Figure 1
Number of iterations varying withk.Figure 2
Distribution ofk_optimal (a) and minimum iteration number (b).
(a)
(b)


## 2.2.2. Influence Factors ofk_optimal

Through the analysis of the iterative process, it is estimated that the possible influence factors ofk_optimal include the iterative convergence error Err, the difference e between the current pose Tcur and the target pose Tend, the initial iterative value θi, and singularity.The difference between the current poseTcur and the target pose Tend is e=fex,ey,ez,eϕ,eθ,eψ, where ex,ey,ez belong to position errors, eϕ,eθ,eψ belong to Euler angular direction errors, and e=∑i=16ei.Generally, the minimum singular valueσm of the Jacobian matrix or the operand w=detJJT=σ1σ1…σm is adopted to characterize the singularity.When the other conditions remain unchanged, the changes ofk_optimal and minimum number of iterations with e,Err,θi,σm,andw are worked out, as shown in Figure 3.Figure 3
Influencing factors ofk_optimal.
(a)
(b)
(c)
(d)It can be seen from Figure3 that when Err≤0.01, Err does not affect k_optimal. When e is small, the effect on k_optimal is minor. Within a certain range, the effect on k_optimal can be great. When e exceeds a certain value, that is, when the current pose and target pose are quite different, the iteration will not converge. The initial iteration value θi has a great influence on k_optimal, and different joint angles can exert different effects on k_optimal. The minimum singular value and operand have no direct influence on k_optimal. On the whole, the main factors influencing k_optimal are θi and e.


## 2.3. Determination ofk_optimal Based on Machine Learning

The analysis of influence factors ofk_optimal shows that the goal is to determine k_optimal through θi and e. Though all kinds of methods are tried, it is very difficult to directly obtain their function expressions. Therefore, with θ1∼θ6 and e as the input and k_optimal as the output, the machine learning regression method is selected.10
5 poses are randomly selected. When other conditions are unchanged, θ1∼θ6 and e are changed to obtain the corresponding k_optimal which acts as original data for training and testing. Three frequently used statistical indicators, that is, root mean square error (RMSE), coefficient of determination (R2), and mean absolute error (MAE), are used to evaluate the model performance:(6)RMSE=1n∑i=1nYi,m−Yi,e2,(7)R2=∑i=1nYi,m−Yi,e2∑i=1nYi,m−Y¯i,m2,(8)MAE=1n∑i=1nYi,m−Yi,e.The regression models are frequently adopted in machine learning; 6 effective models are selected and compared, and the performance analysis is shown in Table1. A total of 1,000 groups of data are selected by adopting four methods to generate the regressive fitting curves in Figure 4.Table 1
Performance comparison of regression models.
Model
Training
Testing
R
2
RMSE
MAE
R
2
RMSE
MAE
Decision tree
1.000
0.000
0.000
0.931
0.721
0.463
K-nearest neighbor
0.848
1.100
0.803
0.796
1.237
0.919
Random forest
0.990
0.280
0.172
0.977
0.412
0.216
Gradient boosting
0.950
0.631
0.480
0.951
0.604
0.431
Bagging
0.988
0.309
0.185
0.975
0.432
0.245
ExtraTree
1.000
0.000
0.000
0.818
1.168
0.766Figure 4
Regression fitting curves of four methods.
(a)
(b)
(c)
(d)As shown in Table1, training effects of the decision tree and ExtraTree on the training samples are good. The effects of the random forest and bagging on test samples are better. It can be intuitively seen from Figure 4 that the random forest and bagging generate better fitting results. Through a comprehensive comparison, the random forest brings the best result. Meanwhile, the trained random forest model is used to predict k_optimal, and the average prediction time is only 0.9735μs, which can be neglected in comparison with solution time.


## 2.4. Flowchart of Optimized Method

The flowchart of the optimized GDLS method is shown in Figure5.Figure 5
Flowchart of the optimized algorithm.


## 2.5. Case of Optimized GDLS Method

A randomly selected 6R robot is taken as an example for the test, and D-H parameters are listed in Table2. The optimized method is named “E-GDLS,” and inverse solution is solved according to the flow in Figure 5. λmax=0.09,ε=0.05, and the iterative convergence error is set as Err=0.0001.Table 2
D-H parameters of a 6R robot in the case of the optimized GDLS method.
a
i
−
1
m
α
i
−
1
°
d
i
m
θ
i
°
1 (S)
0.78
30
0.61
θ
1
2 (L)
0.38
40
0.72
θ
2
3 (U)
0.62
10
0.93
θ
3
4 (R)
0.53
60
0.79
θ
4
5 (B)
0.46
20
0.39
θ
5
6 (T)
0.12
80
0.17
θ
6In Table3, IK results of 4 random data points of this method in the robot working space are summarized, and the number of iterations of the method before and after optimization is compared, as shown in Figure 6.Table 3
IK results of the optimized method based on 4 arbitrary data points.
ID
Target position (m)
Joint angles (deg)
Predictedk_optimal
e (mm)
Iterations
X
Y
Z
θ
1
θ
2
θ
3
θ
4
θ
5
θ
6
1
−0.569
−1.219
2.848
−17
−90
−170
66
−32
9
59
0.044
4
2
2.265
−0.364
2.044
72
−53
173
12
140
−97
65
0.035
5
3
1.522
−1.296
−0.424
54
−70
76
175
19
122
79
0.043
9
4
−2.397
0.168
1.698
−130
17
168
−126
−94
160
53
0.042
4Figure 6
Comparison of the number of iterations.The test results of four arbitrary data points show that the E-GDLS method can effectively predictk_optimal; the solving accuracy meets the requirements; the number of iterations is all within 10. The GDLS method needs about 200 iterations to converge to the error threshold in the same situation. This method can greatly reduce the number of iterations to improve the solving speed.Three trajectories are randomly selected in this study. Trajectory 1 only displays the position increment, while trajectory 2 displays increments of both the position and the Euler angular direction. Firstly, the number of iterations before and after the optimization is compared, and accuracy and stability of the optimized method are analysed through the changes of the spatial joint angle and trajectory tracking error. Trajectory 3 represents the status when the initial pose is approximate to a singular configuration, and it is mainly adopted to analyse the controlling effect of the optimized method on the joint speed relative to the Jacobian matrix pseudoinverse method.Trajectory 1: the initial configuration isθ=0.715,−2.914,1.919,2.897,−2.007,2.059Trad, and the increment is Δp=0.31,−0.72,0.43Tm.Trajectory 2. The initial configuration isθ=−1.658,0.244,2.705,−0.925,0.803,−2.251Trad, and the increment is Δp=−0.12,0.57,0.79,1.8675,0.5236,2.304Tm.Trajectory 3. The initial configuration isθ=0.734,3.1708,0.028,−3.184,0.059,−0.044Trad, and the increment is Δp=0.19,−0.45,0.33Tm.It can be seen from Figures7 and 8 that the number of iterations of the optimized method in this paper is obviously reduced compared with that of the GDLS method, so it can effectively improve solving speed. The joint angle in the joint space changes steadily with a minor trajectory tracking error, so stability and accuracy of the motion are guaranteed. As shown in Figure 9, when the robot is approaching the singular configuration, compared with the Jacobian matrix pseudoinverse method, the optimized method can still effectively control joint speed just as other damping methods.Figure 7
Comparison of the number of iterations (a), joint space trajectory (b), and tracking error of the optimized method (c).
(a)
(b)
(c)Figure 8
Comparison of the number of iterations (a), joint space trajectory (b), and tracking error of the optimized method (c).
(a)
(b)
(c)Figure 9
Joint velocities of the two methods near the singular configuration.
(a)
(b)In order to further analyse the solving performances of different methods,106 poses are randomly selected, the iterative convergence error is set as Err=0.0001, the maximum number of iterations is ilimit=500, and the difference value between the initial pose and the target pose is controlled within a certain range. The test environment is 3.2 GHz CPU, 8 GB RAM, and python. The Jacobian matrix pseudoinverse methods JT, DLS, and GDLS and the optimized method E-GDLS in this paper are adopted to solve the inverse solution, respectively. The average number of iterations, the average solving time, and the proportion of failed convergence due to exceeding maximum number are calculated for each method. The results are listed in Table 4.Table 4
Performance comparison of four methods.
Method
Average number of iterations
Mean solution time (ms)
Proportion of nonconvergence beyond the maximum number of iterations (%)
J
T
161.63
5.254
3.9
DLS
187.34
6.918
5.3
GDLS
182.53
6.546
3.3
E-GDLS
4.23
0.229
0.17From Table4, it can be found that the optimized method in this paper can effectively reduce the number of iterations and the average solving time and make iterative convergence faster so as to improve the method’s convergence performance.


## 3. Optimized Elimination Method

### 3.1. Algorithm Description

The process of the elimination method is as follows: by reversible transformation of the matrix, it is obtained that(9)T3T4T5=T2−1T1−1TendT6−1,where Tend=lxmxnxpxlymynypylzmznzpz0001, in which the elements in columns 3 and 4 of the right and left matrices are equal, and 6 equations unrelated to θ6 are obtained as(10)p1:c3f1+s3f2+a2=c2h1+s2h2e1,(11)p2:c3f2−s3f1=s2h1−c2h2e2,(12)p3:f3=h3e3,(13)l1:c3r1+s3r2=c2n1+s2n2e4,(14)l2:s3r1−c3r2=c2n2−s2n1e5,(15)l3:r3=n3e6,where si=sinθi,ci=cosθi,sij=sinθi+θj,andcij=cosθi+θj.Through the vector calculation ofp and l, p⋅p,p⋅l,p×l,p⋅pl−2p⋅lp, 8 equations are derived as(16)p12+p22+p32e7,(17)p1l1+p2l2+p3l3e8,(18)p1l2−p2l1e9,(19)p2l3−p3l2e10,(20)p3l1−p1l3e11,(21)p12+p22+p32l1−2p1l1+p2l2+p3l3p1e12,(22)p12+p22+p32l2−2p1l1+p2l2+p3l3p2e13,(23)p12+p22+p32l3−2p1l1+p2l2+p3l3p3e14.Eliminatingθ1,θ2 and simplifying the 14 equations e1∼e14, 6 equations irrelevant to θ1,θ2 are obtained. The 6 equations are written in the form of a matrix equation as(24)∑v1=0,where v1=s4s5s4c5c4s5c4c5s4c4s5c51T and ∑ is a 6 ∗ 9 matrix containing only s3,c3. We convert s3,c3,s4,c4,s5,c5 into s3=2x3/1+x32,c3=1−x32/1+x32,s4=2x4/1+x42,c4=1−x42/1+x42,s5=2x5/1+x52,c5=1−x52/1+x52 and obtain the matrix equation(25)∑′v2=0,where v2=x42x52x42x5x42x4x52x4x5x4x52x51T and ∑′ is a 6 ∗ 9 matrix containing only x3. We multiply each row of the matrix equation by x4 and merge the equation obtained with the original one, to get the matrix equation(26)∑″v=0,where v=x43x52x43x5x43x42x52x42x5x42x4x52x4x5x4x52x51T and ∑″ is a 12 ∗ 12 matrix containing only x3.This is an overconstrained linear system. In order for this system to have a nontrivial solution, the coefficient matrix∑″ must be singular. The determinant of the coefficient matrix is a 16th degree polynomial in x3. The roots of this polynomial give the values of x3 corresponding to the solutions of the inverse kinematic problem. And it is simplified to find the matrix eigenvalues and eigenvectors. ∑″ is written as Ax32+Bx3+C, and equation (26) is written as(27)Ax32+Bx3+Cv=0.A, B, and C are known 12 ∗ 12 matrices. Equation (27) is written as(28)0I−A−1C−A−1Bvx3v=x3vx3v,i.e.,Mvx3v=x3vx3v.The eigenvalue ofM is x3, and θ3 is obtained from θ3=2tan−1x3. The eigenvector is V=vx3v/vx3v.


### 3.2. Optimization and Supplement of Elimination Method

The simplification process is given in this paper for the first time.θ1,θ2 are eliminated and 6 equations are obtained through 14 equations e1∼e14.Expressions ofs1,c1 are derived through e3,e6 and then substituted into e7,e8,e9,e14 to obtain 4 equations. Equations e1,e2,e4,e5,e10,e11,e12, and e13 are simplified through the following two formulas:(29)μ122a1e12−δ1e4+δ2e1−λ1μ1e10+μ1we2−μ1r−d1e5=0,(30)μ122a1e13−δ1e5+δ2e2−λ1μ1e11−μ1we1+μ1r−d1e4=0,where δ1=p2+q2+r−d12−a12andδ2=2pu+qv+r−d1w.The final expressions of matricesA,B,andC are derived for the first time so that the one-step solving of the matrix M can be realized in this paper.The complicated coefficient extraction process of the elimination method results in a large workload, so in most cases in other papers, the known quantities are substituted into the coefficient matrix, followed by extract variables, and the whole solving process should be implemented in multiple steps. In this paper, through the complicated coefficient extraction and simplified operation, the intermediate solving steps are streamlined, and the final expressions of matricesA,B, and C are directly derived, so the whole solving process is simplified as much as possible. One-step solving of the matrix M is realized, and only 1 ms or so is needed. This is the greatest contribution that this paper has made to the simplification of the elimination method. The expressions are not presented because they are too long. For the specific expressions, refer to the uploaded MATLAB code at https://github.com/Wangxiaoqi1031/robot1.Solving formulas for other joint anglesθ1,θ2,θ4,θ5, and θ6 in the simplest forms are given for the first time in this paper, so as to improve the integrity of the whole method.Through a large number of experiments,x4 and x5 are obtained by solving the ratio of large absolute values in the eigenvectors of the matrix M more accurately. Through analysis, it is possible that the maximum values are v1,v3,v10,andv12, and the calculation formulas are as follows:(31)θ4=tan−1v1v4θ5=tan−1v1v2maxvi=v1,θ4=tan−1v3v6θ5=tan−1v2v3maxvi=v3,θ4=tan−1v7v10θ5=tan−1v10v11maxvi=v10,θ4=tan−1v9v12θ5=tan−1v11v12maxvi=v12,(32)s1=μ1vd2+f3λ2+λ1d1−r–μ2c3f2–f1s3+qμ1wλ1−r3λ2+μ2c3r2–r1s3pμ12v−quμ12,θ1=sin−1s1orθ1=π−sin−1s1,(33)n=μ6nzλ1−c1nyμ1+nxs1μ1−λ6mzλ1−c1myμ1+mxs1μ1,m=lzλ1−c1lyμ1+lxs1μ1,l=s5μ4λ2λ3−c3μ2μ3+c4λ4μ3λ2+c3μ2λ3−s3s4μ2λ4+c5s4μ3λ2+c3μ2λ3+c4s3μ2,x6=n±m2+n2−l2l+m,θ6=2tan−1x6,(34)n′=lys1μ2+lxc1μ2,m′=lxs1μ2λ1−lzμ1μ2−lyc1μ2λ1,l′=s6μ5λ3λ4−c4μ3μ4+c5λ5μ4λ3+c4μ3λ4−s4s5μ3λ5+c6s5μ4λ3+c4μ3λ4+c5s4μ3−lzλ1λ2+lyc1μ1λ2−lxs1μ1λ2,x2=n′±m′2+n′2−l′2l′+m′,θ2=2tan−1x2.


### 3.3. Flowchart of Optimized Method

The flowchart of the optimized elimination method is shown in Figure10.Figure 10
Flowchart of the optimized algorithm.


### 3.4. Case of Optimized Elimination Method

A randomly selected 6R robot is taken as an example for the test, and D-H parameters are listed in Table5.Table 5
D-H parameters of a 6R robot in the case of the optimized elimination method.
a
i
−
1
m
α
i
−
1
°
d
i
m
θ
i
°
1 (S)
0.25
20
0.19
θ
1
2 (L)
0.95
30
0.37
θ
2
3 (U)
0.30
−45
0.10
θ
3
4 (R)
0.55
80
1.55
θ
4
5 (B)
0.16
−120
0.21
θ
5
6 (T)
0.22
100
0.13
θ
6We setθ1=14.5427°,θ2=−44.5435°,θ3=130.5472°,θ4=−70.2145°,θ5=60.2147°,anddθ6=160.9745°. The matrix of the target pose is obtained as Tend=T1,T2,T3,T4T, where(35)T1=−0.24811675957960,0.85817093890060,−0.44942264433624,2.28037877048099,T2=0.37824219259125,−0.34128340141828,−0.86049897365432,0.30849705188949,T3=−0.8918357008774,−0.38349482335858,−0.23991832588964,0.58096198011612,T4=0,0,0,1.The inverse solution is solved according to Figure10. The real results are shown in Table 6.Table 6
All groups of real solutions.
θ
1
θ
2
θ
3
θ
4
θ
5
θ
6
1
36.64732
55.0222
154.9653
77.53909
−136.2382
93.16235
2
36.64732
−50.54915
154.9653
77.53909
−136.2382
93.16235
3
36.64732
105.916
154.9653
77.53909
−136.2382
−9.495723
4
36.64732
−101.443
154.9653
77.53909
−136.2382
−9.495723
5
143.3527
110.5793
154.9653
77.53909
−136.2382
93.16235
6
143.3527
−59.60432
154.9653
77.53909
−136.2382
93.16235
7
143.3527
145.3161
154.9653
77.53909
−136.2382
−9.495723
8
143.3527
−94.34108
154.9653
77.53909
−136.2382
−9.495723
9
14.5427
20.82859
130.5472
−70.2145
60.2147
160.9745
10
14.5427
−44.5435
130.5472
−70.2145
60.2147
160.9745
11
14.5427
15.28996
130.5472
−70.2145
60.2147
−92.76167
12
14.5427
−39.00486
130.5472
−70.2145
60.2147
−92.76167
13
165.4573
98.19595
130.5472
−70.2145
60.2147
160.9745
14
165.4573
−59.02553
130.5472
−70.2145
60.2147
160.9745
15
165.4573
96.19785
130.5472
−70.2145
60.2147
−92.76167
16
165.4573
−57.02743
130.5472
−70.2145
60.2147
−92.76167We evaluate the performance of the methods with two indicators of accuracy and speed. We randomly select105 data points in the working space of the robot and test them under the environment of 3.2 GHz CPU, 8 GB RAM, and MATLAB and use the elimination method before and after optimization to solve IK, respectively. The worst, best, and average cases are calculated separately. The statistical results are shown in Table 7.Table 7
All groups of real solutions.
Best
Worst
Average
Accuracy
Elimination
2.374e10−13
7.324e10−11
2.318e10−12
Optimized elimination
3.423e10−13
4.012e10−11
2.054e10−12
Speed (ms)
Elimination
11.253
12.454
11.632
Optimized elimination
1.494
1.721
1.583The results show that there is not much difference in the accuracy of the method before and after optimization, and the solution speed of the method after optimization decreases from 11–13 ms to 1.4–1.8 ms. Therefore, it is concluded that the optimization of the elimination method in this paper can effectively improve the solution speed while ensuring high accuracy.When the elimination method is adopted, the proportion of poses which cannot be accurately solved is 0.086% as the matrixA is singular or the robot is close to the singular configuration, and this can be solved through the following scheme:IfA is singular, inverse solving is changed into pseudoinverse solving. The singular region is evaded in the trajectory planning.Poses which cannot be accurately solved can be processed through the iteration method.


## 3.1. Algorithm Description

The process of the elimination method is as follows: by reversible transformation of the matrix, it is obtained that(9)T3T4T5=T2−1T1−1TendT6−1,where Tend=lxmxnxpxlymynypylzmznzpz0001, in which the elements in columns 3 and 4 of the right and left matrices are equal, and 6 equations unrelated to θ6 are obtained as(10)p1:c3f1+s3f2+a2=c2h1+s2h2e1,(11)p2:c3f2−s3f1=s2h1−c2h2e2,(12)p3:f3=h3e3,(13)l1:c3r1+s3r2=c2n1+s2n2e4,(14)l2:s3r1−c3r2=c2n2−s2n1e5,(15)l3:r3=n3e6,where si=sinθi,ci=cosθi,sij=sinθi+θj,andcij=cosθi+θj.Through the vector calculation ofp and l, p⋅p,p⋅l,p×l,p⋅pl−2p⋅lp, 8 equations are derived as(16)p12+p22+p32e7,(17)p1l1+p2l2+p3l3e8,(18)p1l2−p2l1e9,(19)p2l3−p3l2e10,(20)p3l1−p1l3e11,(21)p12+p22+p32l1−2p1l1+p2l2+p3l3p1e12,(22)p12+p22+p32l2−2p1l1+p2l2+p3l3p2e13,(23)p12+p22+p32l3−2p1l1+p2l2+p3l3p3e14.Eliminatingθ1,θ2 and simplifying the 14 equations e1∼e14, 6 equations irrelevant to θ1,θ2 are obtained. The 6 equations are written in the form of a matrix equation as(24)∑v1=0,where v1=s4s5s4c5c4s5c4c5s4c4s5c51T and ∑ is a 6 ∗ 9 matrix containing only s3,c3. We convert s3,c3,s4,c4,s5,c5 into s3=2x3/1+x32,c3=1−x32/1+x32,s4=2x4/1+x42,c4=1−x42/1+x42,s5=2x5/1+x52,c5=1−x52/1+x52 and obtain the matrix equation(25)∑′v2=0,where v2=x42x52x42x5x42x4x52x4x5x4x52x51T and ∑′ is a 6 ∗ 9 matrix containing only x3. We multiply each row of the matrix equation by x4 and merge the equation obtained with the original one, to get the matrix equation(26)∑″v=0,where v=x43x52x43x5x43x42x52x42x5x42x4x52x4x5x4x52x51T and ∑″ is a 12 ∗ 12 matrix containing only x3.This is an overconstrained linear system. In order for this system to have a nontrivial solution, the coefficient matrix∑″ must be singular. The determinant of the coefficient matrix is a 16th degree polynomial in x3. The roots of this polynomial give the values of x3 corresponding to the solutions of the inverse kinematic problem. And it is simplified to find the matrix eigenvalues and eigenvectors. ∑″ is written as Ax32+Bx3+C, and equation (26) is written as(27)Ax32+Bx3+Cv=0.A, B, and C are known 12 ∗ 12 matrices. Equation (27) is written as(28)0I−A−1C−A−1Bvx3v=x3vx3v,i.e.,Mvx3v=x3vx3v.The eigenvalue ofM is x3, and θ3 is obtained from θ3=2tan−1x3. The eigenvector is V=vx3v/vx3v.


## 3.2. Optimization and Supplement of Elimination Method

The simplification process is given in this paper for the first time.θ1,θ2 are eliminated and 6 equations are obtained through 14 equations e1∼e14.Expressions ofs1,c1 are derived through e3,e6 and then substituted into e7,e8,e9,e14 to obtain 4 equations. Equations e1,e2,e4,e5,e10,e11,e12, and e13 are simplified through the following two formulas:(29)μ122a1e12−δ1e4+δ2e1−λ1μ1e10+μ1we2−μ1r−d1e5=0,(30)μ122a1e13−δ1e5+δ2e2−λ1μ1e11−μ1we1+μ1r−d1e4=0,where δ1=p2+q2+r−d12−a12andδ2=2pu+qv+r−d1w.The final expressions of matricesA,B,andC are derived for the first time so that the one-step solving of the matrix M can be realized in this paper.The complicated coefficient extraction process of the elimination method results in a large workload, so in most cases in other papers, the known quantities are substituted into the coefficient matrix, followed by extract variables, and the whole solving process should be implemented in multiple steps. In this paper, through the complicated coefficient extraction and simplified operation, the intermediate solving steps are streamlined, and the final expressions of matricesA,B, and C are directly derived, so the whole solving process is simplified as much as possible. One-step solving of the matrix M is realized, and only 1 ms or so is needed. This is the greatest contribution that this paper has made to the simplification of the elimination method. The expressions are not presented because they are too long. For the specific expressions, refer to the uploaded MATLAB code at https://github.com/Wangxiaoqi1031/robot1.Solving formulas for other joint anglesθ1,θ2,θ4,θ5, and θ6 in the simplest forms are given for the first time in this paper, so as to improve the integrity of the whole method.Through a large number of experiments,x4 and x5 are obtained by solving the ratio of large absolute values in the eigenvectors of the matrix M more accurately. Through analysis, it is possible that the maximum values are v1,v3,v10,andv12, and the calculation formulas are as follows:(31)θ4=tan−1v1v4θ5=tan−1v1v2maxvi=v1,θ4=tan−1v3v6θ5=tan−1v2v3maxvi=v3,θ4=tan−1v7v10θ5=tan−1v10v11maxvi=v10,θ4=tan−1v9v12θ5=tan−1v11v12maxvi=v12,(32)s1=μ1vd2+f3λ2+λ1d1−r–μ2c3f2–f1s3+qμ1wλ1−r3λ2+μ2c3r2–r1s3pμ12v−quμ12,θ1=sin−1s1orθ1=π−sin−1s1,(33)n=μ6nzλ1−c1nyμ1+nxs1μ1−λ6mzλ1−c1myμ1+mxs1μ1,m=lzλ1−c1lyμ1+lxs1μ1,l=s5μ4λ2λ3−c3μ2μ3+c4λ4μ3λ2+c3μ2λ3−s3s4μ2λ4+c5s4μ3λ2+c3μ2λ3+c4s3μ2,x6=n±m2+n2−l2l+m,θ6=2tan−1x6,(34)n′=lys1μ2+lxc1μ2,m′=lxs1μ2λ1−lzμ1μ2−lyc1μ2λ1,l′=s6μ5λ3λ4−c4μ3μ4+c5λ5μ4λ3+c4μ3λ4−s4s5μ3λ5+c6s5μ4λ3+c4μ3λ4+c5s4μ3−lzλ1λ2+lyc1μ1λ2−lxs1μ1λ2,x2=n′±m′2+n′2−l′2l′+m′,θ2=2tan−1x2.


## 3.3. Flowchart of Optimized Method

The flowchart of the optimized elimination method is shown in Figure10.Figure 10
Flowchart of the optimized algorithm.


## 3.4. Case of Optimized Elimination Method

A randomly selected 6R robot is taken as an example for the test, and D-H parameters are listed in Table5.Table 5
D-H parameters of a 6R robot in the case of the optimized elimination method.
a
i
−
1
m
α
i
−
1
°
d
i
m
θ
i
°
1 (S)
0.25
20
0.19
θ
1
2 (L)
0.95
30
0.37
θ
2
3 (U)
0.30
−45
0.10
θ
3
4 (R)
0.55
80
1.55
θ
4
5 (B)
0.16
−120
0.21
θ
5
6 (T)
0.22
100
0.13
θ
6We setθ1=14.5427°,θ2=−44.5435°,θ3=130.5472°,θ4=−70.2145°,θ5=60.2147°,anddθ6=160.9745°. The matrix of the target pose is obtained as Tend=T1,T2,T3,T4T, where(35)T1=−0.24811675957960,0.85817093890060,−0.44942264433624,2.28037877048099,T2=0.37824219259125,−0.34128340141828,−0.86049897365432,0.30849705188949,T3=−0.8918357008774,−0.38349482335858,−0.23991832588964,0.58096198011612,T4=0,0,0,1.The inverse solution is solved according to Figure10. The real results are shown in Table 6.Table 6
All groups of real solutions.
θ
1
θ
2
θ
3
θ
4
θ
5
θ
6
1
36.64732
55.0222
154.9653
77.53909
−136.2382
93.16235
2
36.64732
−50.54915
154.9653
77.53909
−136.2382
93.16235
3
36.64732
105.916
154.9653
77.53909
−136.2382
−9.495723
4
36.64732
−101.443
154.9653
77.53909
−136.2382
−9.495723
5
143.3527
110.5793
154.9653
77.53909
−136.2382
93.16235
6
143.3527
−59.60432
154.9653
77.53909
−136.2382
93.16235
7
143.3527
145.3161
154.9653
77.53909
−136.2382
−9.495723
8
143.3527
−94.34108
154.9653
77.53909
−136.2382
−9.495723
9
14.5427
20.82859
130.5472
−70.2145
60.2147
160.9745
10
14.5427
−44.5435
130.5472
−70.2145
60.2147
160.9745
11
14.5427
15.28996
130.5472
−70.2145
60.2147
−92.76167
12
14.5427
−39.00486
130.5472
−70.2145
60.2147
−92.76167
13
165.4573
98.19595
130.5472
−70.2145
60.2147
160.9745
14
165.4573
−59.02553
130.5472
−70.2145
60.2147
160.9745
15
165.4573
96.19785
130.5472
−70.2145
60.2147
−92.76167
16
165.4573
−57.02743
130.5472
−70.2145
60.2147
−92.76167We evaluate the performance of the methods with two indicators of accuracy and speed. We randomly select105 data points in the working space of the robot and test them under the environment of 3.2 GHz CPU, 8 GB RAM, and MATLAB and use the elimination method before and after optimization to solve IK, respectively. The worst, best, and average cases are calculated separately. The statistical results are shown in Table 7.Table 7
All groups of real solutions.
Best
Worst
Average
Accuracy
Elimination
2.374e10−13
7.324e10−11
2.318e10−12
Optimized elimination
3.423e10−13
4.012e10−11
2.054e10−12
Speed (ms)
Elimination
11.253
12.454
11.632
Optimized elimination
1.494
1.721
1.583The results show that there is not much difference in the accuracy of the method before and after optimization, and the solution speed of the method after optimization decreases from 11–13 ms to 1.4–1.8 ms. Therefore, it is concluded that the optimization of the elimination method in this paper can effectively improve the solution speed while ensuring high accuracy.When the elimination method is adopted, the proportion of poses which cannot be accurately solved is 0.086% as the matrixA is singular or the robot is close to the singular configuration, and this can be solved through the following scheme:IfA is singular, inverse solving is changed into pseudoinverse solving. The singular region is evaded in the trajectory planning.Poses which cannot be accurately solved can be processed through the iteration method.


## 4. Experiment

There is an ordinary 6R configurational robot experimental platform in the laboratory, as shown in Figure11. The processor of the controller is OMAPL-138, a DSP + ARM dual-core processor, the demonstrator is based on WinCE 6.0, and the maximum speed can reach 300°/s.Figure 11
Experimental platform.
(a)
(b)In order to compare ordinary analytical methods with the two optimized methods in this paper in terms of their solving performance, 1000 tracks are randomly selected, the iterative convergence error is set asErr=0.0001, and the maximum number of iterations is ilimit=500; data calculated from the DSP are shown in Table 8.Table 8
Performance comparison of three methods.
Method
Average solution time (μs)
Average solution accuracy
Analytic method
228
1.063e10−12
Optimized elimination method
1504
2.204e10−12
Optimized GDLS
234
0.0000989It can be seen from Table8 that the accuracy of the optimized elimination method is approximate to that of the analytical solution, and the solving time of the optimized Gaussian damping method is approximate to that of the analytical solution, too. There is no analytical solution for the robot configuration which does not conform to the Pieper criterion, and then it can be solved through the two optimized universal methods in this paper. The proper method can be selected, or the two methods can be combined according to different accuracy and speed requirements.


## 5. Conclusion

This paper optimized the elimination method and the Gaussian damped least-squares method. After optimization, for the elimination method, the average accuracy of10−12 can be achieved, and the solution time is 1 to 1.5 ms. For the Gaussian damped least-squares method, the average number of iterations is only 4 to 5, and the solving speed is close to that of the analytical solution. The precision and speed of these two optimized algorithms cannot be achieved by the other optimized algorithms. The elimination method is simplified to the extreme. Time is mainly spent in the calculation of the eigenvalues and eigenvectors of the M matrix. If this step can be improved, the speed of the algorithm can be further improved. The Gaussian damped least-squares method has the advantage of smooth motion itself. In this paper, by introducing the variable step-size coefficient, the number of iterations is greatly reduced, and the solving speed is effectively improved. Applying the method of machine learning to study the coefficient could achieve better results than the general fitting method. In practical application, if high speed is required, the iteration method can be applied to solve the problem. If high precision is required, the elimination method can be applied, or the two methods can be combined to solve the problem.



---

*Source: 8174924-2020-01-21.xml*