A Novel Method for Selecting Inverse Kinematic Solutions Based on Configuration Space Partition for 6R Noncuspidal Manipulators

Abstract

The selection of an optimal solution from multiple inverse kinematics solutions (IKSs) is a fundamental task in manipulator motion. However, the conventional minimum joint motion criterion (MJM) method suffers from drawbacks such as high computational time and the inability to ensure configuration invariance. With the prevalence of noncuspidal structures in commercial manipulators, a novel IKS selection methodology is exigent. This paper analyzes the limitations of the MJM method by geometric representations of the IKS formal and proposes a novel IKS selection method based on configuration space decomposition. The configuration space of noncuspidal manipulators is partitioned into independent subdomains called uniqueness domains (UD). Subsequently, a bijection between configuration, UD, and IKS is established for selecting IKS, and three important related theorems are proven. The proposed method offers low computational cost, and allows configuration invariance in continuous trajectory tracking or point-to-point planning. Finally, the physical experiment results demonstrate the effectiveness of the proposed method.

Appendix A: Inverse Kinematics of 6R Ball Wrist Manipulator

This appendix derives the inverse kinematics of the 6R ball-wrist manipulator based on the method in reference [4]. In this appendix, the exponential product method is used, because its convenience brings more compact kinematic equations [36].

1.1 A.1 Recalls of Exponential Product Method

The relative motion between adjacent joints is described by the exponential product formula (POE), as shown in Eq. A1,

$$\begin{aligned} \begin{array}{l}e^{q \hat{\xi }}=\left[ \begin{array}{cc}e^{q \hat{\varvec{w}}} &{} \left( \varvec{I}-e^{q \hat{\varvec{v}}}\right) (\varvec{w} \times \varvec{v})+q \varvec{w} \varvec{w}^{\textrm{T}} \varvec{v} \\ \textbf{0} &{} 1\end{array}\right] \\ \varvec{v}=\varvec{r} \times \varvec{w}\end{array} \end{aligned}$$

(A1)

where q is the angle of rotation. w is the unit vector of the rotation axis and w is its anti-symmetric matrix. I is a 3×3 identity matrix. r is a point on the axis of rotation. ${\xi }$= (w v)$^T$ is the Lie algebraic representation of a Euclidean group SE(3), called twist.

The $e^{q \hat{\varvec{w}}}$ can be calculated by Rodrigues formula as Eq. A2.

$$\begin{aligned} e^{q \hat{\varvec{w}}} = \varvec{I} + \hat{\varvec{w}\sin {q}+\hat{\varvec{w}}}^{\textrm{2}}(1-\cos {q}) \end{aligned}$$

(A2)

The forward kinematics of the manipulator can be represented by Eq. A3.

$$\begin{aligned} \varvec{T}_t = e^{q_1\hat{\varvec{\xi }}_1}...e^{q_6\hat{\varvec{\xi }}_6}\varvec{g}_0 \end{aligned}$$

(A3)

where g$_0$ is the initial pose matrix of T under S, and T$_t$ denotes the pose of EE, expands as follows

$$\begin{aligned} \begin{aligned} \varvec{T}_{\varvec{t}}&=[\varvec{n}, \varvec{o}, \varvec{a}, \varvec{p} ; 0,0,0,1] \\ {}&=\left[ \begin{array}{cccc}n_{x} &{} o_{x} &{} a_{x} &{} p_{x} \\ n_{y} &{} o_{y} &{} a_{y} &{} p_{y} \\ n_{z} &{} o_{z} &{} a_{z} &{} p_{z} \\ 0 &{} 0 &{} 0 &{} 1\end{array}\right] \end{aligned} \end{aligned}$$

(A4)

1.2 A.2 Inverse Kinematics Solution of 6R Ball Wrist Manipulator

The twists of the 6R ball wrist manipulator are shown in Table 5. The initial pose matrix g$_0$ is given in Eq. A5. The forward kinematic model of the manipulator can be obtained from Eq. A3.

$$\begin{aligned} \begin{aligned} \varvec{g}_{0} =\left[ \begin{array}{cccc} 0 &{} 1 &{} 0 &{} a_2+a_3+a_4 \\ 0 &{} -1 &{} 0 &{} 0 \\ -1 &{} 0 &{} 0 &{} a_1+a_4 \\ 0 &{} 0 &{} 0 &{} 1\end{array}\right] \end{aligned} \end{aligned}$$

