Singularity avoidance for manipulators with spherical wrists using the approximate damped reciprocal algorithm

Due to the loss of freedom, the stability and tracking ability of the manipulator around the singularity become worse. This article aims at improving the accuracy of the manipulator and ensuring the stability of the system with the damped reciprocal method. Firstly, the singularities are separated into forearm and wrist singularities to obtain the singular factors of the manipulator respectively. Secondly, a new mathematical function of the approximate damped reciprocal of the singular factor is proposed. Thirdly, the singularities are avoided by modifying the Jacobian matrixes of the manipulator with the approximate damped reciprocal algorithm. Finally, the effectiveness and the stability of the system are proved by the simulations on a manipulator with the spherical wrist. The simulation results prove that this method can largely improve the accuracy of the end-effector and can ensure the stability of the system around the singular region.


Introduction
The kinematic singularity is an inherent characteristic of a manipulator, in which bounded Cartesian speeds lead to infinite joint speeds, and bounded Cartesian accelerations lead to infinite end-effector forces. When a manipulator passes across a singular region in the work space, the manipulator may inevitably lead to a reduction in the stability and accuracy. Therefore, improving the tracking accuracy and the stability of the manipulator are important to the control of the manipulator during the singularity avoidance.
Many methods have been proposed to avoid the singularity: (a) methods based on the geometry, 1,2 which can intuitively reflect the configuration of singularities, but cannot obtain the mathematical expression of singularities; (b) methods based on task space control, [3][4][5][6][7]8 in which singularities are avoided by motion planning, but it is difficult to find the proper parameters; (c) methods based on optimization, [9][10][11] which take the singularity avoidance problem as an optimization problem, but are relatively timeconsuming; (d) methods based on the Jacobian matrix, [12][13][14] such as pseudoinverse method, 15 Jacobian transpose, 16,17 and damped least-squares inverse of the Jacobian matrix. 18 However, the pseudoinverse method is computationally time-consuming, and the Jacobian transpose and damped least-squares inverse are sensitive to the parameter selection as well as may cause large tracking errors.
A Jacobian transpose method based on the damped reciprocal 19 was proposed, which had less computation complexity, but the piecewise damped reciprocal function had some discontinuity. 20 Then the Gaussian distribution damped reciprocal algorithm 21 was introduced to improve the continuity of the singularity avoidance. After that the improved Gaussian distribution damped reciprocal algorithm was proposed to improve the tracking accuracy of the end-effector. 22 However, the accuracy of those methods based on the damped reciprocal still need to be improved.
In this article, we take a manipulator with the spherical wrist as the research object. Firstly, we adopted the singularity separation method to reduce the computation complexity. Secondly, we proposed a new mathematical function of the approximate damped reciprocal. This function has the following advantages: closer to the common reciprocal, which will have a better accuracy during the singularity avoidance, and a lower maximum of the derivative of the function, which will be helpful to reduce the accelerations of the joints. Thirdly, the singularities are avoided by modifying the Jacobian matrixes of the manipulator with the approximate damped reciprocal algorithm. The contribution of this article is that we propose a mathematical function of the damped reciprocal to improve the tracking accuracy of manipulator and ensure the stability of the system.
The article is organized as follows: The second section describes the kinematics model of the manipulator. The singularities of the manipulator are divided into forearm singularities and wrist singularity in the third section. The fourth section proposes the approximate damped reciprocal algorithm, and analyzes the tracking errors, the velocities, and accelerations of the joints. The numerical simulation experiments are carried out to validate the proposed algorithm in the fifth section. Lately, the discussions are presented in the sixth section and conclusions are made in the last section.

Kinematics modeling
In this article, a manipulator with the spherical wrist is chosen as the research object. The axes of the last three joints of this manipulator intersect at a point.
The kinematics Denavit-Hartenberg (D-H) model of the manipulator is shown in Figure 1. The corresponding Denavit-Hartenberg(D-H) model parameters are presented in Table 1.
In Figure 1, fTg denotes the tool center point (TCP) frame of the manipulator. As the tool directly contacts with the working object during the practical operation, the position and orientation of the TCP in the world frame are calculated as follows where 6 T T denotes the transformation matrix of the fTg frame relative to the end-effector fEg frame, which is determined by the physical dimension and the installation of the tool, and 0 6 T denotes the transformation matrix of the fEg frame relative to the world frame. Since 6 T T is a constant matrix, the position and orientation of the TCP would be calculated after the calculation of 0 6 T .

