An Improved Weighted Gradient Projection Method for Inverse Kinematics of Redundant Surgical Manipulators

Abstract

Different from traditional redundant manipulators, the redundant manipulators used in the surgical environment require the end effector (EE) to have high pose (position and orientation) accuracy to ensure the smooth progress of the operation. When analyzing the inverse kinematics (IK) of traditional redundant manipulators, gradient-projection method (GPM) and weighted least-norm (WLN) method are commonly used methods to avoid joint position limits. However, for the traditional GPM and WLN method, when joints are close to their limits, they stop moving, which greatly reduces the accuracy of the IK solution. When robotic manipulators enter a singular region, although traditional damped least-squares (DLS) algorithms are used to handle singularities effectively, motion errors of the EE will be introduced. Furthermore, selecting singular region through trial and error may cause some joint velocities exceed their corresponding limits. More importantly, traditional DLS algorithms cannot guide robotic manipulators away from singular regions. Inspired by the merits of GPM, WLN, and DLS methods, an improved weighted gradient projection method (IWGPM) is proposed to solve the IK problem of redundant manipulators used in the surgical environment with avoiding joint position limits and singularities. The weighted matrix of the WLN method and the damping factor of the DLS algorithm have been improved, and a joint limit repulsive potential field function and singular repulsive potential field function belong to the null space are introduced to completely keep joints away from the damping interval and redundant manipulators away from the unsafe region. To verify the validity of the proposed IWGPM, simulations on a 7 degree of freedom (DOF) redundant manipulator used in laparoscopic surgery indicate that the proposed method can not only achieve higher accuracy IK solution but also avoid joint position limits and singularities effectively by comparing them with the results of the traditional GPM and WLN method, respectively. Furthermore, based on the proposed IWGPM, simulation tests in two cases show that joint position limits have a great impact on the orientation accuracy, and singular potential energy function has a great impact on the position accuracy.

1. Introduction

Compared with six degree of freedom (DOF) robotic manipulators, 7-DOF robotic manipulators not only ensure motion accuracy of the end-effector (EE), but also optimize other objectives due to the existence of a redundant DOF, such as avoiding obstacles and adapting to human action [1,2,3,4,5,6]. Therefore, redundant manipulators have been widely used in the medical and aerospace fields, etc.

However, 7-DOF manipulators not only improve the flexibility, but also increase the difficulty of solving the inverse kinematics (IK) solution [7]. Indeed, achieving the 6-DOF pose (position and orientation) given by the EE is equivalent to six equations and seven unknowns, and there are innumerable solutions in theory. However, only one set of the IK solution is needed in robot kinematics control. So far, many scholars have studied the IK solution of redundant manipulators. These methods include geometric method [8], gradient-projection method (GPM) [3,9], weighted least-norm (WLN) method [10], extended Jacobian matrix method [11], and analytical method [12], etc. Some artificial intelligence algorithms are also used in the IK solution of redundant manipulators, such as genetic algorithm [13], particle swarm optimization algorithm [14], neural network algorithm [15].

Geometric algorithms are not universal and their modeling and solving process is complex. The extended Jacobian matrix method can only obtain an approximate IK solution, which is not suitable for robotic manipulators with high accuracy. Analytical algorithm is only applicable to robotic manipulators with specific configurations, or robotic manipulators with some simplified processing [16,17]. It takes a long time for intelligent algorithms to solve IK, which is not conducive to real-time control.

The GPM and WLN methods are popular algorithms for solving IK of redundant manipulators, and have the effect of avoiding joint position limits. A weighted GPM [18] and a clamping WLN method [19] were proposed for IK of a 7-DOF manipulator. Due to the introduction of damping factors in the singular region, the position errors of the EE are as high as 96.3 mm and 25 mm, respectively, which are not suitable for robotic manipulators with high accuracy requirements. Hu [20] presented a gradient projection of a weighted Jacobian matrix method for IK of a planar 3-DOF manipulator. However, this method did not solve the problem of motion accuracy reduction caused by joint positions near their limits. Kelemen [21] introduced an IK algorithm to avoid joint position limits, singularities and obstacles. However, the position error of the EE is as high as 18 mm, and the orientation error is not analyzed. This is not conducive to the further operation of the EE. Jun [22] proposed an improved clamping WLN method for IK of a redundant manipulator. However, the constant value of the repulsive potential field would lead to the disadvantage of too large or too small repulsive force, which affected the motion accuracy of the EE.

When robotic manipulators move to a Jacobian singular configuration, the Jacobian inverse matrix becomes numerically unrealizable, which is not conducive to the motion control. In [23,24,25], a damped least-squares (DLS) algorithm was used to handle singularities. Although the introduction of damping factors can solve the shortcomings of singular configurations, the damping factors will lead to the decline of the motion accuracy of the EE.

The existing GPM and WLN methods do not completely keep redundant manipulators away from joint position limits and singular configurations, which can reduce the motion accuracy of the EE. Different from traditional redundant manipulator, the redundant manipulators used in the surgical environment require the EE to have high pose accuracy to ensure the smooth progress of the operation. Therefore, based on the advantages of GPM and WLN method, combined with the traditional DLS algorithm, an improved weighted GPM (IWGPM) which introduces a joint limit repulsive potential field function and singular repulsive potential field function as subtasks is proposed. A clamping weighted matrix can make joints stop at their limits, and the introduced joint limit repulsive potential field function generates virtual forces at the damping interval to make joints return to the flexible interval. In the unsafe region, virtual forces generated by the singular repulsive potential field function can make redundant manipulators move away from unsafe region and solve the singular problem thoroughly.

The remainder of this paper is organized as follows: The kinematics of redundant manipulators and the traditional GPM, WLN method, and DLS algorithm are briefly reviewed in Section 2. The IWGPM is proposed to solve the IK problem of redundant surgical manipulators with avoiding joint position limits and singularities in Section 3. In Section 4, to verify and evaluate the effectiveness of the proposed method, simulation results and discussion are performed. Finally, the conclusions are presented in Section 5.