(A5)

Four equations can be obtained by decoupling Eq. A2, which are shown as follow

$$\begin{aligned} a_{5} c_{23}+a_{3} s_{23}+a_{2} s_{2}=\varvec{p}_{m 1}^{\textrm{T}} \varvec{x}_{m 1} \end{aligned}$$

(A6)

$$\begin{aligned} a_{5} s_{23}-a_{3} c_{23}-a_{2} c_{2}=\varvec{p}_{m 2}^{\textrm{T}} \varvec{x}_{m 2} \end{aligned}$$

(A7)

$$\begin{aligned} \varvec{p}_{m 3}^{\textrm{T}} \varvec{x}_{m 3}=0 \end{aligned}$$

(A8)

$$\begin{aligned} \varvec{z}_{L}^{\textrm{T}} \varvec{o} c_{6}+\varvec{z}_{L}^{\textrm{T}} \varvec{n} s_{6}=0 \end{aligned}$$

(A9)

where $\varvec{p}_{m 1}$ = $[p_x, p_y, -a_{1}]^\textrm{T}$, $\varvec{x}_{m 1}$ = $[c_{1}, s_{1}, -1]^\textrm{T}$, $\varvec{p}_{m 2}$ = $[1, a_{1}, -1]^\textrm{T}$, $\varvec{x}_{m 2}$ = $[0, 1, p_z]^\textrm{T}$, $\varvec{z}_{L}$ = $[c_{1}c_{23}, s_{1}c_{23}, -s_{23}]^\textrm{T}$.

Table 5 The twists of the 6R ball wrist manipulator

From the Eq. A8, the analytical formula of q$_1$ is obtained

$$\begin{aligned} q_1=\arctan 2{(-p_x,p_y)}+\arctan 2{(k_1,0)} \end{aligned}$$

(A10)

where k$_1$ = $\pm 1$, which is the first ambiguous symbol, and $\arctan$2( , ) denotes the arctangent function.

Equations A6 and A7 are squared and summed to obtain the equality

$$\begin{aligned} a_{2}^{2}+2 c_{3} a_{2} a_{3}-2 s_{3} a_{2} a_{5}+a_{3}^{2}+a_{5}^{2}=\left( \varvec{p}_{m 1}^{\textrm{T}} \varvec{x}_{m 1}\right) ^{2}+\left( \varvec{p}_{m 2}^{\textrm{T}} \varvec{x}_{m 2}\right) ^{2} \end{aligned}$$

(A11)

and the analytical formula of q$_{3}$ is

$$\begin{aligned} q_{3}= & {} \arctan _{2}\left( k_{2} \sqrt{a_{3}^{2}\!+\!a_{5}^{2}\!-\!C_{m 3}^{2}}, C_{m 3}\!\right) \!-\!\arctan _{2}\left( a_{5}, a_{3}\right) \nonumber \\ C_{m 3}= & {} \frac{\left( \varvec{p}_{m 1}^{\textrm{T}} \varvec{x}_{m 1}\right) ^{2}+\left( \varvec{p}_{m 2}^{\textrm{T}} \varvec{x}_{m 2}\right) ^{2}-a_{2}^{2}-a_{3}^{2}-a_{5}^{2}}{2 a_{2}} \end{aligned}$$

(A12)

where k$_2$ = $\pm 1$, which is the second ambiguous symbol.

Simultaneous Eqs. A6 and A7 can be obtained

$$\begin{aligned} \begin{array}{l}q_{2}=\arctan _{2}\big (h_{m 1} \varvec{p}_{m 2}^{\textrm{T}} \varvec{x}_{m 2}+h_{m 2} \varvec{p}_{m 1}^{\textrm{T}} \varvec{x}_{m 1}, h_{m 1} \varvec{p}_{m 1}^{\textrm{T}} \varvec{x}_{m 1}\\ \qquad \quad -h_{m 2} \varvec{p}_{m 2}^{\textrm{T}} \varvec{x}_{m 2}\big ) \\ h_{m 1}=a_{5} c_{3}+a_{3} s_{3} \\ h_{m 2}=a_{3} c_{3}-a_{5} s_{3}+a_{2} \end{array} \end{aligned}$$

(A13)

The solution of q$_{6}$ can be obtained by putting q$_{1}$, q$_{2}$ and q$_{3}$ into Eq. A9,