Singularity separation
We adopted the singularity separation algorithm of Xu. 19 Firstly, we divide the six-degree-of-freedom (DOF) singularity avoidance problem into two 3-DOF singularity avoidance subproblems. Secondly, we calculate the singular factors of the two 3-DOF singularity avoidance subproblems correspondingly. Without calculating the singular value decomposition (SVD), or estimating the minimum singularity value of the Jacobian matrix, this method has less computation burden and higher real-time performance.

Jacobian decomposition
The inverse Jacobian matrix of the manipulator is calculated to obtain the singular factors, and the computation complexity of calculating the inverse matrix of a sixdimensional (6D) matrix is oð6 6 Þ.
In order to reduce the computation complexity, we decompose the 6D Jacobian matrix into four 3D matrixes, displayed in equation (2), which is based on that the orientation of the manipulator is only relative to the last three joints, and the computation complexity of calculating the inverse matrix of a 3D matrix is oð3 3 Þ, which is less than where v and w are the linear and angular velocities of the end-effector in the world frame, respectively; _ q f is the vector of the velocities of the first three joints; and _ q l is the vector of the velocities of the last three joints.
According to equation (2), the velocities of joints can be calculated as follows which shows that, when the manipulator is in the forearm singular configuration, J 11 would be ill conditioned, and the velocities of the first three joints would not exist; when the manipulator is in the wrist singular configuration, J 22 would be ill conditioned, and the velocities of the last three joints would not exist.

Singular factors separation
Since the expressions of J 11 and J 22 in the world frame are complicated to separate the singular factors, we transform the Jacobian matrix into different joint frames to simplify the expressions of J 11 and J 22 . Due to the forearm singularities occurring in the first three joints, we calculate the Jacobian matrix in the third joint frame to simplify J 11 as follows where 0 3 R denotes the rotation matrix of the third joint frame relative to the world frame, and 3 J 11 denotes the Jacobian matrix of J 11 in the third joint frame. The formula of 3 J 11 in equation (5) is shown as follows where s i is the representative of sin q i ð Þ; c i is the representative of cos q i ð Þ; s ij is the representative of sin q i þ q j À Á ; and c ij is the representative of cos q i þ q j À Á . The inverse matrix of 3 J 11 is calculated as follows It is obvious that the wrist singularity occurs in the fifth joint, so we calculate the Jacobian matrix J 22 in the fifth joint frame as follows Then the inverse matrix of 5 J 22 is calculated as follows where k 3 ¼ s 5 denotes the wrist singular factor. Equations (7) and (9) show that, when k 1 ; k 2 ; k 3 ¼ 0, the manipulator would be in the singular configuration, and the solutions of equations (3) and (4) would be infinite, so it is necessary to study the singularity avoidance of the manipulator.

