General closed-form inverse kinematics for arbitrary three-joint subproblems based on the product of exponential model

Abstract

The inverse kinematics problems of robots are usually decomposed into several Paden–Kahan subproblems based on the product of exponential model. However, the simple combination of subproblems cannot solve all the inverse kinematics problems, and there is no common approach to solve arbitrary three-joint subproblems in an arbitrary postural relationship. The novel algebraic geometric (NAG) methods that obtain the general closed-form inverse kinematics for all types of three-joint subproblems are presented in this paper. The geometric and algebraic constraints are used as the conditions precedent to solve the inverse kinematics of three-joint subproblems. The NAG methods can be applied in the inverse kinematics of three-joint subproblems in an arbitrary postural relationship. The inverse kinematics simulations of all three-joint subproblems are implemented, and simulation results indicating that the inverse solutions are consistent with the given joint angles validate the general closed-form inverse kinematics. Huaque III minimally invasive surgical robot is used as the experimental platform for the simulation, and a master–slave tracking experiment is conducted to verify the NAG methods. The simulation result shows the inverse solutions and six sets given joint angles are consistent. Additionally, the mean and maximum of the master–slave tracking experiment for the closed-form solution are 0.1486 and 0.4777 mm, respectively, while the mean and maximum of the master–slave tracking experiment for the compensation method are 0.3188 and 0.6394 mm, respectively. The experiments results demonstrate that the closed-form solution is superior to the compensation method. The results verify the proposed general closed-form inverse kinematics based on the NAG methods.

Key words： inverse kinematics; Paden–Kahan subproblems; three-joint subproblems; product of exponential; closed-form solution

1 Introduction

The inverse kinematics problem of a robot manipulator is one of the most fundamental steps to obtain the mapping relationship between the end-effector and the joint angles in real-time control and trajectory planning [1,2]. This problem has been widely studied in various fields, such as medical robotics [3–5], bionic robotics [6,7], reconfigurable robotics [8,9], and redundant manipulator [10]. There are multiple methods derived based on the Denavit–Harbenterg (D–H) model and the product of exponential (POE) model to solve the inverse kinematics problem. For the D–H model, the robot needs to satisfy the Pieper criterion that three adjacent joint axes intersect at a single point. When it does not meet the Pieper criterion, there are other methods that can be applied, such as iterative methods [11,12], genetic methods [13,14], and neural network [15,16]. Tang [5] adopted the two-step approximate and compensation solution method to solve the inverse solution of the surgical instrument that did not meet the Pieper criterion. The wrist length of the surgical instrument was ignored in the solution process, and the relative motion error of the surgical instrument was compensated. However, the relative motion error of the surgical instrument increased when instrument pose was close to the singularity. Xu et al. [12] presented the hierarchical iterative inverse kinematics algorithm, which consisted of two-level iterations. The first-level iteration achieved accurate initial estimates through an extended heuristic iterative method. On this basis, the second-level iteration calculated all the joint angles. The proposed algorithm was applied to a six-degree-of-freedom (6-DOF) manipulator with a 2-DOF reduced wrist. Zhou et al. [14] took the motion constraints into account to design the fitness function and utilized the cosine adaptive genetic algorithm to complete the time-optimal trajectory planning. Tian and Xu [15] designed the convolutional neural network and the effective neural network visual layer pre-training method to control the robot arm end-to-end. The simulation result validated the efficiency of the proposed convolutional neural network. Although these methods can obtain the feasible inverse kinematics solution when the robot does not meet the Pieper criterion, they are time-consuming, and the completeness, convergence, and robustness of the inverse solution cannot be guaranteed. In addition, these methods based on the D–H model are complex to establish the local coordinate system between the adjacent joints, and plenty of specific analyses are required for each manipulator. Therefore, the requirement for the closed-form inverse solution is necessary because of its accuracy and real-time capability. If the trajectory of the end-effector is continuous, then the closed-form solutions of the joints are continuous. In theory, the closed-form solution based on the POE model is error-free, so the robot is not affected by the cumulative error.

The expression of joint axis in the POE model is different from that in the D–H model. In the POE model, the transform of an arbitrary rigid body with respect to the inertial coordinate system can be expressed by a screw displacement [17]. Hence, the singularity problem that happens when adjacent joints are parallel in the D–H model does not occur in the POE model [18,19]. Screw theory and POE formula are used to provide a simplified symbolic representation that can obtain definite geometric significance. Furthermore, the inverse kinematics solution based on the POE model is closed-form, which is the most desired form of the inverse solution. Paden–Kahan method [20] decomposed the integrated manipulator into three classes of subproblems that can be easily solved. However, the simple combination of subproblems cannot solve all inverse solutions. Chen and Gao [21] introduced 11 types of subproblems for different configurations, including revolute and prismatic joints, and presented the solutions of some subproblems. By contrast, the case where the axes of the first two joints do not intersect cannot be solved. Zhao et al. [22] expanded the three classes to 28 detailed types of subproblems, consequently enabling the inverse solution to be easily obtained by assembling several subproblems according to the configuration. The subproblem RRT (R: revolute, T: translational) case where two adjacent axes intersect was solved, but other complex cases were not mentioned. Tan et al. [23] extended the second subproblem by two axes, and the exact solution was obtained. The extended case is where the rotation to a point at a fixed distance from a given point was solved in a constructed manipulator. Leoro et al. [24] presented a new subproblem where the adjacent joints are parallel and both perpendicular to the third axis, and the solving method was applied to the 6-DOF EverRobot RH12. Chen et al. [25] improved an existing subproblem where adjacent axes were parallel and applied to the 6-DOF Qianjiang-I robot. Li et al. [26] proposed a new subproblem where three adjacent axes intersect at one point, and a humanoid arm with the configuration of generalized SRU was taken as the paradigm to validate the correctness of the analytical formula. Wang et al. [27,28] proposed the general frame for arbitrary 3R subproblems based on the POE model, and special cases such as perpendicular and parallel between multiple axes were taken into account. Simulation and experiment were likewise carried out to verify the availability of the proposed method. Wang et al. [29] presented a novel analytical inverse kinematics method for the 7-DOF space station remote manipulator system, and the simulation results showed improved calculation accuracy.

As discussed in the literature, the aforementioned methods focus on three revolute joint subproblems. However, the mixture of prismatic and revolute joints is frequently used in practical working environments, and there is no general approach to solve arbitrary three-joint subproblems in arbitrary posture. As a consequence, novel algebraic geometric (NAG) methods are presented to obtain the general closed-form inverse kinematics of all types of three-joint subproblems in this paper. The geometric and algebraic constraints are used as the conditions precedent to solve the inverse kinematics of three-joint subproblems. The NAG methods can solve inverse kinematics of three-joint subproblems in an arbitrary postural relationship.

The content of the paper is organized as follows. Section 2 introduces some fundamental properties of the POE formula and screw theory. The NAG methods for all types of three-joint subproblems in arbitrary postural relationship are described in detail in Section 3. In Section 4, the inverse kinematics simulations of all three-joint subproblems are implemented, and simulation and the master–slave tracking experiment based on the Huaque III minimally invasive surgical robot are carried out to verify the effectiveness of proposed NAG methods. The paper is concluded in the final section.

2 Preliminary knowledge

2.1 Mathematical description of the POE formula

The POE formula of the kinematics of a 3-DOF serial robot has the following form:

(1)\stackrel{\mathbf{~}}{\mathit{q}}\text{=}{\text{e}}^{{\hat{\mathit{\xi }}}_1{\theta }_1}{\text{e}}^{{\hat{\mathit{\xi }}}_2{\theta }_2}{\text{e}}^{{\hat{\mathit{\xi }}}_3{\theta }_3}\stackrel{\mathbf{~}}{\mathit{p}},

where {\mathit{\xi }}_i \left(i=1,2,3\right) represents the twist coordinate of the ith joint axis, {\theta }_i \left(i=1,2,3\right) represents the generalized angle of the ith joint, \mathit{\xi }=\left[\begin{array}{c}\text{ω}\\ \mathit{r}\times \text{ω}\end{array}\right] represents the revolute joint twist, \mathit{\xi }=\left[\begin{array}{c}{\mathbf{0}}_{3\times 1}\\ \mathit{v}\end{array}\right] represents the prismatic joint twist, \text{ω} represents the unit directional vector of the revolute joint axis, r is the position vector of the reference point r of the revolute joint axis, \mathit{v} represents the unit directional vector of the translational joint axis, {\hat{\mathit{\xi }}}_i \left(i=1,2,3\right) represents the instantaneous joint twist of the ith joint axis, and \stackrel{\mathbf{~}}{\mathit{q}} and \stackrel{\mathbf{~}}{\mathit{p}} represent the homogeneous coordinate of points q and p, respectively. In addition, {\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i} represents the rigid motion. For the revolute and translational joints, {\text{e}}^{\hat{\mathit{\xi }}\theta } is expressed respectively as