$$\begin{aligned} q_{6}=\arctan _{2}\left( k_{3}, 0\right) +\arctan _{2}\left( \varvec{z}_{L}^{\textrm{T}} \varvec{n}, \varvec{z}_{L}^{\textrm{T}} \varvec{o}\right) \end{aligned}$$

(A14)

where k$_3$ = $\pm 1$, which is the third ambiguous symbol.

There are the following constraints

$$\begin{aligned} \begin{array}{l}\varvec{h}_{n 3}^{\textrm{T}} \varvec{h}_{n 1} c_{4}+\varvec{h}_{n 3}^{\textrm{T}} \varvec{h}_{n 2} s_{4}=-1 \\ \left( \varvec{z}_{L}^{\textrm{T}} \varvec{o} s_{6}-\varvec{z}_{L}^{\textrm{T}} \varvec{n} c_{6}\right) c_{5}-\varvec{z}_{L}^{\textrm{T}} \varvec{a} s_{5}=1\end{array} \end{aligned}$$

(A15)

where $\varvec{h}_{n 1}$ = [s$_1$, c$_1$, 0]$^\textrm{T}$, $\varvec{h}_{n 2}$ = [-c$_1$s$_{23}$, -s$_1$s$_{23}$, -c$_{23}$]$^\textrm{T}$, $\varvec{h}_{n 3}$ = $\varvec{o}c_6 + \varvec{n}s_6$.

Therefore, the solution of $q_4$ and $q_5$ can be obtained

$$\begin{aligned} q_{4}=\arctan {_2}(0,-1)+\arctan {_2}\left( \varvec{h}_{n 2}^{\textrm{T}} \varvec{h}_{n 3}, \varvec{h}_{n 1}^{\textrm{T}} \varvec{h}_{n 3}\right) \end{aligned}$$

(A16)

$$\begin{aligned} q_{5}=\arctan {_2}(0,1)+\arctan {_2}\left( -\varvec{z}_{L}^{\textrm{T}} \varvec{a}, \varvec{z}_{L}^{\textrm{T}} \varvec{o} s_{6}-\varvec{z}_{L}^{\textrm{T}} \varvec{n} c_{6}\right) \end{aligned}$$

(A17)

1.3 A.3 Inverse Kinematics of UR-like Manipulator

Similar to the procedure shown in Appendix A.1, the twists of the UR-like robot are first established, as shown in Table 6.

Table 6 The twists of the UR-like manipulator

The initial pose matrix $\varvec{g}_0$ is given in Eq. A18. The forward kinematic model of the manipulator can be obtained from Eq. A3.

$$\begin{aligned} \begin{aligned} \varvec{g}_{0} =\left[ \begin{array}{cccc} 0 &{} 1 &{} 0 &{} -a_2-a_3 \\ 0 &{} 1 &{} 0 &{} -a_4 \\ 0 &{} 0 &{} 1 &{} a_1-a_5 \\ 0 &{} 0 &{} 0 &{} 1\end{array}\right] \end{aligned} \end{aligned}$$

(A18)

By decoupling the forward kinematic we can obtain Eq. A19

$$\begin{aligned} \varvec{T}_t \varvec{g}_0{ }^{-1} e^{-q_6 \xi _6} e^{-q_5 \xi _5} \varvec{g}_1=e^{q_1 \xi _1} e^{q_2 \xi _2} e^{q_3 \xi _3} e^{q_4 \varepsilon _4} \varvec{g}_1 \end{aligned}$$

(A19)

where $\varvec{g}_1=\varvec{g}_0$. We denote the left side of Eq. A19 as $\varvec{T}_{\varvec{L}1}$ and the right side as $\varvec{T}_{\varvec{R}1}$ and use the $\varvec{T}_{(i, j)}$ to denote the element in row i and column j of the matrix T.

From $\varvec{T}_{\varvec{L}1(1,4)}$ $^2$ + $\varvec{T}_{\varvec{L}1(2,4)}$ $^2$ = $\varvec{T}_{\varvec{R}1(1,4)}$ $^2$ + $\varvec{T}_{\varvec{R}1(2,4)}$ $^2$, Eq. A20 can be obtained

$$\begin{aligned} a_2 c_2+a_3 c_{23}-a_5 s_{234}=k_1 \sqrt{p_x^2+p_y^2-a_4^2} \end{aligned}$$