Singularity avoidance algorithm
To solve the problem that the velocities of the joints will be infinitely great when the singular factors are small, a damped reciprocal method was proposed, which used a function to simulate the reciprocal function of the singular factor. The property of the damped reciprocal function can be expressed as follows where l i is called the damped factor. When the absolute value of the singular factor k i is much larger than the damped factor l i , the value of the damped factor can be considered as zero. But when the absolute value of the singular factor k i is less than the damped factor l i , the denominator of the damped reciprocal can be considered as the damped factor squared, which would prevent the reciprocal value of the singular factor k i from being infinite.
In order to achieve this goal, several functions of the damped factor were proposed, such as (a) the piecewise damped factor, 19 as shown in equation (11), which may solve the problem of singularity avoidance, but piecewise damped factor function may be discontinuous 20 and the tracking accuracy of the end-effector was unsatisfactory; (b) the Gaussian distribution damped factor 21 shown in equation (12), improved the continuity of algorithm, but the accuracy was worse than the piecewise method; and (c) the improved Gaussian distribution damped factor 22 was shown in equation (13), which had the best accuracy among the three methods where the parameter e i is a threshold value, l 0 ¼ 0:3 and e 0 ¼ 0:04. The curves of different damped reciprocals are displayed in Figure 2, where the curve closer to the common reciprocal 1 k i , the method may have better performance in accuracy.
Inspired by this deduction, we propose a function of the approximate damped reciprocal to simulate the reciprocal of the singular factor, which is shown in equation (14) f where l 0 ¼ 0:3 and e 0 ¼ 1 90 are empirical values by a trial and error procedure. The curve of the approximate damped reciprocal is displayed in Figure 2, where the curve of our method is closer to the common reciprocal's, so that it would be helpful to have a better accuracy of the manipulator.
Then we modify the Jacobian matrix of the manipulator by displacing the common reciprocal of the damped factor with the approximate damped reciprocal to avoid the singularity.

The velocity analysis
According to equations (3), (4), (7), and (9), the formulas of the joint velocities based on the approximate damped reciprocal algorithm are as follows where 5 Dw¼ 5 wÀ 5 J 21 Á _ q f . According to equations (2), (6), (8), and (15), the formulas of the linear velocities and angular velocities based on the approximate damped reciprocal algorithm are as follows 3v Taken the space velocities of the path planning as references, the practical velocity errors of the end-effector are calculated as follows 3v where the singular factor k 1 only affects the velocity along the Z-axis in the third joint frame; the singular factor k 2 only affects the velocities along the X-axis and Y-axis in the third joint frame; and the singular factor k 3 only affects the angular velocity around the X-axis in the fifth joint frame. An indicator, called the error coefficient, is defined to measure the velocity error, which is displayed in equation (18). The curves of error coefficients of different methods are shown in Figure 3.
We can see from Figure 3 that the error coefficients are zero outside the singular region, while the maxima of the error coefficients are equal to one in the singular region. Moreover, our method has the least total error coefficients among the four methods.

The derivative analysis
The higher the joint acceleration, the greater the joint torque, the worse the stability of the manipulator, so it is necessary to analyze the joint accelerations of the manipulator. The relations between the joint accelerations € q i and the derivatives of the approximate damped reciprocal f 0 ðk i Þ are shown as follows which shows that the accelerations of joints are linear to the derivatives of the approximate damped reciprocals. The larger the derivatives of the approximate damped reciprocal, the accelerations of joints will be larger. The curves of the derivatives of different damped reciprocals are shown in Figure 4, which shows that the proposed method has a higher derivative maximum than the piecewise damped reciprocal method and the Gaussian distribution damped reciprocal method, but has a lower derivative maximum than the improved Gaussian distribution damped reciprocal method, which means that the proposed method would produces less accelerations and moments of joints than the improved Gaussian distribution damped reciprocal method, when the linear and angular accelerations of both methods are equal.

Simulation
In this section, the proposed singularity avoidance algorithm is applied to an industrial manipulator with the spherical wrist. Due to the limitations of the experimental environment, the simulation experiment is only carried out to prove the validity of the proposed algorithm. We have developed a simulation software based on Cþþ to simulate the motion of the manipulator. Our method can be validated on this software. The data analysis is done on the software of MATLAB (2019).