(2)\begin{array}{rl}& {\text{e}}^{\hat{\mathit{\xi }}\theta }=\\ & \{\begin{array}{c}\left[\begin{array}{cc}{\mathit{I}}_{3\times 3}& \mathit{v}\theta \\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right],\text{ω}=\mathbf{0}\text{,}\\ \left[\begin{array}{cc}{\text{e}}^{\widehat{\text{ω}}\theta }& \left({\mathit{I}}_{3\times 3}-{\text{e}}^{\widehat{\text{ω}}\theta }\right)\left(\text{ω}\times \left(\mathit{r}\times \text{ω}\right)\right)+\theta \text{ω}{\text{ω}}^{\text{T}}\left(\mathit{r}\times \text{ω}\right)\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right],\text{ω}\ne \mathbf{0}\text{,}\end{array}\end{array}

where \widehat{\text{ω}} represents the skew-symmetric matrices of \text{ω}. {\text{e}}^{\widehat{\text{ω}}\theta } is a rotation matrix and is equivalent to the rotation matrix \mathit{R}\in SO\left(3\right) according to Euler’s theorem. {\text{e}}^{\widehat{\text{ω}}\theta } can be expressed via Rodrigues’ formula as

(3){\text{e}}^{\widehat{\text{ω}}\theta }=\mathit{I}+\widehat{\text{ω}}\text{sin}\theta +{\widehat{\text{ω}}}^2\left(1-\cos \theta \right).

2.2 Mathematical description of the POE formula

The Paden–Kahan subproblem 1 is \stackrel{\mathbf{~}}{\mathit{q}}={\text{e}}^{\hat{\mathit{\xi }}\theta }\stackrel{\mathbf{~}}{\mathit{p}} and can be transformed as

(4)\mathit{q}={\text{e}}^{\widehat{\text{ω}}\theta }\left(\mathit{p}-\mathit{r}\right)+\mathit{r}\text{,}

where \mathit{q} and \mathit{p} represent the position vectors of points q and p, respectively. Substituting Rodrigues’ formula for {\text{e}}^{\widehat{\text{ω}}\theta } into the above equation yields

(5)\mathit{x}\sin \theta +\mathit{y}\cos \theta +\mathit{z}=\mathbf{0},

where \mathit{x}=\widehat{\text{ω}}\left(\mathit{p}-\mathit{r}\right)\in R^{3\times 1}, \mathit{y}=-{\widehat{\text{ω}}}^2\left(\mathit{p}-\mathit{r}\right)\in R^{3\times 1}, and \mathit{z}={\widehat{\text{ω}}}^2\left(\mathit{p}-\mathit{r}\right)+\mathit{p}-\mathit{q}\in R^{3\times 1}. Since {\mathit{x}}^{\text{T}}\mathit{y}=0 and {\mathit{x}}^{\text{T}}\mathit{x}={\mathit{y}}^{\text{T}}\mathit{y}, \theta can be solved by \sin \theta =-{\displaystyle \frac{{\mathit{x}}^{\text{T}}\mathit{z}}{{\mathit{x}}^{\text{T}}\mathit{x}}} and \cos \theta =-{\displaystyle \frac{{\mathit{y}}^{\text{T}}\mathit{z}}{{\mathit{y}}^{\text{T}}\mathit{y}}}, respectively.

The distance preservation principle is applied to the solution of subproblems with a revolute joint, which is often accompanied by squaring both sides of the equation. For instance, there is a distance \delta between two vectors \mathit{s} and \mathit{t} as \delta =\Vert \mathit{s}-\mathit{t}\Vert. Squaring both sides yields {\Vert \mathit{s}\Vert }^2+{\Vert \mathit{t}\Vert }^2+2{\mathit{t}}^{\text{T}}\mathit{s}={\delta }^2.

3 NAG method for arbitrary three-joint subproblems

Three-joint subproblems are extensions of several base subproblems, with R for revolute joint and T for translational joint. In total, there are many types of three-joint subproblems, namely, RRR, RRT, RTR, TRR, RTT, TRT, TTR, and TTT. Equation (1) is transformed as follows

(6)\stackrel{\mathbf{~}}{\mathit{p}}={\text{e}}^{-{\hat{\mathit{\xi }}}_3{\theta }_3}{\text{e}}^{-{\hat{\mathit{\xi }}}_2{\theta }_2}{\text{e}}^{-{\hat{\mathit{\xi }}}_1{\theta }_1}\stackrel{\mathbf{~}}{\mathit{q}}.

Combining Eqs. (1) and (6) easily derives the equivalence of RRT and RTT to TRR and TTR [21]. Hence, the general closed-form solutions of RRR, RRT, RTR, TRR, TRT, and TTT types of three-joint subproblems are derived, and the special postures of those three-joint subproblems are discussed in this paper.

3.1 NAG method realization for the RRR subproblem

For the RRR three-joint subproblem, point p is rotated around {\mathit{\xi }}_3, {\mathit{\xi }}_2, and {\mathit{\xi }}_1 respectively by {\theta }_3, {\theta }_2, and {\theta }_1 to point q. Point p rotates around {\mathit{\xi }}_3 at {\theta }_3 to point c and point q rotates around {\mathit{\xi }}_1 at -{\theta }_1 to point d, as shown in Fig.1. The vectors c, d, p, p₁, p₂, p₃, and q represent the position vectors of points c, d, p, p₁, p₂, p₃, and q, respectively. The representations below are consistent with the definition here. This means that \mathit{c}={\text{e}}^{{\widehat{\text{ω}}}_3{\theta }_3}\left(\mathit{p}-{\mathit{p}}_3\right)+{\mathit{p}}_3 and \mathit{d}={\text{e}}^{-{\widehat{\text{ω}}}_1{\theta }_1}\left(\mathit{q}-{\mathit{p}}_1\right)+{\mathit{p}}_1, where points p₁ and p₃ are located on the {\mathit{\xi }}_1 and {\mathit{\xi }}_3, respectively. Clearly, points c and d are located on the circle C₂ normal to {\mathit{\xi }}_2. The twist {\mathit{\xi }}_2 passes through point p₂. According to the geometric and algebraic constraints, this is easily accessible as

Fig.1 General form of the RRR three-joint subproblem.

Full size|PPT slide

(7){\text{ω}}_2^{\text{T}}\left(\mathit{c}-\mathit{d}\right)=0,

(8)\Vert \mathit{c}-{\mathit{p}}_2\Vert =\Vert \mathit{d}-{\mathit{p}}_2\Vert .

By substituting the expressions for points c and d into Eqs. (7) and (8) and squaring both sides of Eq. (8), {\text{e}}^{-{\widehat{\text{ω}}}_1{\theta }_1} and {\text{e}}^{{\widehat{\text{ω}}}_3{\theta }_3} are replaced by their Rodrigues’ formulas. The above two equations are transformed as follows:

(9)x_1\sin {\theta }_1+y_1\cos {\theta }_1+x_2\sin {\theta }_3+y_2\cos {\theta }_3+{\mathit{\text{z}}}_1=0,

(10)x_3\sin {\theta }_1+y_3\cos {\theta }_1+x_4\sin {\theta }_3+y_4\cos {\theta }_3+{\mathit{\text{z}}}_2=0,

where x_1=-{\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1\left(\mathit{q}-{\mathit{p}}_1\right), y_1=-{\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1^2\left(\mathit{q}-{\mathit{p}}_1\right), x_2= -{\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_3\left(\mathit{p}-{\mathit{p}}_3\right), y_2={\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_3^2\left(\mathit{p}-{\mathit{p}}_3\right), {\mathit{\text{z}}}_1={\text{ω}}_2^{\text{T}}[{\widehat{\text{ω}}}_1^2(\mathit{q}-{\mathit{p}}_1)- {\widehat{\text{ω}}}_3^2(\mathit{p}-{\mathit{p}}_3)+\mathit{q}-\mathit{p}], x_3=-2{\left({\mathit{p}}_1-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_1\left(\mathit{q}-{\mathit{p}}_1\right), y_3= -2{\left({\mathit{p}}_1-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_1^2\left(\mathit{q}-{\mathit{p}}_1\right), x_4=-2{\left({\mathit{p}}_3-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_3\left(\mathit{p}-{\mathit{p}}_3\right), y_4= 2{\left({\mathit{p}}_3-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_3^2\left(\mathit{p}-{\mathit{p}}_3\right), and {\mathit{\text{z}}}_2={\Vert \mathit{q}-{\mathit{p}}_1\Vert }^2+{\Vert {\mathit{p}}_1-{\mathit{p}}_2\Vert }^2- {\Vert \mathit{p}-{\mathit{p}}_3\Vert }^2-{\Vert {\mathit{p}}_3-{\mathit{p}}_2\Vert }^2+2{\left({\mathit{p}}_1-{\mathit{p}}_2\right)}^{\text{T}}\left(\mathit{I}+{\widehat{\text{ω}}}_1^2\right)\left(\mathit{q}-{\mathit{p}}_1\right)-2{\left({\mathit{p}}_3-{\mathit{p}}_2\right)}^{\text{T}} \cdot \left(\mathit{I}+{\widehat{\text{ω}}}_3^2\right)\left(\mathit{p}-{\mathit{p}}_3\right).

For the revolute joints, {\text{ω}}_1^{\text{T}}{\widehat{\text{ω}}}_{\text{2}}\text{=}{\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_{\text{1}}={\mathbf{0}}_{\text{1}\times \text{3}} holds if {\mathit{\xi }}_1 is parallel to {\mathit{\xi }}_2. The RRR three-joint subproblems for special configurations are explained and illustrated considering the property of parallel joint axes.

Category 1: {\mathit{\xi }}_2\nparallel {\mathit{\xi }}_1 when {\mathit{\xi }}_2\parallel {\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_3={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1\ne {\mathbf{0}}_{\text{1}\times \text{3}}. Then Eq. (9) is transformed to

(11)x_1\sin {\theta }_1+y_1\cos {\theta }_1+{\mathit{\text{z}}}_1=0.

{\theta }_1 can be solved by combining the above equation and the trigonometric relationship. If x_1\ne 0, then {\theta }_1= \arcsin {\displaystyle \frac{-{\mathit{\text{z}}}_1}{\sqrt{x_1^2+y_1^2}}}-\arctan {\displaystyle \frac{y_1}{x_1}}; if y_1\ne 0, then {\theta }_1= \arccos {\displaystyle \frac{-{\mathit{\text{z}}}_1}{\sqrt{x_1^2+y_1^2}}}+\arctan {\displaystyle \frac{x_1}{y_1}}.

The value of {\theta }_1 is substituted into Eq. (10), and similarly, {\theta }_3 can be solved. {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 2: {\mathit{\xi }}_2\nparallel {\mathit{\xi }}_3 when {\mathit{\xi }}_2\parallel {\mathit{\xi }}_1.

This category indicates {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_3\ne {\mathbf{0}}_{\text{1}\times \text{3}}. Then Eq. (9) is transformed to

(12)x_2\sin {\theta }_3+y_2\cos {\theta }_3+{\mathit{\text{z}}}_1=0.

Likewise, {\theta }_3 can be solved by combining the above equation and the trigonometric relationship, and {\theta }_1 can be solved by substituting the value of {\theta }_3 into Eq. (10). {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 3: {\mathit{\xi }}_2\parallel {\mathit{\xi }}_3 when {\mathit{\xi }}_2\parallel {\mathit{\xi }}_1.

This category indicates that {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_3={\mathbf{0}}_{\text{1}\times \text{3}}. Points c, d, p₁, p₂, and p₃ are located on one plane. The plane passes through points p and q and is perpendicular to the twist {\mathit{\xi }}_2. Therefore, the identity of Eq. (7) is always established. For Eq. (8), there are two cases here, namely, the unique solution or an infinite number of solutions. The case of the unique solution is shown in Fig.2. Let l_{12}=\Vert {\mathit{p}}_1-{\mathit{p}}_2\Vert, l_{23}=\Vert {\mathit{p}}_3-{\mathit{p}}_2\Vert, {\delta }_1=\Vert \mathit{q}-{\mathit{p}}_1\Vert, {\delta }_3=\Vert \mathit{p}-{\mathit{p}}_3\Vert. l₁₂ represents the distance between points p₁ and p₂, l₂₃ represents the distance between points p₂ and p₃, δ₁ represents the distance between points q and p₁, and δ₃ represents the distance between points p and p₃. Assuming l_{12}>l_{23}, the following equation is satisfied when there is a unique solution; otherwise, there is an infinite number of solutions:

Fig.2 Form of the unique solution under triaxial parallelism.

Full size|PPT slide

(13)l_{12}-{\delta }_1=l_{23}+{\delta }_3.

The locations of the transition points are \mathit{d}={\mathit{p}}_1+\left({\mathit{p}}_2-{\mathit{p}}_1\right){\delta }_1/\phantom{\left({\mathit{p}}_2-{\mathit{p}}_1\right){\delta }_1{l}_{12}}{l}_{12} and \mathit{c}={\mathit{p}}_2+\left({\mathit{p}}_3-{\mathit{p}}_2\right)\left(l_{23}+{\delta }_3\right)/\phantom{\left({\mathit{p}}_3-{\mathit{p}}_2\right)\left(l_{23}+{\delta }_3\right)l_{23}}{l}_{23}. {\theta }_1, {\theta }_2, and {\theta }_3 are solved by Eq. (4).

Conversely, if , then l_{12}+{\delta }_1=l_{23}-{\delta }_3 is satisfied when there is a unique solution.

Category 4: {\mathit{\xi }}_2\nparallel {\mathit{\xi }}_3 when {\mathit{\xi }}_2\nparallel {\mathit{\xi }}_1.

There are two cases in this category: Two adjacent axes are non-intersecting or two adjacent axes are intersected. For the case where two adjacent axes do not intersect, x_1{y}_3-x_3{y}_1\ne 0 and x_2{y}_4-x_4{y}_2\ne 0 hold at least one. Thus, Eqs. (9) and (10) can be written in the case of x_1{y}_3- x_3{y}_1\ne 0 as

(14)\{\begin{array}{c}\sin {\theta }_1=a_1\sin {\theta }_3+b_1\cos {\theta }_3+c_1,\\ \cos {\theta }_1=a_2\sin {\theta }_3+b_2\cos {\theta }_3+c_2,\end{array}

where a_1={\displaystyle \frac{x_4{y}_1-x_2{y}_3}{x_1{y}_3-x_3{y}_1}}, b_1={\displaystyle \frac{y_1{y}_4-y_2{y}_3}{x_1{y}_3-x_3{y}_1}}, c_1={\displaystyle \frac{y_1{z}_2-y_3{z}_1}{x_1{y}_3-x_3{y}_1}}, a_2={\displaystyle \frac{x_2{x}_3-x_1{x}_4}{x_1{y}_3-x_3{y}_1}}, b_2={\displaystyle \frac{x_3{y}_2-x_1{y}_4}{x_1{y}_3-x_3{y}_1}}, and c_2={\displaystyle \frac{x_3{z}_1-x_1{z}_2}{x_1{y}_3-x_3{y}_1}}. Substituting Eq. (14) into {\sin }^2{\theta }_1+{\cos }^2{\theta }_1=1 is written as

(15){\left(a_1\sin {\theta }_3+b_1\cos {\theta }_3+c_1\right)}^2\text{+}{\left(a_2\sin {\theta }_3+b_2\cos {\theta }_3+c_2\right)}^2=1.

Let t=\tan {\theta }_3/\phantom{{\theta }_{3}2}2, then \sin {\theta }_3=2t/\phantom{2t\left(1+t^2\right)}\left(1+t^2\right) and \cos {\theta }_3= \left(1-t^2\right)/\phantom{\left(1-t^2\right)\left(1+t^2\right)}\left(1+t^2\right). These are substituted into Eq. (15) as

(16)m_1{t}^4+m_2{t}^3+m_3{t}^2+m_{4}t+m_5=0,

where m_1={\left(b_1-c_1\right)}^2+{\left(b_2-c_2\right)}^2-1, m_2=4[a_1(c_1-b_1)+ a_2(c_2-b_2)], m_3=2\left(2a_1^2+2a_2^2-b_1^2-b_2^2+c_1^2+c_2^2-1\right), m_4= 4\left[a_1\left(b_1+c_1\right)+a_2\left(b_2+c_2\right)\right], and m_5=(b_1+c_1{)}^2+ {\left(b_2+c_2\right)}^2-1. For the quartic equation of Eq. (16), the solution of t is easily obtained by adopting Ferrari’s method. The value of {\theta }_3 can be solved as

(17){\theta }_3=2\arctan t.

The value of {\theta }_3 is substituted into Eq. (14) to solve {\theta }_1. Similarly, {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

The schematic for the case where two adjacent axes intersect is shown in Fig.3. The twists {\mathit{\xi }}_1 and {\mathit{\xi }}_2 intersect at one point. Points c and d are located on one plane that is perpendicular to the twist {\mathit{\xi }}_2. The geometric constraint is the same as Eq. (7), and algebraic constraint in Fig.3 is

Fig.3 General form of the two adjacent axes of the RRR three-joint subproblem.

Full size|PPT slide

(18)\Vert \mathit{c}-{\mathit{p}}_2\Vert =\Vert \mathit{q}-{\mathit{p}}_2\Vert .

By substituting the expression for point c into Eq. (18) and squaring both sides of Eq. (18), {\text{e}}^{{\widehat{\text{ω}}}_3{\theta }_3} is replaced by Rodrigues’ formula. The above equation is transformed as follows:

(19)a_1\sin {\theta }_3+b_1\cos {\theta }_3+c_1=0,

where a_1=2{\left({\mathit{p}}_3-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_{\text{3}}\left(\mathit{p}-{\mathit{p}}_3\right), b_1=-2{\left({\mathit{p}}_3-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_3^2 \cdot \left(\mathit{p}-{\mathit{p}}_3\right), and c_1={\Vert \mathit{p}-{\mathit{p}}_3\Vert }^2+{\Vert {\mathit{p}}_3-{\mathit{p}}_2\Vert }^2-{\Vert \mathit{q}-{\mathit{p}}_2\Vert }^2+ 2{\left({\mathit{p}}_3-{\mathit{p}}_2\right)}^{\text{T}}\left(\mathit{I}+{\widehat{\text{ω}}}_3^2\right)\left(\mathit{p}-{\mathit{p}}_3\right).

{\theta }_3 can be solved by combining the above equation and the trigonometric relationship, and {\theta }_1 can be solved by substituting the value of {\theta }_3 into Eq. (9). {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

3.2 NAG method realization for the RRT subproblem

For the RRT three-joint subproblem, it is assumed that point p moves along {\mathit{\xi }}_3 by {\theta }_3 to point c and point q rotates around {\mathit{\xi }}_1 at -{\theta }_1 to point d, as shown in Fig.4. This means that \mathit{c}={\theta }_3{\mathit{v}}_3+\mathit{p} and \mathit{d}={\text{e}}^{-{\widehat{\text{ω}}}_1{\theta }_1}\left(\mathit{q}-{\mathit{p}}_1\right)+{\mathit{p}}_1, where p₁ is located on the {\mathit{\xi }}_1. It is obvious that points c and d are located on the circle C₂ normal to {\mathit{\xi }}_2. The twist {\mathit{\xi }}_2 passes through point p₂. This is easily accessible according to the geometric and algebraic constraints as

Fig.4 General form of the RRT three-joint subproblem.

Full size|PPT slide

(20){\text{ω}}_2^{\text{T}}\left(\mathit{d}-\mathit{c}\right)=0,

(21)\Vert \mathit{c}-{\mathit{p}}_2\Vert =\Vert \mathit{d}-{\mathit{p}}_2\Vert .

By substituting the expressions for points c and d into Eqs. (20) and (21), respectively, and squaring both sides of Eq. (21), {\text{e}}^{-{\widehat{\text{ω}}}_1{\theta }_1} is replaced by Rodrigues’ formula. The above two equations are transformed as follows:

(22)x_1\sin {\theta }_1+y_1\cos {\theta }_1+{\lambda }_1{\theta }_3+{\mathit{\text{z}}}_1=0,

(23)x_2\sin {\theta }_1+y_2\cos {\theta }_1+{\lambda }_2{{\theta }_3}^{\text{2}}\text{+}{\lambda }_3{\theta }_3+{\mathit{\text{z}}}_2=0,

where x_1=-{\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1\left(\mathit{q}-{\mathit{p}}_1\right), y_1=-{\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1^2\left(\mathit{q}-{\mathit{p}}_1\right), {\lambda }_1= -{\text{ω}}_2^{\text{T}}{\mathit{v}}_3, {\mathit{\text{z}}}_1={\text{ω}}_2^{\text{T}}\left[{\widehat{\text{ω}}}_1^2\left(\mathit{q}-{\mathit{p}}_1\right)+\mathit{q}-\mathit{p}\right], x_2=-2{\left({\mathit{p}}_1-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_{\text{1}} \cdot \left(\mathit{q}-{\mathit{p}}_1\right), y_2=-2{\left({\mathit{p}}_1-{\mathit{p}}_2\right)}^{\text{T}}{\widehat{\text{ω}}}_1^2\left(\mathit{q}-{\mathit{p}}_1\right), {\lambda }_2=-1, {\lambda }_3= -2{\left(\mathit{p}-{\mathit{p}}_2\right)}^{\text{T}}{\mathit{v}}_3, {\mathit{\text{z}}}_2={\Vert \mathit{q}-{\mathit{p}}_1\Vert }^2+{\Vert {\mathit{p}}_1-{\mathit{p}}_2\Vert }^2-{\Vert \mathit{p}-{\mathit{p}}_2\Vert }^2+ 2{\left({\mathit{p}}_1-{\mathit{p}}_2\right)}^{\text{T}}\left(\mathit{I}+{\widehat{\text{ω}}}_1^2\right)\left(\mathit{q}-{\mathit{p}}_1\right).

For the revolute joints, {\text{ω}}_1^{\text{T}}{\widehat{\text{ω}}}_{\text{2}}={\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_{\text{1}}={\mathbf{0}}_{\text{1}\times \text{3}} holds if {\mathit{\xi }}_1 is parallel to {\mathit{\xi }}_2. For arbitrary two axes, {\text{ω}}_1^{\text{T}}{\text{ω}}_2=0 holds if {\mathit{\xi }}_1 is perpendicular to {\mathit{\xi }}_2. The RRT subproblems for special configurations are explained and illustrated considering the characteristics of parallel and vertical joint axes.

Category 1: {\mathit{\xi }}_2\parallel {\mathit{\xi }}_1 when {\mathit{\xi }}_2\mathrm{\perp }{\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3=0. Points c, d, p₁, and p₂ are located on one plane. The plane passes through points p and q and is perpendicular to the twist {\mathit{\xi }}_2. Therefore, the identity of Eq. (20) is always established. For Eq. (23), there are two cases here: the unique solution or an infinite number of solutions, as shown in Fig.5 and Fig.5, respectively. Let l_{12}=\Vert {\mathit{p}}_1-{\mathit{p}}_2\Vert, l_{\text{c}2}=\Vert \mathit{c}-{\mathit{p}}_2\Vert, and {\delta }_1=\Vert \mathit{q}-{\mathit{p}}_1\Vert, where l₁₂ represents the distance between the points p₁ and p₂, l_c2 represents the distance between the points c and p₂, and δ₁ represents the distance between the points q and p₁. Point c is the tangent point between the circle C₂ and twist {\mathit{\xi }}_3, and l_{12}+{\delta }_1=l_{\text{c}2} is satisfied when there is a unique solution. This means that \mathit{c}=\mathit{p}+{\mathit{v}}_3{\mathit{v}}_3^{\text{T}}\left({\mathit{p}}_2-\mathit{p}\right). For the case of a unique solution, Eq. (23) should satisfy that {\theta }_3 has a unique solution as follows:

Fig.5 Solution of RRT satisfying {\text{ω}}_2\parallel {\text{ω}}_1 and {\text{ω}}_2\mathrm{\perp }{\mathit{v}}_3. (a) Unique solution of category 1, (b) infinite number of solutions of category 1.

Full size|PPT slide

(24){\lambda }_3^2-4{\lambda }_2\left(x_2\sin {\theta }_1+y_2\cos {\theta }_1+{\mathit{\text{z}}}_2\right)=0.

{\theta }_1 can be solved by combining the above equation and the trigonometric relationship, and {\theta }_3 can be solved by substituting the value of {\theta }_1 into Eq. (23).

Another solution to this problem is to obtain {\theta }_3=-{\lambda }_3/\phantom{-{\lambda }_3\left(2{\lambda }_2\right)}\left(2{\lambda }_2\right) based on the fact that {\theta }_3 has a unique solution. Then, {\theta }_1 can be solved by substituting the value of {\theta }_3 into Eq. (23) and the trigonometric relationship. {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

The infinite number of solutions of {\theta }_3 are depicted as red line segments in Fig.5(b), with the inverse solution of {\theta }_3 at any point on the line segments.

Category 2: {\mathit{\xi }}_2 is not parallel to {\mathit{\xi }}_1 when {\mathit{\xi }}_2\mathrm{\perp }{\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1\ne {\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3=0. Then Eq. (22) is transformed to

(25)x_1\sin {\theta }_1+y_1\cos {\theta }_1+{\mathit{\text{z}}}_1=0.

{\theta }_1 can be solved by combining the above equation and the trigonometric relationship. {\theta }_3 can be solved by substituting the value of {\theta }_1 into Eq. (23) and the trigonometric relationship. {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 3: {\mathit{\xi }}_2\parallel {\mathit{\xi }}_1 when {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3\ne 0. Then {\theta }_3 can be solved by {\theta }_3=-{\mathit{\text{z}}}_1/\phantom{{\mathit{\text{z}}}_1{\lambda }_1}{\lambda }_1 according to Eq. (22). The value of {\theta }_3 is substituted into Eq. (23) to solve {\theta }_1. {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 4: {\mathit{\xi }}_2\nparallel {\mathit{\xi }}_1 when {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_1\ne {\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3\ne 0. If twists {\mathit{\xi }}_1 and {\mathit{\xi }}_2 intersect, the choices of points p₁ and p₂ should not be the point where the twists intersect. {\theta }_3 is replaced with \sin {\theta }_1 and \cos {\theta }_1 in Eq. (22) and substituted into Eq. (23) as

(26)\begin{array}{rl}& a_1{\sin }^2{\theta }_1+a_2{\cos }^2{\theta }_1+a_3\sin {\theta }_1\cos {\theta }_1\\ +& a_4\sin {\theta }_1+a_5\cos {\theta }_1+a_6=0,\end{array}

where a_1={\lambda }_2{x}_1^2, a_2={\lambda }_2{y}_1^2, a_{\text{3}}=2{\lambda }_2{x}_1{y}_1, a_4={\lambda }_1^2{x}_2+ 2{\lambda }_2{x}_1{\mathit{\text{z}}}_1-{\lambda }_1{\lambda }_3{x}_1, a_5={\lambda }_1^2{y}_2+2{\lambda }_2{y}_1{\mathit{\text{z}}}_1-{\lambda }_1{\lambda }_3{y}_1, and a_6= {\lambda }_2{\mathit{\text{z}}}_1^2-{\lambda }_1{\lambda }_3{\mathit{\text{z}}}_1+{\lambda }_1^2{\mathit{\text{z}}}_2. Let t=\tan {\theta }_1/\phantom{{\theta }_{1}2}2, then {\theta }_1= \sin (2t)/\phantom{2t\left(1+t^2\right)}\left(1+t^2\right) and \cos {\theta }_1=\left(1-t^2\right)/\phantom{\left(1-t^2\right)\left(1\text{+}{t}^2\right)}\left(1\text{+}{t}^2\right). Those are substituted into Eq. (26) as

(27)m_1{t}^4+m_2{t}^3+m_3{t}^2+m_{4}t+m_5=0,

where m_1=a_2-a_5+a_6, m_2=2(a_4-a_3), m_3=2(2a_1- a_2+a_6), m_4=2\left(a_3+a_4\right), and m_5=a_2+a_5+a_6. For the quartic equation of Eq. (27), the solution t is easily obtained by adopting Ferrari’s method. The value of {\theta }_1 can be solved as

(28){\theta }_1=2\arctan t.

The value of {\theta }_1 is substituted into Eq. (22) to solve {\theta }_3. Similarly, {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

3.3 NAG method realization for the RTR subproblem

For the RTR three-joint subproblem, it is assumed that point p rotates around {\mathit{\xi }}_3 at {\theta }_3 to point c and point c moves along {\mathit{\xi }}_2 by {\theta }_2 to point d, as shown in Fig.6. This means that \mathit{c}={\text{e}}^{{\widehat{\text{ω}}}_3{\theta }_3}\left(\mathit{p}-{\mathit{p}}_3\right)+{\mathit{p}}_3 and \mathit{d}=\mathit{c}+{\theta }_2{\mathit{v}}_2, where p₃ is located on the {\mathit{\xi }}_3. Clearly, points d and q are located on the circle C₁ normal to {\mathit{\xi }}_1. The twist {\mathit{\xi }}_1 passes through point p₁. This is easily accessible according to the geometric and algebraic constraints as

Fig.6 General form of the RTR three-joint subproblem.

Full size|PPT slide

(29){\text{ω}}_1^{\text{T}}\left(\mathit{d}-\mathit{q}\right)=0,

(30)\Vert \mathit{q}-{\mathit{p}}_1\Vert =\Vert \mathit{d}-{\mathit{p}}_1\Vert .

By substituting the expressions for points c and d into Eqs. (29) and (30), respectively, and squaring both sides of Eq. (30), {\text{e}}^{{\widehat{\text{ω}}}_3{\theta }_3} is replaced by Rodrigues’ formula. The above two equations are transformed as follows:

(31)x_1\sin {\theta }_3+y_1\cos {\theta }_3+{\lambda }_1{\theta }_2+{\mathit{\text{z}}}_1=0,

(32)\begin{array}{rl}& x_2\sin {\theta }_3+y_2\cos {\theta }_3+{\lambda }_2{{\theta }_2}^{\text{2}}\text{+}{\lambda }_3{\theta }_2\\ +& x_3{\theta }_2\sin {\theta }_3+y_3{\theta }_2\cos {\theta }_3+{\mathit{\text{z}}}_2=0,\end{array}

where x_1={\text{ω}}_1^{\text{T}}{\widehat{\text{ω}}}_3\left(\mathit{p}-{\mathit{p}}_3\right), y_1=-{\text{ω}}_1^{\text{T}}{\widehat{\text{ω}}}_3^2\left(\mathit{p}-{\mathit{p}}_3\right), {\lambda }_1={\text{ω}}_1^{\text{T}}{\mathit{v}}_2, {\mathit{\text{z}}}_1={\text{ω}}_1^{\text{T}}\left({\widehat{\text{ω}}}_3^2\left(\mathit{p}-{\mathit{p}}_3\right)+\mathit{p}-\mathit{q}\right), x_2=2{\left({\mathit{p}}_3-{\mathit{p}}_1\right)}^{\text{T}}{\widehat{\text{ω}}}_3\left(\mathit{p}-{\mathit{p}}_3\right), y_2=-2{\left({\mathit{p}}_3-{\mathit{p}}_1\right)}^{\text{T}}{\widehat{\text{ω}}}_3^2\left(\mathit{p}-{\mathit{p}}_3\right), {\lambda }_2=1, {\lambda }_3=2{\mathit{v}}_2^{\text{T}}[\left(\mathit{I}+{\widehat{\text{ω}}}_3^2\right)\left(\mathit{p}-{\mathit{p}}_3\right)+ \left({\mathit{p}}_3-{\mathit{p}}_1\right)], x_3=2{\mathit{v}}_2^{\text{T}}{\widehat{\text{ω}}}_3\left(\mathit{p}-{\mathit{p}}_3\right), y_3=-2{\mathit{v}}_2^{\text{T}}{\widehat{\text{ω}}}_3^2\left(\mathit{p}-{\mathit{p}}_3\right), {\mathit{\text{z}}}_2= \Vert \mathit{p}-{\mathit{p}}_3{\Vert }^2+\Vert {\mathit{p}}_3-{\mathit{p}}_1{\Vert }^2-\Vert \mathit{q}-{\mathit{p}}_1{\Vert }^2+2{\left({\mathit{p}}_3-{\mathit{p}}_1\right)}^{\text{T}}\left(\mathit{I}+{\widehat{\text{ω}}}_3\right)\left(\mathit{p}-{\mathit{p}}_3\right).

For the revolute joints, {\text{ω}}_1^{\text{T}}{\widehat{\text{ω}}}_{\text{2}}={\text{ω}}_2^{\text{T}}{\widehat{\text{ω}}}_{\text{1}}={\mathbf{0}}_{\text{1}\times \text{3}} holds if {\mathit{\xi }}_1 is parallel to {\mathit{\xi }}_2. For two arbitrary axes, {\text{ω}}_1^{\text{T}}{\text{ω}}_2=0 holds if {\mathit{\xi }}_1 is perpendicular to {\mathit{\xi }}_2. The RTR three-joint subproblems for special configurations are explained and illustrated considering the characteristics of parallel and vertical joint axes.

Category 1: {\mathit{\xi }}_1\parallel {\mathit{\xi }}_3 when {\mathit{\xi }}_1\mathrm{\perp }{\mathit{\xi }}_2.

This category indicates {\text{ω}}_3^{\text{T}}{\widehat{\text{ω}}}_1={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_1^{\text{T}}{\mathit{v}}_2=0. Points c, d, p₁, and p₃ are located on one plane, which passes through points p and q and is perpendicular to the twist {\mathit{\xi }}_1. Therefore, the identity of Eq. (29) is always established. For Eq. (32), there are two cases here, namely, the unique solution or an infinite number of solutions, as shown in Fig.7(a) and 7(b), respectively. For the unique solution of special posture, both transition points c and d are tangent points, and both {\mathit{\xi }}_1 and {\mathit{\xi }}_3 are on opposite sides of {\mathit{\xi }}_2, as shown in Fig.7(a). Let {\delta }_1=\Vert \mathit{p}-{\mathit{p}}_3\Vert, {\delta }_3\text{=}\Vert \mathit{q}-{\mathit{p}}_1\Vert, and {\mathit{p}}_{13}={\mathit{p}}_1-{\mathit{p}}_3, where δ₁ represents the distance between the points p and p₃, δ₃ represents the distance between the points q and p₁, and p₁₃ represents the difference between the vectors p₁ and p₃. When \Vert {\mathit{p}}_{13}-{\mathit{v}}_2{\mathit{v}}_2^{\text{T}}{\mathit{p}}_{13}\Vert ={\delta }_1+{\delta }_3 is satisfied, it means that there is a unique solution; otherwise, there is an infinite number of solutions. The position solutions of the two tangent points c and d have the most critical selections. The solution for the position of point c is used as an example, as shown in Fig.8.

Fig.7 Solution of RTR satisfying {\text{ω}}_1\parallel {\text{ω}}_3 and {\text{ω}}_1\mathrm{\perp }{\mathit{v}}_2. (a) Unique solution of category 1, (b) infinite number of solutions of category 1.

Full size|PPT slide

Fig.8 Solution for the position of point c.

Full size|PPT slide

As seen from Fig.8, point c is the tangent point of circle C₃ and twist {\mathit{\xi }}_2. Let \mathit{m}={\mathit{p}}_3-\mathit{p}, the vector m represents the difference between the vectors p₃ and p, and the projection of the vector \mathit{m} on {\mathit{\xi }}_2 is {\mathit{v}}_2{\mathit{v}}_2^{\text{T}}\mathit{m}. Thus, {\mathit{m}}^{\mathbf{\prime }}=\mathit{m}-{\mathit{v}}_2{\mathit{v}}_2^{\text{T}}\mathit{m} can be obtained. Since the tangent point of a line and a circle may exist in two opposite directions, the location of point c is \mathit{c}={\mathit{p}}_3\pm \Vert \mathit{m}\Vert \cdot {\displaystyle \frac{{\mathit{m}}^{\mathbf{\prime }}}{\Vert {\mathit{m}}^{\mathbf{\prime }}\Vert }}. Similarly, the position of point d can be solved and then {\theta }_2 can be solved from {\theta }_2=\Vert \mathit{d}-\mathit{c}\Vert /\phantom{\Vert \mathit{d}-\mathit{c}\Vert \Vert {\mathit{v}}_2\Vert }\Vert {\mathit{v}}_2\Vert. The value of {\theta }_2 is substituted into Eq. (32) to solve {\theta }_3. {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4).

The infinite number of solutions of {\theta }_3 is depicted as the red circular arc segments in Fig.7(b), with the inverse solution of {\theta }_3 at any point on the red circular arc segments.

Category 2: {\mathit{\xi }}_1\nparallel {\mathit{\xi }}_3 when {\mathit{\xi }}_1\mathrm{\perp }{\mathit{\xi }}_2.

This category indicates {\text{ω}}_3^{\text{T}}{\text{ω}}_1\ne {\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_1^{\text{T}}{\mathit{v}}_2=0. Then Eq. (31) is transformed to

(33)x_1\sin {\theta }_3+y_1\cos {\theta }_3+{\mathit{\text{z}}}_1=0.

{\theta }_3 can be solved by combining the above equation and the trigonometric relationship. {\theta }_2 can be solved by substituting the value of {\theta }_3 into Eq. (32). {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4).

Category 3: {\mathit{\xi }}_1\parallel {\mathit{\xi }}_3 when {\mathit{\xi }}_1 is not perpendicular to {\mathit{\xi }}_2.

This category indicates {\text{ω}}_3^{\text{T}}{\widehat{\text{ω}}}_1={\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_1^{\text{T}}{\mathit{v}}_2\ne 0. Then {\theta }_2 can be obtained by {\theta }_2\text{=}-{\mathit{\text{z}}}_1/\phantom{{\mathit{\text{z}}}_1{\lambda }_1}{\lambda }_1. {\theta }_3 can be solved by substituting the value of {\theta }_2 into Eq. (32) and the trigonometric relationship. {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4). If twists {\mathit{\xi }}_1 and {\mathit{\xi }}_3 intersect, the choices of points p₁ and p₃ should not be the point where the twists intersect.

Category 4: {\mathit{\xi }}_1\nparallel {\mathit{\xi }}_3 when {\mathit{\xi }}_1 is not perpendicular to {\mathit{\xi }}_2.

This category indicates {\text{ω}}_3^{\text{T}}{\widehat{\text{ω}}}_1\ne {\mathbf{0}}_{\text{1}\times \text{3}} and {\text{ω}}_1^{\text{T}}{\mathit{v}}_2\ne 0. It is easy to know that the relationship between {\mathit{\xi }}_2 and {\mathit{\xi }}_3 in Eq. (6) is equivalent to the relationship between {\mathit{\xi }}_1 and {\mathit{\xi }}_2 in Eq. (1) for the RTR three-joint subproblem. Therefore, it is likely that {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_3 in this category. If twists {\mathit{\xi }}_1 and {\mathit{\xi }}_3 intersect, the choices of points p₁ and p₃ should not be the point where the twists intersect. {\theta }_2 is replaced with \sin {\theta }_3 and \cos {\theta }_3 in Eq. (31) and substituted into Eq. (32) as

(34)\begin{array}{rl}& a_1{\sin }^2{\theta }_3+a_2{\cos }^2{\theta }_3+a_3\sin {\theta }_3\cos {\theta }_3\\ +& a_4\sin {\theta }_3+a_5\cos {\theta }_3+a_6=0,\end{array}

where a_1={\lambda }_2{x}_1^2-{\lambda }_1{x}_1{x}_3, a_2={\lambda }_2{y}_1^2-{\lambda }_1{y}_1{y}_3, a_3=2{\lambda }_2{x}_1{y}_1- {\lambda }_1{x}_3{y}_1-{\lambda }_1{x}_1{y}_3, a_4={\lambda }_1^2{x}_2+2{\lambda }_2{x}_1{\mathit{\text{z}}}_1-{\lambda }_1{\lambda }_3{x}_1-{\lambda }_1{x}_3{\mathit{\text{z}}}_1, a_5= {\lambda }_1^2{y}_2+2{\lambda }_2{y}_1{\mathit{\text{z}}}_1-{\lambda }_1{\lambda }_3{y}_1-{\lambda }_1{y}_3{\mathit{\text{z}}}_1, a_6={\lambda }_2{\mathit{\text{z}}}_1^2-{\lambda }_1{\lambda }_3{\mathit{\text{z}}}_1+{\lambda }_1^2{\mathit{\text{z}}}_2. Likewise, let t=\tan {\theta }_3/\phantom{{\theta }_{3}2}2. Then \sin {\theta }_3=2t/\phantom{2t\left(1+t^2\right)}\left(1+t^2\right) and \cos {\theta }_3=\left(1-t^2\right)/\phantom{\left(1-t^2\right)\left(1\text{+}{t}^2\right)}\left(1\text{+}{t}^2\right). Those are substituted into Eq. (34) as

(35)m_1{t}^4+m_2{t}^3+m_3{t}^2+m_{4}t+m_5=0,

where m_1=a_2-a_5+a_6, m_2=2\left(a_4-a_3\right), m_3=2\left(2a_1-a_2+a_6\right), m_4=2\left(a_3+a_4\right), m_5=a_2+a_5+a_6. For the above quartic equation, the solution t is easily obtained by adopting Ferrari’s method. The value of {\theta }_3 can be solved as

(36){\theta }_3=2\arctan t.

The value of {\theta }_3 is substituted into Eq. (31) to solve {\theta }_2. Similarly, {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4).

3.4 NAG method realization for the RTT subproblem

For the RTT three-joint subproblem, it is assumed that point p moves along {\mathit{\xi }}_3 by {\theta }_3 to point c and point c moves along {\mathit{\xi }}_2 by {\theta }_2 to point d, as shown in Fig.9. This means that \mathit{c}=\mathit{p}+{\theta }_3{\mathit{v}}_3 and \mathit{d}=\mathit{c}+{\theta }_2{\mathit{v}}_2. It is obvious that points d and q are located on the circle C₁ normal to {\mathit{\xi }}_1. The twist {\mathit{\xi }}_1 passes through point p₁. This is easily accessible according to the geometric and algebraic constraints as

Fig.9 General form of the RTT three-joint subproblem.

Full size|PPT slide

(37){\text{ω}}_1^{\text{T}}\left(\mathit{d}-\mathit{q}\right)=0,

(38)\Vert \mathit{q}-{\mathit{p}}_1\Vert =\Vert \mathit{d}-{\mathit{p}}_1\Vert .

By substituting the expressions for points c and d into Eqs. (37) and (38), respectively, and squaring both sides of Eq. (38), the above two equations are transformed as follows:

(39)x_1{\theta }_2+y_1{\theta }_3+{\mathit{\text{z}}}_1=0,

(40)x_2{{\theta }_2}^2+y_2{{\theta }_3}^2+a_1{\theta }_2{\theta }_3+a_2{\theta }_2+a_3{\theta }_3+{\mathit{\text{z}}}_2=0,

where x_1={\text{ω}}_1^{\text{T}}{\mathit{v}}_2, y_1={\text{ω}}_1^{\text{T}}{\mathit{v}}_3, {\mathit{\text{z}}}_1={\text{ω}}_1^{\text{T}}\left(\mathit{p}-\mathit{q}\right), x_2=y_2=1, a_1=2{\mathit{v}}_3^{\text{T}}{\mathit{v}}_2, a_2=2{\left(\mathit{p}-{\mathit{p}}_1\right)}^{\text{T}}{\mathit{v}}_2, a_3=2{\left(\mathit{p}-{\mathit{p}}_1\right)}^{\text{T}}{\mathit{v}}_3, and {\mathit{\text{z}}}_2= {\Vert \mathit{p}-{\mathit{p}}_1\Vert }^2-{\Vert \mathit{q}-{\mathit{p}}_1\Vert }^2.

For two arbitrary axes, {\text{ω}}_1^{\text{T}}{\text{ω}}_2\text{=}0 holds if {\mathit{\xi }}_1 is perpendicular to {\mathit{\xi }}_2. The RTT three-joint subproblems for special configurations are explained and illustrated considering the characteristics of vertical joint axes.

Category 1: {\mathit{\xi }}_1 is not perpendicular to {\mathit{\xi }}_3 when {\mathit{\xi }}_1\mathrm{\perp }{\mathit{\xi }}_2.

This category indicates {\text{ω}}_1^{\text{T}}{\mathit{v}}_3\ne 0 and {\text{ω}}_1^{\text{T}}{\mathit{v}}_2=0. {\theta }_3 is easily obtained through {\theta }_3=-{\mathit{\text{z}}}_1/\phantom{z_1{y}_1}{y}_1. The value of {\theta }_3 is substituted into Eq. (40) to solve {\theta }_2. The value of {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4).

Category 2: {\mathit{\xi }}_1 is not perpendicular to {\mathit{\xi }}_2 when {\mathit{\xi }}_1\mathrm{\perp }{\mathit{\xi }}_3.

This category indicates {\text{ω}}_1^{\text{T}}{\mathit{v}}_2\ne 0 and {\text{ω}}_1^{\text{T}}{\mathit{v}}_3=0. {\theta }_2 is easily obtained through {\theta }_2=-{\mathit{\text{z}}}_1/\phantom{{\mathit{\text{z}}}_1{x}_1}{x}_1. The value of {\theta }_2 is substituted into Eq. (40) to solve {\theta }_3. The value of {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4).

Category 3: {\mathit{\xi }}_1 is not perpendicular to {\mathit{\xi }}_2 when {\mathit{\xi }}_1 is not perpendicular to {\mathit{\xi }}_3.

This category indicates {\text{ω}}_1^{\text{T}}{\mathit{v}}_2\ne 0 and {\text{ω}}_1^{\text{T}}{\mathit{v}}_3\ne 0. Combining Eqs. (39) and (40), {\theta }_3 is used in place of {\theta }_2 by {\theta }_2=-\left(y_1{\theta }_3+{\mathit{\text{z}}}_1\right)/\phantom{\left(y_1{\theta }_3+{\mathit{\text{z}}}_1\right)x_1}{x}_1 as

(41)m_1{\theta }_3^2+m_2{\theta }_3+m_3=0,

where m_1=x_2{y}_1^2+x_1^2{y}_2-a_1{x}_1{y}_1, m_2=2x_2{y}_1{\mathit{\text{z}}}_1-a_1{x}_1{\mathit{\text{z}}}_1- a_2{x}_1{y}_1+a_3{x}_1^2, and m_3=x_2{\mathit{\text{z}}}_1^2-a_2{x}_1{\mathit{\text{z}}}_1+x_1^2{\mathit{\text{z}}}_2. If m_1\ne 0, then {\theta }_3 can be solved by {\theta }_3=\left(-m_2\pm \sqrt{{m_2}^2-4m_1{m}_3}\right)/\phantom{\left(-m_2\pm \sqrt{{m_2}^2-4m_1{m}_3}\right)\left(2m_1\right)} \left(2m_1\right); if m_1=0, then Eq. (41) is degraded to m_2{\theta }_3+m_3=0. The value of {\theta }_3 is substituted into Eq. (39) to solve {\theta }_2. The value of {\theta }_1 can be solved by combining the value of {\theta }_2, the value of {\theta }_3, and Eq. (4).

Category 4: {\mathit{\xi }}_1\mathrm{\perp }{\mathit{\xi }}_2 when {\mathit{\xi }}_1\mathrm{\perp }{\mathit{\xi }}_3.

This category indicates {\text{ω}}_1^{\text{T}}{\mathit{v}}_2=0 and {\text{ω}}_1^{\text{T}}{\mathit{v}}_3\text{=}0. Points c, d, p, and q are located on one plane, which is perpendicular to the twist {\mathit{\xi }}_1. Therefore, the identity of Eq. (37) is always established. It is not possible to solve for the values of {\theta }_2 and {\theta }_3 by relying only on Eq. (40). The form of an infinite number of solutions is shown in Fig.10. The arbitrary point on the red circle C₁ is the solution of the inverse kinematics.

Fig.10 Solution of RTT satisfying {\text{ω}}_1\mathrm{\perp }{\mathit{v}}_2 and {\text{ω}}_1\mathrm{\perp }{\mathit{v}}_3.

Full size|PPT slide

3.5 NAG method realization for the TRT subproblem

For the TRT three-joint subproblem, it is assumed that point p moves along {\mathit{\xi }}_3 by {\theta }_3 to point c and point q moves along {\mathit{\xi }}_1 by -{\theta }_1 to point d, as shown in Fig.11. This means that \mathit{c}=\mathit{p}+{\theta }_3{\mathit{v}}_3 and \mathit{d}=\mathit{q}-{\theta }_1{\mathit{v}}_1. It is obvious that points c and d are located on the circle C₂ normal to {\mathit{\xi }}_2. The twist {\mathit{\xi }}_2 passes through point p₂. This is easily accessible according to the geometric and algebraic constraints as

Fig.11 General form of the TRT three-joint subproblem.

Full size|PPT slide

(42){\text{ω}}_2^{\text{T}}\left(\mathit{c}-\mathit{d}\right)=0,

(43)\Vert \mathit{c}-{\mathit{p}}_2\Vert =\Vert \mathit{d}-{\mathit{p}}_2\Vert .

By substituting the expressions for points c and d into Eqs. (42) and (43), respectively, and squaring both sides of Eq. (43), the above two equations are transformed as follows:

(44)x_1{\theta }_1+y_1{\theta }_3+{\mathit{\text{z}}}_1=0,

(45)x_2{{\theta }_1}^2+y_2{{\theta }_3}^2+a_1{\theta }_1+a_2{\theta }_3+{\mathit{\text{z}}}_2=0,

where x_1={\text{ω}}_2^{\text{T}}{\mathit{v}}_1, y_1={\text{ω}}_2^{\text{T}}{\mathit{v}}_3, {\mathit{\text{z}}}_1={\text{ω}}_2^{\text{T}}\left(\mathit{p}-\mathit{q}\right), x_2=-1, y_2=1, a_1=2{\left(\mathit{q}-{\mathit{p}}_2\right)}^{\text{T}}{\mathit{v}}_1, a_2=2{\left(\mathit{p}-{\mathit{p}}_2\right)}^{\text{T}}{\mathit{v}}_3, and {\mathit{\text{z}}}_2={\Vert \mathit{p}-{\mathit{p}}_2\Vert }^2- {\Vert \mathit{q}-{\mathit{p}}_2\Vert }^2.

For two arbitrary axes, {\text{ω}}_1^{\text{T}}{\text{ω}}_2\text{=}0 holds if {\mathit{\xi }}_1 is perpendicular to {\mathit{\xi }}_2. The TRT three-joint subproblems for special configurations are explained and illustrated considering the characteristics of vertical joint axes.

Category 1: {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_3 when {\mathit{\xi }}_2\mathrm{\perp }{\mathit{\xi }}_1.

This category indicates {\text{ω}}_2^{\text{T}}{\mathit{v}}_3\ne 0 and {\text{ω}}_2^{\text{T}}{\mathit{v}}_1=0. {\theta }_3 is easily obtained through {\theta }_3=-{\mathit{\text{z}}}_1/\phantom{{\mathit{\text{z}}}_1{y}_1}{y}_1. The value of {\theta }_3 is substituted into Eq. (45) to solve {\theta }_1. The value of {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 2: {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_1 when {\mathit{\xi }}_2\mathrm{\perp }{\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\mathit{v}}_1\ne 0 and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3=0. {\theta }_1 is easily obtained through {\theta }_1=-{\mathit{\text{z}}}_1/\phantom{{\mathit{\text{z}}}_1{x}_1}{x}_1. The value of {\theta }_1 is substituted into Eq. (45) to solve {\theta }_3. The value of {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 3: {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_1 when {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\mathit{v}}_1\ne 0 and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3\ne 0. Combining Eqs. (44) and (45), {\theta }_3 is used in place of {\theta }_1 by {\theta }_1=-\left(y_1{\theta }_3+{\mathit{\text{z}}}_1\right)/\phantom{\left(y_1{\theta }_3+{\mathit{\text{z}}}_1\right)x_1}{x}_1 as

(46)m_1{{\theta }_3}^2+m_2{\theta }_3+m_3=0,

where m_1=x_2{y}_1^2+x_1^2{y}_2, m_2=2x_2{y}_1{\mathit{\text{z}}}_1-a_1{x}_1{y}_1+a_2{x}_1^2, and m_3=x_2{\mathit{\text{z}}}_1^2-a_1{x}_1{\mathit{\text{z}}}_1+x_1^2{\mathit{\text{z}}}_2. If m_1\ne 0, then {\theta }_3 can be solved by {\theta }_3=\left(-m_2\pm \sqrt{m_2^2-4m_1{m}_3}\right)/\phantom{\left(-m_2\pm \sqrt{m_2^2-4m_1{m}_3}\right)\left(2m_1\right)}\left(2m_1\right); on the contrary, if m_1=0, then Eq. (46) is degraded to m_2{\theta }_3+m_3=0. The value of {\theta }_3 is substituted into Eq. (44) to solve {\theta }_1. The value of {\theta }_2 can be solved by combining the value of {\theta }_1, the value of {\theta }_3, and Eq. (4).

Category 4: {\mathit{\xi }}_2\mathrm{\perp }{\mathit{\xi }}_1 when {\mathit{\xi }}_2\mathrm{\perp }{\mathit{\xi }}_3.

This category indicates {\text{ω}}_2^{\text{T}}{\mathit{v}}_1=0 and {\text{ω}}_2^{\text{T}}{\mathit{v}}_3=0. Points c, d, p, and q are located on one plane, which is perpendicular to the twist {\mathit{\xi }}_2. Therefore, the identity of Eq. (42) is always established. It is not possible to identify the values of {\theta }_1 and {\theta }_3 by relying only on Eq. (45). The form of the infinite number of solutions is shown in Fig.12. The arbitrary point on the red line segments is the inverse kinematics solution of {\theta }_3.

Fig.12 Solution of TRT satisfying {\text{ω}}_2\mathrm{\perp }{\mathit{v}}_1 and {\text{ω}}_2\mathrm{\perp }{\mathit{v}}_3.

Full size|PPT slide

3.6 NAG method realization for the TTT subproblem

For the TRT three-joint subproblem, it is assumed that point p moves along {\mathit{\xi }}_3 by {\theta }_3 to point c, point q moves along {\mathit{\xi }}_2 by {\theta }_2 to point d, and point d moves along {\mathit{\xi }}_1 by {\theta }_1 to point q, as shown in Fig.13. Hence, it is easy to obtain the relationship equation between points p and q as

Fig.13 General form of the TTT three-joint subproblem.

Full size|PPT slide

(47)\mathit{q}=\mathit{p}+{\theta }_1{\mathit{v}}_1+{\theta }_2{\mathit{v}}_2+{\theta }_3{\mathit{v}}_3.

Equation (47) can be converted into the matrix form as

(48)\mathit{V}\mathit{\theta }=\mathit{Q},

where \mathit{V}=\left[\begin{array}{ccc}{\mathit{v}}_1& {\mathit{v}}_2& {\mathit{v}}_3\end{array}\right], \mathit{\theta }={\left[\begin{array}{ccc}{\theta }_1& {\theta }_2& {\theta }_3\end{array}\right]}^{\text{T}}, \mathit{Q}=\mathit{q}-\mathit{p}, and Q represents the difference between the vectors q and p. The precondition for Eq. (48) to have a unique solution is that \mathit{V} is full-rank, that is, {\mathit{\xi }}_1, {\mathit{\xi }}_2, and {\mathit{\xi }}_3 are linearly independent of one another. The solution of \mathit{\theta } is \mathit{\theta }={\mathit{V}}^{-1}\mathit{Q}. If two axes are parallel, then \mathit{V} is not full-rank and there is an infinite number of solutions.

4 Results

To validate the effectiveness and practicability of the proposed NAG methods, the inverse kinematics simulations of arbitrary three-joint subproblems were implemented, and the simulation and master–slave tracking experiment based on the Huaque III minimally invasive surgical robot were carried out. Simulations were executed in MATLAB R2016b. According to the given joint motion ranges of each subproblem, 51 points were uniformly sampled to calculate the positions of the end-effector and then the NAG method was applied to calculate the joint angle values. The symbol x represents 51 integers from 1 to 51. In the simulation diagrams of joint inverse solutions, the symbol “o” represents the sample points of each joint while the symbol “+” represents the joint angles of the inverse solution. Note that the joint direction vectors need to be standardized before kinematic calculations can begin. There are several sets of joint angles of inverse solutions in simulations based on the NAG methods. The solutions that joint angles are in the angle ranges and the end posture is consistent with expected posture are screened. Then, the joint angles of inverse solutions are output and compared with sample points.

4.1 Simulations of arbitrary three-joint subproblems

4.1.1 Simulation of RRR subproblem

For the simulation of the RRR subproblem, category 4 is taken as an example to verify effectiveness of the NAG method. Three joints satisfy that {\mathit{\xi }}_2 is neither parallel to {\mathit{\xi }}_1 nor parallel to {\mathit{\xi }}_3. In this case, the three joint axes do not intersect one another. The directions of the three joint axes are {\text{ω}}_1=[\begin{array}{ccc}0& 0& 1\end{array}{]}^{\text{T}}, {\text{ω}}_2=[\begin{array}{ccc}0& 1& 1\end{array}{]}^{\text{T}}, and {\text{ω}}_3=[\begin{array}{ccc}2& 1& 0\end{array}{]}^{\text{T}}. The positions of the three reference points are {\mathit{p}}_1=[\begin{array}{ccc}1& 0& 0\end{array}{]}^{\text{T}}, {\mathit{p}}_2=[\begin{array}{ccc}5& 0& 0\end{array}{]}^{\text{T}}, and {\mathit{p}}_3=[\begin{array}{ccc}0& 5& 0\end{array}{]}^{\text{T}}. The position of initial point p is \mathit{p}=[\begin{array}{ccc}5& -10& -10\end{array}{]}^{\text{T}}. The movement ranges of the three joints are determined as {\theta }_1={\displaystyle \frac{2\text{π}}{5}}\cos {\displaystyle \frac{x\text{π}}{25}}, {\theta }_2={\displaystyle \frac{\text{π}}{3}}\cos {\displaystyle \frac{x\text{π}}{25}}, and {\theta }_3={\displaystyle \frac{4\text{π}}{9}}\cos \left({\displaystyle \frac{x\text{π}}{25}}-\text{π}\right). The sample points and joint angles of the inverse solution are shown in Fig.14. Clearly, the joint angles of the inverse solution are coincident with the sample points.

Fig.14 Sample points and joint angles of the inverse solution of RRR subproblem.

Full size|PPT slide

4.1.2 Simulation of the RRT subproblem

For the simulation of the RRT subproblem, category 4 is taken as an example to verify the effectiveness of the NAG method. Three joints satisfy that {\mathit{\xi }}_2 is neither parallel to {\mathit{\xi }}_1 nor perpendicular to {\mathit{\xi }}_3. The directions of the three joint axes are {\text{ω}}_1=[\begin{array}{ccc}1& 0& 1\end{array}{]}^{\text{T}}, {\text{ω}}_2=[\begin{array}{ccc}0& 1& 1\end{array}{]}^{\text{T}}, and {\mathit{v}}_3=[\begin{array}{ccc}2& 1& 0\end{array}{]}^{\text{T}}. The positions of two reference points are {\mathit{p}}_1=[\begin{array}{ccc}0& 0& 0\end{array}{]}^{\text{T}} and {\mathit{p}}_2=[\begin{array}{ccc}5& 0& 0\end{array}{]}^{\text{T}}. The position of the initial point p is \mathit{p}=[\begin{array}{ccc}5& -10& -5\end{array}{]}^{\text{T}}. The movement ranges of the three joints are determined as {\theta }_1={\displaystyle \frac{2\text{π}}{5}}\cos {\displaystyle \frac{x\text{π}}{25}}, {\theta }_2={\displaystyle \frac{\text{π}}{3}}\cos {\displaystyle \frac{x\text{π}}{25}}, and {\theta }_3=50\cos \left({\displaystyle \frac{x\text{π}}{25}}-\text{π}\right). The sample points and joint angles of the inverse solution are shown in Fig.15. It is easy to know that the sample points and the joint angles of the inverse solution are coincident.

Fig.15 Sample points and joint angles of the inverse solution of RRT subproblem.

Full size|PPT slide

4.1.3 Simulation of the RTR subproblem

For the simulation of the RTR subproblem, category 4 is taken as an example to verify the effectiveness of the NAG method. Three joints satisfy that {\mathit{\xi }}_2 is neither parallel to {\mathit{\xi }}_1 nor perpendicular to {\mathit{\xi }}_3. The directions of three joint axes are {\text{ω}}_1=[\begin{array}{ccc}1& 0& 1\end{array}{]}^{\text{T}}, {\mathit{v}}_2=[\begin{array}{ccc}0& 1& 1\end{array}{]}^{\text{T}}, and {\text{ω}}_3=[\begin{array}{ccc}2& 1& 0\end{array}{]}^{\text{T}}. The positions of two reference points are {\mathit{p}}_1=[\begin{array}{ccc}0& 0& 0\end{array}{]}^{\text{T}} and {\mathit{p}}_3=[\begin{array}{ccc}0& 5& 0\end{array}{]}^{\text{T}}. The position of initial point p is \mathit{p}=[\begin{array}{ccc}5& -10& -5\end{array}{]}^{\text{T}}. The movement ranges of the three joints are determined as {\theta }_1={\displaystyle \frac{2\text{π}}{5}}\cos {\displaystyle \frac{x\text{π}}{25}}, {\theta }_2=50\cos \left({\displaystyle \frac{x\text{π}}{25}}-\text{π}\right), and {\theta }_3={\displaystyle \frac{\text{π}}{3}}\cos {\displaystyle \frac{x\text{π}}{25}}. The sample points and joint angles of the inverse solution are shown in Fig.16. Clearly, the sample points and joint angles of the inverse solution are coincident.

Fig.16 Sample points and joint angles of the inverse solution of RTR subproblem.

Full size|PPT slide

4.1.4 Simulation of the RTT subproblem

For the simulation of the RTT subproblem, category 3 is taken as an example to verify the effectiveness of the NAG method. Three joints satisfy that {\mathit{\xi }}_2 is neither parallel to {\mathit{\xi }}_1 nor perpendicular to {\mathit{\xi }}_3. The directions of the three joint axes are {\text{ω}}_1=[\begin{array}{ccc}1& 0& 1\end{array}{]}^{\text{T}}, {\mathit{v}}_2=[\begin{array}{ccc}0& 1& 1\end{array}{]}^{\text{T}}, and {\mathit{v}}_3=[\begin{array}{ccc}2& 1& 0\end{array}{]}^{\text{T}}. The position of reference points is {\mathit{p}}_1=[\begin{array}{ccc}0& 0& 0\end{array}{]}^{\text{T}}. The position of the initial point p is \mathit{p}=[\begin{array}{ccc}5& -10& -5\end{array}{]}^{\text{T}}. The movement ranges of the three joints are determined as {\theta }_1={\displaystyle \frac{2\text{π}}{5}}\cos {\displaystyle \frac{x\text{π}}{25}}, {\theta }_2=50\cos \left({\displaystyle \frac{x\text{π}}{25}}-\text{π}\right), and {\theta }_3=50\cos {\displaystyle \frac{x\text{π}}{25}}. The sample points and joint angles of the inverse solution are shown in Fig.17. Apparently, the joint angles of the inverse solution are coincident with the sample points.

Fig.17 Sample points and joint angles of the inverse solution of RTT subproblem.

Full size|PPT slide

4.1.5 Simulation of the TRT subproblem

For the simulation of the TRT subproblem, category 3 is taken as an example to verify the effectiveness of the NAG method. Three joints satisfy that {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_1 when {\mathit{\xi }}_2 is not perpendicular to {\mathit{\xi }}_3. The directions of three joint axes are {\mathit{v}}_1=[\begin{array}{ccc}1& 0& 1\end{array}{]}^{\text{T}}, {\text{ω}}_2=[\begin{array}{ccc}0& 1& 1\end{array}{]}^{\text{T}}, and {\mathit{v}}_3=[\begin{array}{ccc}2& 1& 0\end{array}{]}^{\text{T}}. The position of reference points is {\mathit{p}}_2=[\begin{array}{ccc}5& 0& 0\end{array}{]}^{\text{T}}. The position of initial point p is \mathit{p}=[\begin{array}{ccc}5& -10& -5\end{array}{]}^{\text{T}}. The movement ranges of the three joints are determined as {\theta }_1=50\cos \left({\displaystyle \frac{x\text{π}}{25}}-\text{π}\right), {\theta }_2={\displaystyle \frac{2\text{π}}{5}}\cos {\displaystyle \frac{x\text{π}}{25}}, and {\theta }_3=50\cos {\displaystyle \frac{x\text{π}}{25}}. The sample points and joint angles of the inverse solution are shown in Fig.18. Obviously, the joint angles of the inverse solution and the sample points remain coincident.

Fig.18 Sample points and joint angles of the inverse solution of TRT subproblem.

Full size|PPT slide

4.1.6 Simulation of the TTT subproblem

For the simulation of the TTT subproblem, three joints are linearly independent of one another. The directions of the three joint axes are {\mathit{v}}_1=[\begin{array}{ccc}1& 0& 1\end{array}{]}^{\text{T}}, {\mathit{v}}_2=[\begin{array}{ccc}0& 1& 1\end{array}{]}^{\text{T}}, and {\mathit{v}}_3=[\begin{array}{ccc}2& 1& 0\end{array}{]}^{\text{T}}. The position of the initial point p is \mathit{p}=[\begin{array}{ccc}5& -10& -5\end{array}{]}^{\text{T}}. The movement ranges of the three joints are determined as {\theta }_1=50\cos \left({\displaystyle \frac{x\text{π}}{25}}-\text{π}\right), {\theta }_2=25\cos {\displaystyle \frac{x\text{π}}{25}}, and {\theta }_3=50\cos {\displaystyle \frac{x\text{π}}{25}}. The sample points and joint angles of the inverse solution are shown in Fig.19. Distinctly, the joint angles of the inverse solution are coincident with the sample points.

Fig.19 Sample points and joint angles of the inverse solution of TTT subproblem.

Full size|PPT slide

4.2 Simulation of 6-DOF problem based on Huaque III surgical robot

Huaque III minimally invasive surgical robot [30] includes two instrument manipulators and one endoscopy manipulator. The instrument manipulator consists of passive joints, active joints, and a surgical instrument, as shown in Fig.20. Passive joints are used to locate the remote center of motion (RCM) at any point in space. Active joints are used to achieve rotation and translational motion around the RCM point. Surgical instrument coordinates with active joints to adjust the position and posture of the end-effector. Note that three joint axes of instrument do not intersect at one point, that is, it does not satisfy the Pieper criterion. Joints 7 and 8 are coaxial, and joints 7 and 8 are prismatic and rotary joints, respectively. Two joints can be exchanged on the premise that the order of interchanging joints does not affect the end-effector pose [31]. To conveniently solve the inverse solution, joints 7 and 8 are swapped in turn. The screw representation and reference coordinate points of each joint axis are as follows:

Fig.20 Structure diagram of the instrument manipulator of Huaque III surgical robot.

Full size|PPT slide

(49)\begin{array}{rl}& {\text{ω}}_1={\text{ω}}_8=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right],{\text{ω}}_2={\text{ω}}_3={\text{ω}}_4=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],{\text{ω}}_5=\left[\begin{array}{c}\cos {\alpha }_4\\ 0\\ -\sin {\alpha }_4\end{array}\right],\\ & {\text{ω}}_6={\text{ω}}_9=\left[\begin{array}{c}0\\ -1\\ 0\end{array}\right],{\text{ω}}_7=\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right],{\text{ω}}_{10}=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],\end{array}

(50)\begin{array}{rl}& {\mathit{r}}_2=\left[\begin{array}{c}0\\ 0\\ d_1\end{array}\right],{\mathit{r}}_3=\left[\begin{array}{c}{a}_2\\ 0\\ d_1\end{array}\right],{\mathit{r}}_4=\left[\begin{array}{c}{a}_2+a_3\\ 0\\ d_1\end{array}\right],\\ & {\mathit{r}}_5={\mathit{r}}_6={\mathit{r}}_7={\mathit{r}}_9=\left[\begin{array}{c}{a}_2+a_3+d_5\cos {\alpha }_4\\ 0\\ d_1-d_5\sin {\alpha }_4\end{array}\right],\\ & {\mathit{r}}_{10}=\left[\begin{array}{c}{a}_2+a_3+d_5\cos {\alpha }_4\\ 0\\ d_1-d_5\sin {\alpha }_4-a_9\end{array}\right].\end{array}

The forward kinematics solution of the Huaque III surgical instrument end-effector is

(51){\mathit{g}}_{\text{st}}\left(\mathit{\theta }\right)=\prod _{i=1}^{10}{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}{\mathit{g}}_{\text{st}}\left(\mathbf{0}\right),

where {\mathit{g}}_{\text{st}}(\mathbf{0}) and {\mathit{g}}_{\text{st}}(\mathit{\theta }) respectively represent the initial transformation matrix and transformation matrix at different joint angles of the tool frame. The passive joint remains stationary during the operation, and Eq. (51) can be transformed as

(52){\left(\prod _{i=1}^4{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}\right)}^{-1}{\mathit{g}}_{\text{st}}\left(\mathit{\theta }\right){\mathit{g}}_{\text{st}}^{-1}\left(\mathbf{0}\right)=\prod _{i=5}^{10}{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}.

Let {\mathit{g}}_1={\left({\displaystyle \prod _{i=1}^4{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}}\right)}^{-1}{\mathit{g}}_{\text{st}}\left(\mathit{\theta }\right){\mathit{g}}_{\text{st}}^{-1}\left(\mathbf{0}\right). Thus, the above formula is converted as

(53){\mathit{g}}_1{\left(\prod _{i=8}^{10}{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}\right)}^{-1}=\prod _{i=5}^7{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}.

Joints 5, 6, and 7 are all rotary joints, and their axes converge at a point. The homogeneous coordinate vector of the intersection point is multiplied on both sides of the above equation by right, and according to the position preservation principle, the equation can be obtained as

(54)\left(\prod _{i=8}^{10}{\text{e}}^{{\hat{\mathit{\xi }}}_i{\theta }_i}\right){\mathit{g}}_1^{-1}{\stackrel{\mathbf{~}}{\mathit{p}}}_5={\stackrel{\mathbf{~}}{\mathit{p}}}_5,

where {\stackrel{\mathbf{~}}{\mathit{p}}}_5=[\begin{array}{cc}{\mathit{r}}_5^{\text{T}}& 1\end{array}{]}^{\text{T}}. r₅ and {\stackrel{\mathbf{~}}{\mathit{p}}}_5 represent the position vector and homogeneous coordinate of the point p₅. Let {\stackrel{\mathbf{~}}{\mathit{q}}}_1={\mathit{g}}_1^{-1}{\stackrel{\mathbf{~}}{\mathit{p}}}_5, then {\theta }_8, {\theta }_9, and {\theta }_{10} can be solved by the NAG method for the RRT subproblem. Next, {\theta }_5, {\theta }_6, and {\theta }_7 can be solved by the NAG method for the RRR subproblem by substituting the joint angles of {\theta }_8, {\theta }_9, and {\theta }_{10} into Eq. (53). The parameter values of the minimally invasive surgical robot are d_1=1000 mm, a_2=500 mm, a_3= 500 mm, {\alpha }_4=\text{π}/\phantom{\text{π}4}4, d_5=800 mm, and a_9=10 mm. The angles of the passive joints are constant as {\theta }_1=200, {\theta }_2=\text{π}/\phantom{\text{π}9}9, {\theta }_3=-\text{π}/\phantom{\text{π}18}18, and {\theta }_4=-\text{π}/\phantom{\text{π}18}18. The motion ranges of the joints are {\theta }_5\in \left[-\text{π}/\phantom{\text{π}2},2,\text{π}/\phantom{\text{π}2}2\right], {\theta }_6\in \left[-\text{π}/\phantom{\text{π}3,}3,\text{π}/\phantom{\text{π}3}3\right], {\theta }_7\in \left[-\text{π},\text{π}\right], {\theta }_8\in \left[100,350\right] mm, {\theta }_9\in \left(-\text{π}/\phantom{\text{π}2,}2,\text{π}/\phantom{\text{π}2}2\right), and {\theta }_{10}\in \left(-\text{π}/\phantom{\text{π}2,}2,\text{π}/\phantom{\text{π}2}2\right). The motion curves of the other joints are {\theta }_5={\displaystyle \frac{\text{π}}{2}}\sin {\displaystyle \frac{x\text{π}}{25}}, {\theta }_6={\displaystyle \frac{\text{π}}{3}}\sin {\displaystyle \frac{\left(x-25\right)\text{π}}{25}}, {\theta }_7={\displaystyle \frac{2\text{π}}{9}}\cdot \sin {\displaystyle \frac{\left(x-25\right)\text{π}}{25}}, {\theta }_8=4\left(x-1\right)\text{+}100, {\theta }_9={\displaystyle \frac{4\text{π}}{9}}\sin {\displaystyle \frac{x\text{π}}{25}}, and {\theta }_{10}={\displaystyle \frac{2\text{π}}{9}}\sin {\displaystyle \frac{x\text{π}}{25}}. The sample points and joint angles of the inverse solution are shown in Fig.21. It is evident that the joint angles of the inverse solution are coincident with the sample points.

Fig.21 Sample points and joint angles of the inverse solution for Huaque III surgical robot.

Full size|PPT slide

4.3 Master–slave tracking experiment based on the Huaque III surgical robot

The surgeon operates the master manipulator on the master console to control the slave manipulator and perform a series of surgical operations (Fig.22). The two-stage approximation and compensation method [5] has relatively higher accuracy and faster compensation speed, and thus the compensation method is chosen as a contrast to the closed-form solution for the Huaque III surgical robot. The master manipulator is moved in a random curve trajectory, and the movement trajectory from the slave manipulator is recorded based on two methods. The trajectories of the master and instrument manipulators are compared in the situation where the master manipulator trajectories have been reduced to one-fourth and their tracking errors are calculated, as shown in Fig.23 and Fig.24. In Fig.23(a) and 23(b), the incremental ranges of the slave manipulator trajectory are x\in \left(-15,15\right) mm, y\in \left(-10,30\right) mm, and \mathit{\text{z}}\in \left(-10,20\right) mm for the closed-form solution, while the incremental ranges of the slave manipulator trajectory are x\in \left(-20,30\right) mm, y\in \left(-15,5\right) mm, and \mathit{\text{z}}\in \left(-15,20\right) mm for the compensation method. The motion ranges of the two methods are close. Additionally, the trajectories of the slave manipulator are very close to those of the master manipulator. The end-effector tracking errors of master and slave manipulators are very small. The mean and maximum errors of the master–slave tracking experiment are 0.1486 and 0.4777 mm, respectively, for the closed-form solution, while the mean and maximum errors are 0.3188 and 0.6394 mm, respectively, for the compensation method. The mean and maximum tracking errors of the closed-form solution are 53.59% and 25.29% less than that of the compensation method. The maximum tracking errors are less than the absolute positioning accuracy of the robot at 1.4 mm, which meets the control accuracy requirements of the master–slave tracking of the surgical robot.

Fig.22 Huaque III minimally invasive surgical robot.

Full size|PPT slide

Fig.23 Movement trajectories for two methods: (a) closed-form solution and (b) compensation solution.

Full size|PPT slide

Fig.24 End-effector tracking errors for two methods: (a) closed-form solution and (b) compensation solution.

Full size|PPT slide

5 Conclusions

In this paper, the NAG methods are presented, and on their basis, the general closed-form inverse kinematics for all types of three-joint subproblems are obtained. The preconditions consisting of the geometric and algebraic constraints are applied to solve the inverse kinematics of three-joint subproblems. Special postures (parallel, perpendicular, and intersect) of three-joint subproblems are also discussed and illustrated. The inverse kinematics of arbitrary three-joint subproblems in an arbitrary postural relationship can be solved by the NAG methods. The inverse kinematics simulations of all three-joint subproblems are implemented, and the inverse solutions are consistent with the given joint angles. Simulation results validate the general closed-form inverse kinematics. The simulation and master–slave tracking experiment are carried out to verify the NAG methods of the RRR and RRT subproblems based on the experimental platform of the Huaque III minimally invasive surgical robot. The simulation result shows that the inverse solutions are consistent with six sets given joint angles. The mean and maximum of the master–slave tracking experiment for the closed-form solution are 0.1486 and 0.4777 mm, respectively, while the mean and maximum of the master–slave tracking experiment for the compensation method are 0.3188 and 0.6394 mm, respectively. The mean and maximum tracking errors of the closed-form solution are 53.59% and 25.29% less than those of the compensation method. Clearly, the closed-form solution is superior to the compensation method. The general closed-form inverse kinematics are validated according to the simulation and experiment results. The arbitrary three-joint subproblems expand the Paden–Kahan subproblems, and the proposed general closed-form inverse kinematics can be combined and widely applied in series, parallel, reconfigurable, and other types of robots.

Nomenclature

Abbreviations
D–H	Denavit–Harbenterg
DOF	Degree-of-freedom
NAG	Novel algebraic geometric
POE	Product of exponential
R, T	revolute joint and translational joint, respectively
RCM	Remote center of motion
Variables
c, d, p, p1, p2, p3, and q	Position vectors of points c, d, p, p1, p2, p3, and q, respectively
{\mathit{g}}_{\text{st}}(\mathbf{0}), {\mathit{g}}_{\text{st}}(\mathit{\theta })	Initial transformation matrix and transformation matrix at different joint angles of the tool frame, respectively
lc2	Distance between points c and p2
lij (i, j = 1, 2, 3)	Distance between points pi and pj
m	Difference between the vectors p3 and p
{\stackrel{\mathbf{~}}{\mathit{p}}}_5	Homogeneous coordinate of point p5
\stackrel{\mathbf{~}}{\mathit{q}}, \stackrel{\mathbf{~}}{\mathit{p}}	Homogeneous coordinate of points q and p, respectively
\mathit{r}	Position vector of the reference point r of the revolute joint axis
r5	Position vector of point p5
\mathit{v}	Unit directional vector of the translational joint axis
{\theta }_i	Generalized angle of the ith joint
δ	Distance between two vectors s and t, δ = ||s − t||
\text{ω}	Unit directional vector of the revolute joint axis
\widehat{\text{ω}}	Skew-symmetric matrices of \text{ω}
\mathit{\xi }	Joint twist
{\mathit{\xi }}_i \left(i=1,2,3\right)	Twist coordinate of the ith joint axis
{\hat{\mathit{\xi }}}_i \left(i=1,2,3\right)	Instantaneous joint twist of the ith joint axis

Acknowledgements

This study was supported by the Foundation for Innovative Research Groups of the National Natural Science Foundation of China (Grant No. 51521003), the National Natural Science Foundation of China (Grant No. 61803341), and the Self-planned Task of State Key Laboratory of Robotics and System (Harbin Institute of Technology) (Grant No. SKLRS202009B). No conflicts of interest exist in this paper.

References

1 Husty M L , Pfurner M , Schröcker H P . A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator. Mechanism and Machine Theory, 2007, 42( 1): 66– 81

2 Murray R M Li Z X Sastry S S. A Mathematical Introduction to Robotic Manipulation. Boca Raton: CRC Press, 1994

3 Niu G J , Pan B , Ai Y , Fu Y L . Intuitive control algorithm of a novel minimally invasive surgical robot. Computer Assisted Surgery, 2016, 21( sup1): 92– 101

4 Stoica A , Pisla D , Andras S , Gherman B , Gyurka B Z , Plitea N . Kinematic, workspace and singularity analysis of a new parallel robot used in minimally invasive surgery. Frontiers of Mechanical Engineering, 2013, 8( 1): 70– 79

5 Tang A L. Research on the teleoperation motion control strategy for a master-slave minimally invasive surgical robot. Dissertation for the Doctoral Degree. Shanghai: Shanghai Jiao Tong University, 2014 (in Chinese)

6 Zhang F , Teng S , Wang Y F , Guo Z J , Wang J J , Xu R L . Design of bionic goat quadruped robot mechanism and walking gait planning. International Journal of Agricultural and Biological Engineering, 2020, 13( 5): 32– 39

7 Lakhal O , Melingui A , Merzouki R . Hybrid approach for modeling and solving of kinematics of a compact bionic handling assistant manipulator. IEEE/ASME Transactions on Mechatronics, 2016, 21( 3): 1326– 1335

8 Yang H , Fang H R , Fang Y F , Li X Y . Dimensional synthesis of a novel 5-DOF reconfigurable hybrid perfusion manipulator for large-scale spherical honeycomb perfusion. Frontiers of Mechanical Engineering, 2021, 16( 1): 46– 60

9 Zhang W X , Lu S N , Ding X L . Recent development on innovation design of reconfigurable mechanisms in China. Frontiers of Mechanical Engineering, 2019, 14( 1): 15– 20

10 Kim J , Jie W , Kim H , Lee M C . Modified configuration control with potential field for inverse kinematics solution of redundant manipulator. IEEE/ASME Transactions on Mechatronics, 2021, 26( 4): 1782– 1790

11 Patil A , Kulkarni M , Aswale A . Analysis of the inverse kinematics for 5 DOF robot arm using D–H parameters. In: Proceedings of 2017 IEEE International Conference on Real-time Computing and Robotics (RCAR). Okinawa: IEEE, 2017, 688– 693

12 Xu J , Song K C , He Y , Dong Z P , Yan Y H . Inverse kinematics for 6-DOF serial manipulators with offset or reduced wrists via a hierarchical iterative algorithm. IEEE Access: Practical Innovations, Open Solutions, 2018, 6 : 52899– 52910

13 Soleimani Amiri M , Ramli R . Intelligent trajectory tracking behavior of a multi-joint robotic arm via genetic–swarm optimization for the inverse kinematic solution. Sensors, 2021, 21( 9): 3171

14 Zhou H B , Zhou S , Yu J , Zhang Z D , Liu Z Z . Trajectory optimization of pickup manipulator in obstacle environment based on improved artificial potential field method. Applied sciences, 2020, 10( 3): 935

15 Tian X M , Xu Y . Low delay control algorithm of robot arm for minimally invasive medical surgery. IEEE Access: Practical Innovations, Open Solutions, 2020, 8 : 93548– 93560

16 Xu Z H , Li S , Zhou X F , Yan W , Cheng T B , Huang D . Dynamic neural networks based kinematic control for redundant manipulators with model uncertainties. Neurocomputing, 2019, 329 : 255– 266

17 Gallardo-Alvarado J , Aguilar-Nájera C R , Casique-Rosas L , Rico-Martínez J M , Islam M N . Kinematics and dynamics of 2(3-RPS) manipulators by means of screw theory and the principle of virtual work. Mechanism and Machine Theory, 2008, 43( 10): 1281– 1294

18 Hayati S A . Robot arm geometric link parameter estimation. In: Proceedings of the 22nd IEEE Conference on Decision & Control. San Antonio: IEEE, 1983, 1477– 1483

19 Zhao R B Shi Z P Guan Y Shao Z Z Zhang Q Y Wang G H. Inverse kinematic solution of 6R robot manipulators based on screw theory and the Paden–Kahan subproblem. International Journal of Advanced Robotic Systems, 2018, 15(6): 1729881418818297

20 Paden B E. Kinematics and control of robot manipulators. Dissertation for the Doctoral Degree. Berkeley: University of California, 1985

21 Chen I M , Gao Y . Closed-form inverse kinematics solver for reconfigurable robots. In: Proceedings of 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164). Seoul: IEEE, 2001, 3 : 2395– 2400

22 Zhao J , Wang W D , Gao Y S , Cai H G . Generation of closed-form inverse kinematics for reconfigurable robots. Frontiers of Mechanical Engineering in China, 2008, 3( 1): 91– 96

23 Tan Y S Cheng P L Xiao A P. Solution for a new sub-problem in screw theory and its’ application in the inverse kinematics of a manipulator. Applied Mechanics and Materials, 2010, 34–35: 271– 275

24 Leoro J , Hsiao T S , Betancourt C . A new geometric subproblem to extend solvability of inverse kinematics based on screw theory for 6R robot manipulators. International Journal of Control, Automation, and Systems, 2021, 19( 1): 562– 573

25 Chen Q C , Zhu S Q , Zhang X Q . Improved inverse kinematics algorithm using screw theory for a six-DOF robot manipulator. International Journal of Advanced Robotic Systems, 2015, 12( 10): 140

26 Li W W , Zhou G B , Zhou X F , Chen Z L , Wu L , Zeng S F . Kinematics analysis of generalized SRU manipulator based on screw theory. In: Proceedings of 2018 the 3rd International Conference on Robotics and Automation Engineering (ICRAE). Guangzhou: IEEE, 2018, 160– 165

27 Wang H X Lu X Zhang Z Y Li Y X Sheng C Y Gao L. A novel second subproblem for two arbitrary axes of robots. International Journal of Advanced Robotic Systems, 2018, 15(2): 1729881418769194

28 Wang H X , Lu X , Sheng C Y , Zhang Z G , Cui W , Li Y X . General frame for arbitrary 3R subproblems based on the POE model. Robotics and Autonomous Systems, 2018, 105 : 138– 145

29 Wang Y C , Ding X L , Tang Z X , Hu C W , Xu K . A novel analytical inverse kinematics method for SSRMS-type space manipulators based on the POE formula and the Paden-Kahan subproblem. International Journal of Aerospace Engineering, 2021, 6690696

30 Song T , Pan B , Niu G J , Fu Y L . Preoperative planning algorithm for robot-assisted minimally invasive cholecystectomy combined with appendectomy. IEEE Access: Practical Innovations, Open Solutions, 2020, 8 : 177100– 177111

31 Yu L T Wang W J Wang Z Y Gu Q Wang L. Acquisition method of inverse kinematics analytical solutions for a class of robots dissatisfying the Pieper criterion. Robot, 2016, 38(4): 486– 494 (in Chinese)