(A20)

where $k_1 = \pm 1$, which is the first ambiguous symbol.

Let $k_1\sqrt{(p_x^2+p_y^2-a_4^2)}=A$. Substituting A into $\varvec{T}_{\varvec{L}}$, the solution to $q_1$ can be obtained from $\varvec{T}_{\varvec{L}1(1,4)}$ = $\varvec{T}_{\varvec{R}1(1,4)}$ and $\varvec{T}_{\varvec{L}1(2,4)}$ = $\varvec{T}_{\varvec{R}1(2,4)}$, as shown in Eq. A21.

$$\begin{aligned} q_1=\arctan {_2}\left( -p_y, p_x\right) +\arctan {_2}\left( A, d_4\right) \end{aligned}$$

(A21)

Transform Eqs. A19 into A22,

$$\begin{aligned} e^{-q_1 \hat{\xi }_1} \varvec{T} \varvec{T}_0^{-1} e^{-q_6 \hat{\xi }_6}=e^{q_2 \hat{\xi }_2} e^{q_3 \hat{\xi }_3} e^{q_4 \hat{\varepsilon }_4} e^{q_5 \hat{\xi }_5} \end{aligned}$$

(A22)

Denote Eq. A22 as $\varvec{T}_{\varvec{L}2}=\varvec{T}_{\varvec{R}2}$. From $\varvec{T}_{\varvec{L}2(2,3)}=\varvec{T}_{\varvec{R}2(2,3)}$, we can get $q_5$

$$\begin{aligned} q_5=\arctan {_2}(0,1)+\arctan {_2}\left( k_2 \sqrt{1-\left( o_y c_1-o_x s_1\right) ^2}, o_y c_1-o_x s_1\right) \end{aligned}$$

(A23)

where $k_2 =\pm 1$, which is the second ambiguous symbol.

Similarly, by $\varvec{T}_{\varvec{L}2(2,1)}=\varvec{T}_{\varvec{R}2(2,1)}$, $q_6$ is obtained

$$\begin{aligned} q_6=\arctan {_2}(0,1)+\arctan {_2}\left( \frac{n_x s_1-n_y c_1}{s_5}, \frac{a_x s_1-a_y c_1}{s_5}\right) \end{aligned}$$

(A24)

The solution of $q_{234}$ is obtained from $\varvec{T}_{\varvec{L}1(3,1)}=\varvec{T}_{\varvec{R}1(3,1)}$ and $\varvec{T}_{\varvec{L}1(3,3)}=\varvec{T}_{\varvec{R}1(3,3)}$

$$\begin{aligned} q_{234}=\arctan {_2}\left( o_z s_5+c_5\left( n_z c_6-a_z s_6\right) , a_z c_6+n_z s_6\right) \end{aligned}$$

(A25)

Then, put $q_{234}$ into Eq. A20 and combine $\varvec{T}_{\varvec{L}1(3,4)}=\varvec{T}_{\varvec{R}1(3,4)}$, the solution of $q_3$ is obtained.

$$\begin{aligned} \begin{aligned}&q_3=\arctan {_2}(1,0)+\arctan {_2}\left( k_3 \sqrt{1-c_3^2}, c_3\right) \\&c_3=\frac{\left( a_1-p_z-d_5 c_{234}\right) ^2+\left( A+a_5 s_{234}\right) ^2-a_2{ }^2-a_3^2}{2 a_2 a_3} \\&\end{aligned} \end{aligned}$$

(A26)

where $k_3 = \pm 1$, which is the third ambiguous symbol.

By taking $q_3$ into $\varvec{T}_{\varvec{L}1(1,4)}=\varvec{T}_{\varvec{R}1(1,4)}$ and Eq. A18, we can obtain

$$\begin{aligned} \begin{aligned}&q_2=\arctan {_2}(M, N) \\&M=\frac{-\left( A+a_5 s_{234}\right) m-n\left( a_1-p_z-a_5 c_{234}\right) }{-n^2-m^2} \\&N=\frac{-\left( A+a_5 s_{234}\right) m+n\left( A+a_5 s_{234}\right) }{-n^2-m^2} \\&\end{aligned} \end{aligned}$$

(A27)

where $n=a_3s_3$, $m=a_2+a_3c_3$.

Finally,

$$\begin{aligned} q_4=q_{234}-q_2-q_3 \end{aligned}$$

(A28)