Numerical simulation introduction
Let fq x ; q y ; q z ; q w g, fp x ; p y ; p z g be the coordinates of the manipulator's TCP in the world frame. fq x ; q y ; q z ; q w g represents the TCP attitude, measured with quaternion, which can be displaced with transformation matrixes, or Euler angles, and so on and fp x ; p y ; p z g represents the position of the TCP with unit of meter. The positions we take for experiments are listed as follows:   We take the position of f0, À1.570796, 0.0, 0.0, 3.1415926, 0.0g, which denotes the angles of joints with unit of rad, as the original position to reach position A, and take the position of f0, À2.1423, À0.498749, 0.0, 2.641050, 0.0g, which denotes the angles of joints with unit of rad, as the original position to reach position B. Two linear paths are selected to do the simulation. The manipulator would enter the wrist singular region during completing the first path and would enter the forearm singular region during completing the second path.
The paths of manipulator in Cartesian space are generated by the path planning module, then the paths are transformed into the joints space through the trajectory interpolation module to control the motions of the joints. Our method would be applied on the trajectory interpolation module to avoid the singularities. Due to the singularity separation, our method has lower computational complexity. The period of trajectory interpolation is 4 ms, which would meet the real-time requirements of the system.
In fact, it is the tool that contacts with the working object directly, so it is necessary to consider the path accuracy of the TCP. As both the linear and angular velocity error can produce the position error of the TCP, we take the position of the TCP, calculated by equation (1), and the linear and angular velocity error of TCP, as indicators to compare with other methods. Finally, the stability of the system during the singularity avoidance is analyzed by verifying the velocity and acceleration of each joint.

Error simulation analysis
The linear paths of TCP of several methods are displayed in Figure 5, which shows that all four methods can finish the task of singularity avoidance, but the manipulator would deviate from the linear paths during passing across the singularities. Moreover the proposed method would approach the linear paths best among the four methods.
Taking v and w, the linear and angular velocities of TCP in the world frame of the path planning as references, we calculate the velocity errors of TCP as follows wherev;ŵ are the practical velocities of TCP in the world frame. The velocity errors of TCP in the world frame are shown in Figure 6, which shows that compared with the other three methods, the proposed method has the least angular velocity error around the singular region.

Velocity and acceleration simulation analysis
High velocities and accelerations of joints would result in high force moments, which may have bad influence in system stability, so the velocity and acceleration analyses of different methods are important. The curves of the velocities and accelerations of joints are displayed in Figures 7 and 8. Since the wrist singularity is located on the first path, only the velocities and accelerations of the last three joints are shown in Figure 7; while the forearm singularity is located on the second path, only the velocities and accelerations of the first three joints are shown in Figure 8. Due to the small values of the accelerations of the second and the third joints, we approximate them to zero. We can see from Figures 7 and 8 that the proposed method can control the velocities and accelerations of joints within the allowable range of the manipulator. Moreover, the proposed method has lower accelerations than the Gaussian distribution damped reciprocal method in Figure 7.

Discussion
The singularities of a 6R manipulator are avoided by the approximate damped reciprocal method in this article. All the accuracy, velocity, and acceleration simulation results prove that the proposed method can improve the path accuracy of the manipulator with limited velocities and  Figure 8 is that the accelerations of the joints are relative to the linear, angular accelerations of the path planning and the derivatives of the damped reciprocals (equation (19)). The path of our method is closer to the linear path in the singular region, which results in that our method would have higher linear and angular accelerations than other methods. However, due to the constraint of the derivative of the approximate damped reciprocal, the accelerations of the joints can be controlled in a limited range. As the singular factors are obtained by the singularity separation method based on the structure of the spherical wrist, which may limit the application of the proposed method, the method to obtain the singular factors from the Jacobian matrix of manipulators with nonspherical wrists will be presented in an upcoming paper. The singularity avoidance method would be applied on a 6-DOF manipulator with a spherical wrist, which can operate in 4500 km underwater environment.

Conclusion
In this article, the analysis and avoidance of the kinematics singularities of manipulators with spherical wrists are studied using the approximate damped reciprocal algorithm. Firstly, the 6-DOF singularity problem is separated into two 3-DOF singularity subproblems and the singular factors are calculated correspondingly. Secondly, a new mathematical function of the approximate damped reciprocal of the singular factor is proposed to improve the accuracy of the manipulator. Finally, the singularities are avoided by modifying the Jacobian matrixes of the manipulator with the approximate damped reciprocal algorithm. Without calculating the SVD or estimating the minimum singular value of the Jacobian matrix, the proposed algorithm may have better real-time performance. The simulation results prove that the proposed method may have better accuracy than the traditional damped reciprocal methods and can make the joint's velocities and accelerations within the allowable range of the manipulator, which means that our method may have better accuracy and can ensure the stability of the manipulator around the singular region.

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.