2. Kinematics Formulations

The forward kinematics is a nonlinear function that describes the relation between the pose of the EE $x\in R^m$ and joint position $q\in R^n$ as follows:

$$x=f\left(q\right)$$

(1)

The first-order derivation of the function:

$$\dot{x}=J\dot{q}$$

(2)

describes the velocities from the joint to the EE, where $\dot{x}\in R^m$ represents the velocity vector of the EE, $\dot{q}\in R^n$ is the velocity vector of joints, and $J\in R^{m\times n}$ is a Jacobian matrix.

To obtain a unique solution $\dot{q}$, the Jacobian inverse $J^{-1}$ or pseudo inverse $J^{+}$ for non-redundant or redundant manipulators, respectively, is used, as follows [26]:

$$\dot{q}=J^{-1}\dot{x}$$

(3)

for non-redundant manipulators (m = n) and:

$$\dot{q}=J^{+}\dot{x}=J^{\mathrm{T}}{\left({\mathit{JJ}}^{\mathrm{T}}\right)}^{-1}\dot{x}$$

(4)

for redundant manipulators (m < n).

It is not difficult to conclude that Equations (3) and (4) hold when $J^{-1}$ and $J^{+}$ exists, and Equation (4) provides a least-norm solution. When robotic manipulators approach a singular configuration, $J^{-1}$ or $J^{+}$ of the manipulators becomes numerically ill conditioned. Furthermore, the vicinity of singular configurations may also cause joint velocities to exceed the corresponding limits. To overcome this drawback, a DLS algorithm [23,25] is a widely used approach that sacrifices accuracy of the IK solution to generate an improved Jacobian matrix in the singular region. The DLS algorithm is expressed as:

$$J^{\mathrm{T}}\dot{x}=(J^{\mathrm{T}}J+{\lambda }^2{I}_m)\dot{q}$$

(5)

where $I_m\in R^n$ is an identity matrix, and $\lambda$ is a damping factor.

For redundant manipulators, the self-motion can be realized with some subtasks, with not affecting the main task, which can be expressed by GPM [3]:

$$\dot{q}=J^{+}\dot{x}+(I_n-J^{+}J)z$$

(6)

where $I_n-J^{+}J$ is the projection operator onto the null space of the matrix $J$, and z is a gradient vector of performance criterion as a subtask. The $(I_n-J^{+}J)z$ is the homogeneous solution for self-motion.

Avoidance for joint position limits as a subtask were presented as:

$$H(q)=\frac{1}{n}\sum _{i=1}^n{\left(\frac{2q_i-q_{\mathit{imax}}-q_{\mathit{imin}}}{q_{\mathit{imax}}-q_{\mathit{imin}}}\right)}^2$$

(7)

where $q_{\mathit{imin}}$ and $q_{\mathit{imax}}$ are the lower and upper limits of the i-th joint position, and $q_i$ is the i-th joint position.

As a result, replace $z$ with $\nabla H\left(q\right)$, Equation (6) can be derived further as follows:

$$\dot{q}=J^{+}\dot{x}+k(I_n-J^{+}J)\nabla H\left(q\right)$$

(8)

where $k$ is a positive scalar coefficient to make the gradient rate of $H\left(q\right)$ to be minimized, and $\nabla H\left(q\right)$ is the gradient vector of $H\left(q\right)$, which is described as:

$$\nabla H\left(q\right)=\left[\begin{array}{cccc}\frac{\partial H\left(q\right)}{\partial q_1}& \frac{\partial H\left(q\right)}{\partial q_2}& \cdots & \frac{\partial H\left(q\right)}{\partial q_n}\end{array}\right]$$

(9)

The WLN method [10] is also used to avoid joint position limits through weighted factors, and the transformations are introduced as follows:

$$J_w=J{W}^{-\frac{1}{2}}$$

(10)

$${\dot{q}}_w=W^{\frac{1}{2}}\dot{q}$$

(11)

$$\dot{q}=W^{\frac{1}{2}}{J}_w^{+}\dot{x}=W^{-1}{J}^{\mathrm{T}}{\left({\mathit{JW}}^{-1}{J}^{\mathrm{T}}\right)}^{-1}\dot{x}$$

(12)

where $J_w$ and ${\dot{q}}_w$ are a weighted Jacobian matrix and a weighted joint velocity, respectively. The $W\in R^{n\times n}$ is a diagonal and positive weighted matrix, and the i-th diagonal element is expressed as:

$$w_i(q_i)=\{\begin{array}{c}1+\left|\frac{\partial H^{\ast }\left(q\right)}{\partial q_i}\right|\begin{array}{cc}& \Delta \left|\frac{\partial H^{\ast }\left(q\right)}{\partial q_i}\right|\ge 0\end{array}\\ 1\begin{array}{cc}& \Delta \left|\frac{\partial H^{\ast }\left(q\right)}{\partial q_i}\right|<0\end{array}\end{array}$$

(13)

where $\Delta \left|\frac{\partial H^{\ast }\left(q\right)}{\partial q_i}\right|$ is the change rate of $\frac{\partial H^{\ast }\left(q\right)}{\partial q_i}$, and $H^{\ast }\left(q\right)$ is defined as follows:

$$H^{\ast }(q)=\sum _{i=1}^n\frac{{(q_{\mathit{imax}}-q_{\mathit{imin}})}^2}{4(q_{\mathit{imax}}-q_i\left)\right(q_i-q_{\mathit{imin}})}$$

(14)

where $q_{\mathrm{imin}}$ and $q_{\mathrm{imax}}$ are the lower and upper limits of the i-th joint position, and q_i is the i-th joint position.

Figure 1 shows the distributions of $w_i\left(q\right)$ with $q_i$. When $q_i$ is close to $q_{\mathit{imin}}$ and $q_{\mathit{imax}}$, $\left|\frac{\partial H^{\ast }\left(q\right)}{\partial q_i}\right|\to \infty$, this is $w_i\left(q\right)\to \infty$. From Equation (12), $q_i$ tends to zero, the corresponding joint stops at its limit position, but it is not far away from its limit.

Both GPM and WLN method can deal with singular problems, therefore, combining Equations (5) and (8) can be redefined by:

$$\dot{q}=J_{\mathrm{G}}^{+}\dot{x}+k\left(I_n-J_{\mathrm{G}}^{+}J\right)\nabla H^{\ast }\left(q\right)$$

(15)

$$J_{\mathrm{G}}^{+}=J^{\mathrm{T}}{\left({\mathit{JJ}}^{\mathrm{T}}+{\lambda }^2{I}_m\right)}^{-1}$$

(16)

Equation (12) can be rewritten as:

$$\dot{q}=W^{\frac{1}{2}}{J}_w^{+}\dot{x}=W^{-1}{J}^{\mathrm{T}}{({\mathit{JW}}^{-1}{J}^{\mathrm{T}}+{\lambda }^2{I}_m)}^{-1}\dot{x}$$

(17)

3. Improved Weighted Gradient Projection Method

The WLN method is much more efficient than the GPM one in avoiding joint position limits [20]. Due to the weighted matrix $W$, some joints stop at their limits, but this does not keep them away from their limits. Furthermore, on solving singular problems, DLS algorithms can prevent the problem of joint velocities exceeding the corresponding limits, but motion errors will be introduced. More importantly, DLS algorithms cannot guide redundant manipulators away from singular region. These conditions occur suddenly during the operation, which is very unfavorable to patients. Therefore, based on GPM, a joint limit repulsive potential field function and a singular repulsive potential field function are introduced in the null space to solve the shortages of GPM, WLN, and DLS methods.

3.1. Clamping Weighted Matrix and Joint Limit Repulsive Potential Field Function

From Figure 1, it is easy to find that the starting and ending positions of $W$ are difficult to determine, which sometimes affects the validity of the weighted matrix. Therefore, a buffer is added before the joint position limits, and the whole joint motion interval is divided into damping interval and flexible interval, as shown in Figure 2. That is a progressive clamping weighted matrix designed as follows:

$$W_c=\mathrm{diag}(w_c(q_i\left)\right)$$

(18)

$$w_c(q_i)=\{\begin{array}{cc}f(\frac{q_{\mathit{itmin}}-q_i}{q_{\mathit{itmin}}-q_{\mathit{imin}}})& q_{\mathit{imin}}\le q_i\le q_{\mathit{itmin}}\\ 1& q_{\mathit{itmin}}<q_i<q_{\mathit{itmax}}\\ f(\frac{q_i-q_{\mathit{itmax}}}{q_{\mathit{imax}}-q_{\mathit{itmax}}})& q_{\mathit{itmax}}\le q_i\le q_{\mathit{imax}}\end{array}$$

(19)

where $f(·)$ is a smooth function varying from 0 to 1. The $q_{\mathit{itmin}}$ and $q_{\mathit{itmax}}$ are the lower and upper damping thresholds before $q_{\mathit{imin}}$ and $q_{\mathit{imax}}$, respectively:

$$f\left(x\right)=(-2x^3+3x^2{)}^2$$

(20)

$$q_{\mathit{itmin}}=q_{\mathit{imin}}+\xi (q_{\mathit{imax}}-q_{\mathit{imin}})$$

(21)

$$q_{\mathit{itmax}}=q_{\mathit{imax}}-\xi (q_{\mathit{imax}}-q_{\mathit{imin}})$$

(22)

where $\xi$ is a positive constant scalar to determine the width of damping interval.

Then, Equation (17) can be rewritten as:

$$\dot{q}=W_c{J}^{\mathrm{T}}{({\mathit{JW}}_c{J}^{\mathrm{T}}+{\lambda }^2{I}_m)}^{-1}\dot{x}$$

(23)

When $q_i$ run in the flexible interval, $w_c\left(q_i\right)=1$. While $q_i$ runs in the damping interval, the weighting matrix $w_c\left(q_i\right)$ prevents $q_i$ from approaching its limit. When $w_c\left(q_i\right)\to 0$, the corresponding joints stop moving forever, which will affect motion control and reduce pose accuracy of the EE.

A joint limit repulsive potential field function $(I_n-W_c)R_i\left(q\right)$ is introduced to solve the drawback of Equation (23), and $I_n-W_c$ is also a smooth weighted matrix, when $q_i$ runs in the flexible interval, the joint limit repulsive potential field function is invalid. On the contrary, when $q_i$ starts to enter the damping interval, the joint limit repulsive potential field function $R_i\left(q)=\mathrm{diag}(r\right(q_i\left)\right)$ comes into play to force the corresponding joints back the flexible interval as far as possible. The $r\left(q_i\right)$ is defined as:

$$r(q_i)=\{\begin{array}{cc}\frac{q_i-q_{\mathit{itmin}}}{q_{\mathit{itmin}}-q_{\mathit{imin}}}{r}_{\mathit{max}}& q_{\mathit{imin}}\le q_i\le q_{\mathit{itmin}}\\ 0& q_{\mathit{itmin}}<q_i<q_{\mathit{itmax}}\\ \frac{q_i-q_{\mathit{itmax}}}{q_{\mathit{imax}}-q_{\mathit{itmax}}}{r}_{\mathit{max}}& q_{\mathit{itmax}}\le q_i\le q_{\mathit{imax}}\end{array}$$

(24)

where $r_{\mathit{max}}$ is the maximum potential field force, the closer the limit is, the bigger the potential field force. When $q_i$ goes towards $q_{\mathit{imin}}$, $r\left(q_i\right)$ is negative, and $r\left(q_i\right)$ is positive when $q_i$ goes towards $q_{\mathit{imax}}$.

Considering the joint limit repulsive potential field function and combining with GPM, Equation (23) becomes:

$$\begin{array}{l}\dot{q}=W_c{J}^{\mathrm{T}}{(J{W}_c{J}^{\mathrm{T}}+{\lambda }^2{I}_m)}^{-1}\dot{x}\\ -(I_n-W_c{J}^{\mathrm{T}}{(J{W}_c{J}^{\mathrm{T}}+{\lambda }^2{I}_m)}^{-1}J)(I_n-W_c)R_i\left(q\right)\end{array}$$

(25)

where the second part on the right-hand side is a homogeneous solution, which does not affect the main task.

3.2. A Novel DLS Method and Singular Repulsive Potential Field Function

In the traditional DLS algorithm, the damping factor function [27] is:

$${\lambda }_{\mathit{traditional}}^2=\{\begin{array}{ll}{\lambda }_{\mathit{max}}^2\left(1-{\left(\frac{\sigma }{{\sigma }_{\mathrm{b}}}\right)}^2\right)& 0\le \sigma \le {\sigma }_{\mathrm{b}}\\ 0& \sigma >{\sigma }_{\mathrm{b}}\end{array}$$

(26)

The $\epsilon \in [0,{\sigma }_{\mathrm{b}}]$ is the singular region, and the selection of ${\sigma }_{\mathrm{b}}$ through trial and error may result in joint velocities exceeding the corresponding limits, so a novel damping function which includes a micro-buffer region $\overline{\epsilon }\in \begin{array}{cc}({\sigma }_{\mathrm{b}},& {\overline{\sigma }}_{\mathrm{b}}\end{array}]$ is presented to avoid the joint maximum velocities being exceeded owing to the small selection of ${\sigma }_{\mathrm{b}}$. The novel damping function is defined as:

$${\lambda }_{\mathit{noval}}^2=\{\begin{array}{ll}{\lambda }_{\mathit{max}}^2\left(1-0.874·{\left(\frac{\sigma }{{\sigma }_{\mathrm{b}}}\right)}^2\right)& 0\le \sigma \le {\sigma }_{\mathrm{b}}\\ {\lambda }_{\mathit{max}}^2\left(0.5+0.5\cos \left(\frac{\mathsf{\pi }\sigma }{{\overline{\sigma }}_{\mathrm{b}}}\right)\right)& {\sigma }_{\mathrm{b}}<\sigma \le {\overline{\sigma }}_{\mathrm{b}}\\ 0& \sigma >{\overline{\sigma }}_{\mathrm{b}}\end{array}$$

(27)

$${\overline{\sigma }}_{\mathrm{b}}=\gamma {\sigma }_{\mathrm{b}}$$

(28)

where $\gamma$ is a positive constant scalar to determine the width of micro-buffer region. In this paper, the single region and micro-buffer region are called unsafe region.

The distributions of ${\lambda }^2$ and damped inverse $\frac{\sigma }{{\sigma }^2+{\lambda }^2}$ with singular value is depicted in Figure 3a,b, respectively. Through Figure 3b, it can be seen that in the case of no damping, infinite damping inverse will occur at a singular configuration which will result in very high joint velocities. Figure 3a,b show that the fixed damping value has the same effect in the unsafe region, which is unfair to the micro-buffer region, while the traditional damping function has an effect only in the singular region. However, except for the singular region, the new damping function still has a micro-damping factor in the micro-buffer to prevent the disadvantage that the joint speed exceeds the limit due to the small selection of ${\sigma }_{\mathrm{b}}$. Furthermore, the damping continuity in the singular and micro-buffer regions ensures the continuity of joint velocities.

When solving singular problems, traditional DLS algorithms generate motion errors due to the introduction of damping factor in unsafe region, and it cannot guide robotic manipulators away from unsafe region. It is suitable for the manipulator with low accuracy requirements. However, this paper studies a surgical manipulator, which has very high requirements for pose. Therefore, a singular repulsive potential field function $F\left(q\right)$ [28] is introduced to address the two deficiencies:

$$F(q)=\mathrm{K}\cdot \left[\frac{J\cdot \nabla d(q)}{\Vert \left|J\cdot \nabla d(q)\right|\Vert }\right]\cdot f(d\left(\sigma \right)|{\sigma }_{\mathrm{b}},{\overline{\sigma }}_{\mathrm{b}})$$

(29)

$$f(d(\sigma )|{\sigma }_{\mathrm{b}},{\overline{\sigma }}_{\mathrm{b}})=\{\begin{array}{ll}1& d(\sigma )<{\sigma }_{\mathrm{b}}\\ \frac{1}{1+e^{\delta \cdot (d(\sigma )-\frac{{\sigma }_{\mathrm{b}}+{\overline{\sigma }}_{\mathrm{b}}}{2})}}& {\sigma }_{\mathrm{b}}\le d(\sigma )\le {\overline{\sigma }}_{\mathrm{b}}\\ 0& d\left(\sigma \right)>{\overline{\sigma }}_{\mathrm{b}}\end{array}$$

(30)

where $\mathrm{K}$ is a diagonal matrix that contains the maximum torque or force that we allow to be applied to the operator in each spatial dimension. The matrix $\nabla d\left(q\right)$ is used to direct the wrench away from unsafe region. The $f\left(d\left(\sigma \right)\right|{\sigma }_{\mathrm{b}},{\overline{\sigma }}_{\mathrm{b}})$ is given as [29], and $d\left(\sigma \right)$ is the distance to ${\sigma }_{\mathit{min}}=0$, the $\delta =12/({\overline{\sigma }}_{\mathrm{b}}-{\sigma }_{\mathrm{b}})$ is recommended. It can be observed from Figure 4 that the change of $f\left(d\right(\sigma \left)\right)$ in [$\begin{array}{cc}0,& {\overline{\sigma }}_{\mathrm{b}}\end{array}$] is smooth and continuous.

3.3. Resolution IWGPM

Considering 3.1 and 3.2, and combining with GPM, a novel method called IWGPM is defined as:

$$\begin{array}{l}\dot{q}=W_c{J}^{\mathrm{T}}{(J{W}_c{J}^{\mathrm{T}}+{\lambda }_{\mathit{noval}}^2{I}_m)}^{-1}\dot{x}\left(q\right)\\ -(I_n-W_c{J}^{\mathrm{T}}{(J{W}_c{J}^{\mathrm{T}}+{\lambda }_{\mathit{noval}}^2{I}_m)}^{-1}J)(I_n-W_c)R_i\left(q\right)\\ -(I_n-W_c{J}^{\mathrm{T}}{({\mathit{JW}}_c{J}^{\mathrm{T}}+{\lambda }_{\mathit{noval}}^2{I}_m)}^{-1}J)F\left(q\right)\end{array}$$

(31)

We define $J_c^{+}=W_c{J}^{\mathrm{T}}{({\mathit{JW}}_c{J}^{\mathrm{T}}+{\lambda }_{\mathit{noval}}^2{I}_m)}^{-1}$, the Equation (30) is represented as:

$$\dot{q}=J_c^{+}\dot{x}\left(q\right)-(I_n-J_c^{+}J\left)\right(I_n-W_c)R_i\left(q\right)-(I_n-J_c^{+}J)F\left(q\right)$$

(32)

In Equation (31), when $W_c=I_n$, Equation (25) becomes Equation (23). On the contrary, when $q_i$ is in the damping interval, $w_c\left(q_i\right)$ decreases gradually from 1 to 0, and the corresponding joint velocity also gradually decreases to 0. Meanwhile, $r\left(q_i\right)$ increases gradually from 0 to make $q_i$ return to flexible interval. In the safe region, all elements in $F\left(q\right)$ are zero, indicating that the singular repulsive potential field function does not work. When redundant manipulators are gradually entering the micro-buffer region from the safe region, some elements that cause singularities in $F\left(q\right)$ increase gradually from 0 to $F_{\mathit{max}}$. In the singular region, the function $F\left(q\right)$ is always $F_{\mathit{max}}$. The function $F\left(q\right)$ has pushed redundant manipulators away from the unsafe region. From Equation (32), it is found that $(I_n-J_c^{+}J\left)\right(I_n-W_c)R_i\left(q\right)$ and $(I_n-J_c^{+}J)F\left(q\right)$ pick from the null space of $J_c$ do not generate any motion at the EE.

4. Simulation Results and Discussion

In this section, simulations in the paper are implemented with the aid of the MATLAB R2015a tool, and a computer equipped with an Intel Core™ i5-2450M CPU @ 2.50 GHz and 2 GB RAM as the control platform.

A path starts from an initial pose of the EE, $x_{\mathit{initial}}$, to a desired pose, $x_{\mathit{desired}}$. The path is divided into M smaller line segments for numerical integration. At any integration step, the joint rates required to move the EE are calculated. Joint velocities are integrated sequentially until the EE reaches the desired pose. Equations (8), (23), (25) and (32) can be realized by an iterative algorithm and further applied to IK solution of redundant manipulators. The iterative algorithm steps are as follows:

Step (1): Assume an initial pose expressed as Euler angles of the EE, $x_{\mathit{initial}}\in R^{6\times 1}$.

Step (2): Plan a trajectory from $x_{\mathit{initial}}$ to $x_{M+1}=x_{\mathit{desired}}\in R^{6\times 1}$ with M intervals, and assume the running time of the motion, T.

Step (3): Calculate a planned velocity at the interval k that moves the EE toward $x_{\mathit{desired}}$ as

$$\dot{x}(k)=\beta \frac{(x_{\mathit{desired}}-x(k\left)\right)M}{\left(M+1-k\right)T}$$

(33)

where $\beta >1$ is a deceleration factor.

Step (4): Compute general equation $\dot{q}\left(k)=J^{+}\right(q\left(k\left)\right)\dot{x}\right(k)$. Equations (8), (23), (25) and (32) can be substituted the general equation.

Step (5): Calculate $q(k+1)$ through the following equation:

$$q(k+1)=q(k)+\dot{q}(k)\frac{T}{M}$$

(34)

Step (6): Through Equation (1), the new pose of the EE is obtained.

$$x\left(k+1)=f\right(q(k+1\left)\right)$$

(35)

Step (7): Letting $q\left(k)=q\right(k+1)$

Step (8): If $x_{\mathit{desired}}-x\left(k\right)\le \rho$, where $\rho$ is the accuracy of the actual pose and desired pose of the EE specified by the user.

Return;

Else;

Repeat steps 3 to 6 for $k=1\cdots M$.

End.

To make the actual pose of the EE as close to the desired pose as possible, the closed-loop algorithm is used in the IK solution process [30]. Therefore, Equations (8), (23) and (32) can be represented as:

GPM:

$$\dot{q}\left(k\right)=J^{+}\left(k\left)\right(\dot{x}\right(k)+\mathsf{\Lambda }{E}_e)+k(I_n-J^{+}\left(k\right)J(k\left)\right)\nabla H(q(k\left)\right)$$

(36)

WLN:

$$\dot{q}\left(k\right)=W_c\left(k\right)J^{\mathrm{T}}\left(k\left)\right(J\right(k\left)W_c\right(k\left)J^{\mathrm{T}}\right(k)+{\lambda }^2{I}_m{)}^{-1}(\dot{x}\left(k\right)+\mathsf{\Lambda }{E}_e)$$

(37)

IWGPM:

$$\begin{array}{l}\dot{q}\left(k\right)=J_c^{+}\left(k\left)\right(\dot{x}\right(k)+\mathsf{\Lambda }{E}_e)-(I_n-J_c^{+}(k\left)J\right(k\left)\right)(I_n-W_c\left(k\left)\right)R_i\right(q(k\left)\right)\\ -(I_n-J_c^{+}(k\left)J\right(k\left)\right)F\left(q\right(k\left)\right)\end{array}$$

(38)

where $\mathsf{\Lambda }$ is positive feedback gain, The $E_e\in R^{6\times 1}$ is tracking error between the desired pose and the actual pose of the EE, which is defined as follows:

$$E_e={[\begin{array}{cccccc}{E}_x,& E_y,& E_z,& E_{\varphi },& E_{\theta },& E_{\psi }\end{array}]}^{\mathrm{T}}$$

(39)

where $E_x=p_{\mathit{xdesired}}-p_{\mathit{xactual}}$, $E_y=p_{\mathit{ydesired}}-p_{\mathit{yactual}}$, $E_z=p_{\mathit{zdesired}}-p_{\mathit{zactual}}$, $E_{\varphi }={\varphi }_{\mathit{desired}}-{\varphi }_{\mathit{actual}}$, $E_{\theta }={\theta }_{\mathit{desired}}-{\theta }_{\mathit{actual}}$, $E_{\psi }={\psi }_{\mathit{desired}}-{\psi }_{\mathit{actual}}$. The ($p_{\mathit{xdesired}}$, $p_{\mathit{ydesired}}$, $p_{\mathit{zdesired}}$) and ($p_{\mathit{xactual}}$, $p_{\mathit{yactual}}$, $p_{\mathit{zactual}}$) are desired position and actual position, respectively. The (${\varphi }_{\mathit{desired}}$, ${\theta }_{\mathit{desired}}$, ${\psi }_{\mathit{desired}}$) and (${\varphi }_{\mathit{actual}}$, ${\theta }_{\mathit{actual}}$, ${\psi }_{\mathit{actual}}$) are desired orientation and actual orientation expressed as Euler angles of the EE, respectively.

4.1. Case Study 1

Simulations are provided to demonstrate the validity and practicability of the proposed IWGPM method for IK solutions of the redundant manipulators. The self-developed 7-DOF surgical manipulator with a diameter of 10 mm is taken as the research object. Its structure diagram and coordinate system are shown in Figure 5a,b.

The DH parameters of manipulator with joint position limit range are shown in

Table 1. Modified DH parameters and joint position limits of the 7-DOF manipulator.

i	${\mathbf{\alpha }}_{\mathit{i}-1}\left(\mathbf{rad}\right)$	${\mathit{a}}_{\mathit{i}-1}\left(\mathbf{mm}\right)$	${\mathit{d}}_{\mathit{i}}\left(\mathbf{mm}\right)$	${\mathit{q}}_{\mathit{i}}\left(\mathbf{rad}\right)$	$[{\mathit{q}}_{\mathit{imin}},{\mathit{q}}_{\mathit{imax}}]$
1	0	0	$d_1$	0	[−100,100] (mm)
2	$\mathsf{\pi }/2$	20	0	$q_2$	$[0,\mathsf{\pi }$] (rad)
3	0	68	0	$q_3$	$[-\mathsf{\pi }/2,\mathsf{\pi }/2$] (rad)
4	$\mathsf{\pi }/2$	32	0	$q_4$	$[0,\mathsf{\pi }$] (rad)
5	$\mathsf{\pi }/2$	0	86	$q_5$	$[-\mathsf{\pi },\mathsf{\pi }$] (rad)
6	$\mathsf{\pi }/2$	0	0	$q_6$	$[0,\mathsf{\pi }$] (rad)
7	$\mathsf{\pi }/2$	20	0	$q_7$	$[-\mathsf{\pi }/2,\mathsf{\pi }/2$] (rad)

.

To illustrate the effect of the joint limit repulsive potential field function and singular repulsive potential field function, a straight-line path is selected as the trajectory. Moreover, the manipulator will enter a singular region, and some joints will be near their limit positions. The initial joint position $q_{\mathit{initial}}=[44,\frac{\mathsf{\pi }}{3},\frac{\mathsf{\pi }}{6},\frac{\mathsf{\pi }}{10},-1.4349,\frac{\mathsf{\pi }}{4},\frac{\mathsf{\pi }}{3}{]}^{\mathrm{T}}$ and final joint position $q_{\mathit{desired}}=[50,\frac{\mathsf{\pi }}{5},\frac{\mathsf{\pi }}{3},\frac{\mathsf{\pi }}{6},\frac{\mathsf{\pi }}{4},\frac{\mathsf{\pi }}{3},\frac{\mathsf{\pi }}{6}{]}^{\mathrm{T}}$ of the manipulator is set (Note: The final joint position is used to calculate the desired pose of EE, which is to clarify that the joint position is different from that obtained by the proposed IWGPM.), respectively, which determines the initial pose and desired pose expressed as Euler angles of the EE:

$$p_{\mathit{initial}}={[39.9883,117.4741,175.0739]}^{\mathrm{T}}\left(\mathrm{mm}\right)$$

$$o_{\mathit{initial}}={[0.7865,1.4431,-0.8411]}^{\mathrm{T}}\left(\mathrm{rad}\right)$$

and the desired pose is:

$$p_{\mathit{desired}}={[71.4062,106.7273,191.9349]}^{\mathrm{T}}\left(\mathrm{mm}\right)$$

$$o_{\mathit{desired}}={[-0.9057,1.2209,-0.0824]}^{\mathrm{T}}\left(\mathrm{rad}\right)$$

For comparison, simulations with traditional GPM and WLN method are also presented. To reflect the coincidence degree of IK solution $x$ and $x_{\mathit{desired}}$, the pose accuracy is defined as follows:

$$E=x_{\mathit{desired}}-x_{\mathit{final}}={[E_x^{\ast },E_y^{\ast },E_z^{\ast },E_{\varphi }^{\ast },E_{\theta }^{\ast },E_{\psi }^{\ast }]}^{\mathrm{T}}$$

(40)

where the $E_x^{\ast }$, $E_y^{\ast }$, and $E_z^{\ast }$ are the position errors in the x, y, and z directions between the final position and the desired position, respectively. The $E_{\varphi }^{\ast }$, $E_{\theta }^{\ast }$, and $E_{\psi }^{\ast }$ is orientation error in $\varphi$, $\theta$, and $\psi$ directions between the final orientation and the desired orientation, respectively. The average deviation $E_p$ between the final position and the desired position, and the average deviation $E_o$ between the final orientation and the desired orientation are defined as follows:

$$E_p=\frac{\left|E_x^{\ast }\right|+\left|E_y^{\ast }\right|+\left|E_z^{\ast }\right|}{3}$$

(41)

$$E_o=\frac{\left|E_{\varphi }^{\ast }\right|+\left|E_{\theta }^{\ast }\right|+\left|E_{\psi }^{\ast }\right|}{3}$$

(42)

To make simulations more convincing, in IWGPM, GPM, and WLN methods, we set ${\lambda }_{\mathit{max}}=0.86$, $\epsilon =0.038$, $\xi =0.03$, $r_{\mathit{max}}=8$, $\beta =2$, $K={\left[\begin{array}{ccccccc}0,& 0.08,& 0.08,& 0.08,& 0.08,& 0.08,& 0\end{array}\right]}^{\mathrm{T}}$, T = 10 s, M = 100, $\mathsf{\Lambda }=0.005$.

4.1.1. Simulation Analysis of the Proposed IWGPM

Figure 6 shows the results obtained with the proposed IWGPM, in which the IK solution is: $q_{\mathrm{IWGPM}}=[43.955,1.7786,-0.8667,0.5413,0.2849,1.2034,0.9499{]}^{\mathrm{T}}$. The corresponding pose of the EE is $p_{\mathrm{IWGPM}}={[71.3652,106.7190,191.6448]}^{\mathrm{T}}\left(\mathrm{mm}\right)$ and $o_{\mathrm{IWGPM}}={[-0.9381,1.2661,-0.0038]}^{\mathrm{T}}\left(\mathrm{rad}\right)$, which are very close to $p_{\mathit{desired}}={[71.4062,106.7273,191.9349]}^{\mathrm{T}}\left(\mathrm{mm}\right)$ and $o_{\mathit{desired}}={[-0.9057,1.2209,-0.0824]}^{\mathrm{T}}\left(\mathrm{rad}\right)$. The average position deviation $E_p$ is 0.113mm and the average orientation deviation $E_o$ is 0.0387 rad. The pose accuracy of the EE is very high and meets the needs of surgery.

During the operation of the EE of the manipulator from the initial pose to the desired pose, Figure 6a shows the change curve of joint position. It is indicated that all joints operate within their position limits, and the position change curve of each joint shows continuity and small fluctuation. Figure 6b,c describe the deviation between the actual pose and the desired pose of the EE. In the initial stage, the actual pose is far from the desired pose. With the operation of time, it is more and more close to the desired pose. Figure 6d shows that $q_7$ is in the damping interval when $t=1\mathrm{s}$ and $t=1.8\mathrm{s}$. Figure 6e indicates that the limit repulsive potential energy function of $q_7$ produces virtual forces $r\left(q_7\right)=4.7111$ and $r\left(q_7\right)=7.9569$ at the same time point, which makes $q_7$ enter the flexible interval. The joint $q_3$ is also in the damping interval when $t=8.7\mathrm{s}$. Figure 6e also indicates that the limit repulsive potential energy function of $q_3$ produces virtual forces $r\left(q_3\right)=-6.066$ at the same time point, which makes $q_3$ enter the flexible interval. Figure 6f shows that the manipulator runs in the unsafe region from $t=0.3\mathrm{s}$ to $t=8.6\mathrm{s}$, and the damping factor is generated during this time period to ensure that joint velocities operate within their limit, as shown in Figure 6g,i. Meanwhile, the singular potential energy repulsion function generates a virtual force $F\left(q_2)=F\right(q_3)=F(q_4)=F(q_5)=F(q_6)=0.08$ to make the manipulator enter the safe region at $t=8.7\mathrm{s}$, as shown in Figure 6h.

4.1.2. Simulation Analysis of the GPM and WLN Algorithm

Figure 7 and Figure 8 show the results of the traditional GPM and WLN method. In the two methods, $q_7$ exceeded its limit at $t=2.1\mathrm{s}$ and $t=1.8\mathrm{s}$, respectively. It can also be clearly seen from Figure 8d that $q_7$ is in the damping interval from $t=1\mathrm{s}$ to $t=1.8\mathrm{s}$ and exceeds $q_{7\mathit{max}}$ from $t=1.8\mathrm{s}$ to the end of operation. When a joint exceeds its position limit, the corresponding joint will stop moving. Furthermore, there is no virtual force to push the joint away from its limit position, so when $q_7$ exceeds its limit, it remains unchanged, as shown in Figure 7a and Figure 8a. It is known that the orientation of the manipulator is mainly controlled by the rear three joints. Therefore, the stop motion of $q_7$ seriously affects the orientation accuracy of the EE, as shown in Figure 7c and Figure 8c. Figure 7b and Figure 8b describe the deviation between the actual position and the desired position of the EE. In the initial stage, the actual position is far from the desired position. With the operation of time, it is more and more close to the desired position. According to Equations (41) and (42), for GPM, the average position deviation $E_p$ is 1.0452 mm and the average orientation deviation $E_o$ is 0.6058 rad. For WLN method, the average position deviation $E_p$ is 1.46 mm and the average orientation deviation $E_o$ is 0.6144 rad. It also shows that the position accuracy of the EE is acceptable, but the orientation accuracy is very low. Figure 7d and Figure 8e show that the manipulator runs in the unsafe region from $t=0.5\mathrm{s}$ to $t=10\mathrm{s}$, and the damping value is generated during this time period to ensure that joint velocities operate within their limit, as shown in Figure 7e,f and Figure 8f,g, respectively.

In addition, Figure 7d and Figure 8e show that the manipulator is always in the unsafe region because of no singular potential energy function, while Figure 6f shows that the existence of singular potential energy function urges the manipulator to stay away from the unsafe region. i.e., the singular potential energy function introduced in this paper is very effective in solving singular problems.

Table 2. The deviation of the EE between the actual pose and the desired pose at $t=10\mathrm{s}$.

Method	${\mathit{E}}_{\mathit{x}}\left(\mathbf{mm}\right)$	${\mathit{E}}_{\mathit{y}}\left(\mathbf{mm}\right)$	${\mathit{E}}_{\mathit{z}}\left(\mathbf{mm}\right)$	$${\mathit{E}}_{\mathbf{\varphi }}\left(\mathbf{rad}\right)$$	${\mathit{E}}_{\mathbf{\theta }}\left(\mathbf{rad}\right)$	${\mathit{E}}_{\mathbf{\psi }}\left(\mathbf{rad}\right)$
IWGPM	0.041	0.0083	0.2901	0.0324	−0.0052	−0.0786
GPM	0.9143	−0.5202	1.7011	0.5647	−0.2269	−1.0259
WLN	1.0137	−0.7137	2.6513	0.5159	0.259	−1.0682

shows that the IK solution obtained by the proposed IWGPM. Compared with the proposed IWGPM, the position accuracy of the traditional GPM in x, y and z directions is reduced by 95.5%, 98.4%, and 82.9%, respectively. The orientation accuracy in $\varphi$, $\theta$ and $\psi$ directions decreased by 94.3%, 97.7%, and 92.3%, respectively. The position accuracy of the traditional WLN in x, y and z directions is reduced by 96.0%, 98.8%, and 89.1%, respectively. The orientation accuracy in $\varphi$, $\theta$ and $\psi$ directions decreased by 93.7%, 98.0%, and 92.6%, respectively. It can be seen that the proposed IWGPM can obtain very high pose accuracy, which fully meets the requirements of surgical accuracy.

4.2. Case Study 2

To observe the influence of joint position limits and singular potential energy function on the motion of the redundant manipulators. In this section, only the pose accuracy of the EE is considered for the simulation test in the following two cases. In case 1, based on the proposed IWGPM, joint position limit is removed and singular repulsive potential energy function is retained, Equation (38) is expressed as:

$$\dot{q}\left(k\right)=J_c^{+}\left(k\right)(\dot{x}\left(k\right)+\mathsf{\Lambda }{E}_e)-(I_n-J_c^{+}\left(k\right)J\left(k\right))F\left(q\left(k\right)\right)$$

(43)

In case 2, based on the proposed IWGPM, singular repulsive potential energy function is removed and joint position limit is retained, Equation (38) is presented as:

$$\dot{q}\left(k\right)=J_c^{+}\left(k\right)\left(\dot{x}\right(k)+\mathsf{\Lambda }{E}_e)-(I_n-J_c^{+}(k\left)J\right(k\left)\right)(I_n-W_c(k\left)\right)R_i\left(q\right(k\left)\right)$$

(44)

The initial pose, desired pose of the EE and other parameter settings are the same as those in the previous section. The simulations are shown in Figure 9 and Figure 10. In the first case, the average position deviation of the EE is 0.6553 mm and the average orientation deviation is 0.3353 rad. Compared with the proposed IWGPM, the average position deviation is reduced by 82.8% and the average orientation deviation is reduced by 88.4%. In the second case, the average position deviation of the EE is 2.5319mm and the average orientation deviation is 0.2547 rad. Compared with the proposed IWGPM, the average position deviation is reduced by 95.5% and the average orientation deviation is reduced by 84.8%. It is not difficult to find that the position and orientation of the EE are affected differently in the two cases. The first case has a great impact on the orientation accuracy, and the second case has a great impact on the position accuracy.

In addition, Figure 9a shows that $q_7$ exceeds $q_{7\mathit{max}}$ from $t=1.1\mathrm{s}$ to $t=10\mathrm{s}$. However, Figure 10d shows that $q_7$ is in the damping interval at $t=1.3\mathrm{s},1.6\mathrm{s},2.85\mathrm{s}$. Meanwhile, the virtual force generated by the joint position potential energy function urges $q_7$ to enter the flexible interval, as shown in Figure 10a, which shows that the joint position potential energy function is very effective. Figure 10e,f show that the manipulator is always in the unsafe region from $t=0.7\mathrm{s}$ to $t=10\mathrm{s}$. However, Figure 9d,e show that the manipulator after $t=3\mathrm{s}$ is in the safe region, which shows that the singular potential energy function is very effective. From another point of view, this section verifies that the proposed IWGPM is very effective in solving joint position limits and singularity problems.

5. Conclusions

In this paper, the proposed IWGPM for the IK solution of redundant surgical manipulators is proposed to avoid joint position limits and singularities. The IWGPM introdues a clamping weighted matrix and joint limit repulsive potential field function. The clamping weighting matrix prevents approaching joint position limits, and the joint limit repulsive potential field function belong to the null space of weighted matrix only generates virtual force in the damping interval to make joints return to the flexible interval. Furthermore, a singular repulsive potential field function of belong to the null space of weighted matrix is introduced. When robotic manipulators enter an unsafe region, the singular repulsive potential field function produces an elastic force to drive manipulators back to the safe region. The proposed IWGPM is applied to the 7-DOF redundant surgical manipulator and compared with the traditional GPM and WLN methods. Simulations show that the proposed IWGPM is effective in avoiding joint position limits and singularities, and the pose accuracy is higher than achievable with the traditional GPM and WLN methods.

Furthermore, based on the proposed IWGPM, simulation tests in two cases are carried out. The results show that joint position limits have a great impact on the orientation accuracy, and singular potential energy function has a great impact on the position accuracy. Future work will focus on obstacle avoidance and optimal path planning of redundant surgical manipulators.