6R Robot Inverse Solution Algorithm Based on Quaternion Matrix and Groebner Base

DOI: 10.4236/alamt.2018.81004   PDF   HTML     681 Downloads   1,242 Views   Citations
This article proposes a new algorithm of quaternion and dual quaternion in matrix form. It applies quaternion in special cases of rotated plane, transforming the sine and cosine of the rotation angle into matrix form, then exporting flat quaternions base in two matrix form. It establishes serial 6R manipulator kinematic equations in the form of quaternion matrix. Then five variables are eliminated through linear elimination and application of lexicographic Groebner base. Thus, upper bound of the degree of the equation is determined, which is 16. In this way, a 16-degree equation with single variable is obtained without any extraneous root. This is the first time that quaternion matrix modeling has been used in 6R robot inverse kinematics analysis.

Keywords

Share and Cite:

Ni, Z. and Wu, R. (2018) 6R Robot Inverse Solution Algorithm Based on Quaternion Matrix and Groebner Base. Advances in Linear Algebra & Matrix Theory, 8, 33-40. doi: 10.4236/alamt.2018.81004.

1. Introduction

This article uses quaternion matrix to solve the problem of 6R robot inverse kinematics in the three-dimensional space. In fact, this problem has been solved in 1986  , through Closed Displacement Equation established by Homogeneous Transforms Matrix. It makes use of the product of the closed equation to conduct linear combination and then gets a new series of related equations. Finally, it performs linear elimination according to the linear combination among equations and does final elimination by the resultant, obtaining the 16-degree equation with single variable in the end. The kinematic model of 6R robot can also be built using Dual quaternion or Double quaternion, but there are some problems existed. When Qiao Shuguang  used Double quaternion to solve the problem, 8 extraneous roots appear. In order to eliminate the extraneous roots, four 1 + t2 common factors were extracted in Dixon Resultant. Thus, a 16-degree equation with single variable is obtained. Huang Xiguang  and Gan Dongming  transformed the sine and cosine of one rotation angle into plural form instead of extracting common factors, but numerical calculation is needed. Because only in that way, ±12 degree equation can be transformed to ±8 degree equation, and then a 16-degree equation with single variable can be obtained. Even though they have obtained the single-variable equation, the deduction process depends on numerical computation lacking rigorous proof. A new algorithm using quaternion and dual quaternion in matrix form is put forward in this article. It applies quaternion in special cases of rotated plane, transforming the sine and cosine of the rotation angle into matrix form, then exporting flat quaternion base in two matrixes form. It establishes serial 6R manipulator kinematic equations in the form of quaternion matrix. Then five variables are eliminated through linear elimination and application of lexicographic Groebner base. Thus, upper bound of the degree of the equation is determined, which is 16. In this way, a 16-degree equation with single variable is obtained without any extraneous root. This is the first time that quaternion matrix   modeling has been used in 6R robot inverse kinematics analysis.

2. Homogeneous Transformations in the Form of Quaternion Matrix

A dual quaternion can be expressed as:

$\stackrel{⌢}{q}=ai+bj+ck+d+{a}_{0}i\epsilon +{b}_{0}j\epsilon +{c}_{0}k\epsilon +{d}_{0}\epsilon$ (1)

which the last four part can be written as

${a}_{0}i\epsilon +{b}_{0}j\epsilon +{c}_{0}k\epsilon +{d}_{0}\epsilon =\left[\begin{array}{cc}{d}_{0}+{c}_{0}i& {a}_{0}+{b}_{0}i\\ -{a}_{0}+{b}_{0}i& {d}_{0}-{c}_{0}i\end{array}\right]\epsilon$

which the part in the bracket can be transformed to matrix form using the method above, so the matrix form of dual quaternion can be expressed as:

$\begin{array}{c}\stackrel{⌢}{q}=ai+bj+ck+d+{a}_{0}i\epsilon +{b}_{0}j\epsilon +{c}_{0}k\epsilon +{d}_{0}\epsilon \\ =\left[\begin{array}{cc}d+ci& a+bi\\ -a+bi& d-ci\end{array}\right]+\left[\begin{array}{cc}{d}_{0}+{c}_{0}i& {a}_{0}+{b}_{0}i\\ -{a}_{0}+{b}_{0}i& {d}_{0}-{c}_{0}i\end{array}\right]\epsilon \end{array}$

Using D-H method to build robot coordinate system, the relationship of two adjacent joint coordinate systems i-1 and i, can be described by the parameters in Figure 1.

In Figure 1, a new coordinate system can be derived by the coordinate system i-1 rotating on its axis Z with angle θi and transforming si along the same axis. Similarly, by rotating this new coordinate system on its axis X with angle αi and transforming ai along the same X, the coordinate system i can be obtained. This kinematics can be described in matrix form like this

${Q}_{i-1}^{z}\left({\theta }_{i},{s}_{i}\right){Q}_{i-1}^{x}\left({\alpha }_{i-1i},{a}_{i-1i}\right)$

${Q}_{i-1}^{z}\left({\theta }_{i},{s}_{i}\right)={Z}_{i-1}+\epsilon {Z}_{i-1}^{\text{0}}={Ζ}_{i-1}+\epsilon {S}_{i-1}{Z}_{i-1}/2$

in which:

Figure 1. D-H coordinate transform.

$\begin{array}{l}{Z}_{i-1}=\left\{0,0,\mathrm{sin}\frac{\theta }{2}K,\mathrm{cos}\frac{\theta }{2}\right\}\\ ⇒\left(\begin{array}{cc}X& \text{0}\\ \text{0}& \stackrel{¯}{X}\end{array}\right),\\ X=\mathrm{cos}\frac{\theta }{2}+i\mathrm{sin}\frac{\theta }{2}={\text{e}}^{\frac{i\theta }{2}},\\ \stackrel{¯}{X}=\mathrm{cos}\frac{\theta }{2}-i\mathrm{sin}\frac{\theta }{2}={\text{e}}^{\frac{-i\theta }{2}}\end{array}$

$\begin{array}{l}s=\left\{0,0,{s}_{1},0\right\}\\ ⇒\left(\begin{array}{cc}i{s}_{1}& 0\\ 0& -i{s}_{1}\end{array}\right).\end{array}$

So

${Q}_{i-1}^{z}\left({\theta }_{i},{s}_{i}\right)=\left[\begin{array}{cc}{\text{e}}^{\frac{i{\theta }_{i}}{2}}& 0\\ 0& {\text{e}}^{-\frac{i{\theta }_{i}}{2}}\end{array}\right]+\frac{1}{2}\left[\begin{array}{cc}i{s}_{1}{\text{e}}^{\frac{i{\theta }_{i}}{2}}& 0\\ 0& i{s}_{1}{\text{e}}^{-\frac{i{\theta }_{i}}{2}}\end{array}\right]\epsilon$

among which the latter one is a conversion related to x-axis.

${Q}_{i-1}^{x}\left({\alpha }_{i-1i},{a}_{i-1i}\right)={X}_{i-1}+\epsilon {X}_{i-1}^{\text{0}}={X}_{i-1}+\epsilon {A}_{i-1}{X}_{i-1}/2$ ,

in which

${X}_{i-1}=\left[\begin{array}{cc}\mathrm{cos}\frac{\alpha }{2}& \mathrm{sin}\frac{\alpha }{2}\\ -\mathrm{sin}\frac{\alpha }{2}& \mathrm{cos}\frac{\alpha }{2}\end{array}\right]$

${A}_{i-1}=\left[\begin{array}{cc}0& a\\ -a& 0\end{array}\right]$

${Q}_{i-1}^{x}\left({\alpha }_{i-1i},{a}_{i-1i}\right)=\left[\begin{array}{cc}\mathrm{cos}\frac{\alpha }{2}& \mathrm{sin}\frac{\alpha }{2}\\ -\mathrm{sin}\frac{\alpha }{2}& \mathrm{cos}\frac{\alpha }{2}\end{array}\right]+\frac{1}{2}\epsilon \left[\begin{array}{cc}-a\mathrm{sin}\frac{\alpha }{2}& a\mathrm{cos}\frac{\alpha }{2}\\ -a\mathrm{cos}\frac{\alpha }{2}& -a\mathrm{sin}\frac{\alpha }{2}\end{array}\right]$

3. Closed-Form Kinematics Equations in Term of Double Quaternion Matrix

3.1. Mathematical Modeling

Inverse solution of position of 6R robot in three dimensions is that given the position and gesture and parameter si, ai, αi , find the input angles ( ${\theta }_{i}\left(i=1,2,\cdots ,6\right)$ ) of every joints. 6R serial mechanism (Figure 2) has six joints and it needs 6 three-dimension movements described in Figure 1. According to basic theories of quaternion matrix, the mathematical model of inverse solution of kinematics position of 6R serial mechanism in three dimensions can be expressed in matrix form:

${Q}_{1}^{z}\left({\theta }_{1},{s}_{1}\right){Q}_{1}^{x}\left({\alpha }_{12},{a}_{12}\right){Q}_{2}^{z}\left({\theta }_{2},{s}_{2}\right){Q}_{2}^{x}\left({\alpha }_{23},{a}_{23}\right)\cdots {Q}_{1}^{z}\left({\theta }_{6},{s}_{6}\right){Q}_{6}^{x}\left({\alpha }_{61},{a}_{61}\right)=M$ . (2)

θi, si, αi and ai in the equations are structural parameters of 6R robot manipulate arm. ${Q}_{i}^{Z}\left({\theta }_{i},{S}_{i}\right)$ and ${Q}_{i}^{x}\left({\alpha }_{i},{a}_{i}\right)$ are quaternion matrix transformations that rotate and move along z-axis and x-axis respectively. M is the expression in quaternion matrix form of the position and gesture of the end of the robot. For general 6R robot, unknown variables are ${\theta }_{i}\left(i=1,2,\cdots ,6\right)$ and other variable are determined by the structure of robot which are known. Our goal is to find out the six angles. We use Ti and Ai to represent ${Q}_{i}^{Z}\left({\theta }_{i},{S}_{i}\right)$ and ${Q}_{i}^{x}\left({\alpha }_{i},{a}_{i}\right)$ . As Qiao  said that two of the six variables on the left of Equation (2) can be moved to the right and then it can be solved by Dixon resultant. Here, we move three of six variables on the left to the right, then Equation (2) becomes:

${T}_{1}{A}_{1}{T}_{2}{A}_{2}{T}_{3}=M{A}_{3}^{-1}{A}_{4}^{-1}{T}_{4}^{-1}{A}_{6}^{-1}{T}_{6}^{-1}{A}_{5}^{-1}{T}_{5}^{-1}$ . (3)

Assume that the left and right part in equation is T7 and T8 respectively, which are quaternions in matrix form. Using Mathematic 6.0, we get:

Figure 2. A general 6R manipulator.

$\begin{array}{l}{T}_{7}={U}_{\text{1}}+\epsilon {U}_{\text{2}}=\left[\begin{array}{cc}{u}_{11}& {u}_{13}\\ {u}_{12}& {u}_{14}\end{array}\right]+\epsilon \left[\begin{array}{cc}{u}_{21}& {u}_{23}\\ {u}_{22}& {u}_{24}\end{array}\right]\\ {T}_{8}={V}_{\text{1}}+\epsilon {V}_{\text{2}}=\left[\begin{array}{cc}{v}_{11}& {v}_{13}\\ {v}_{12}& {v}_{14}\end{array}\right]+\epsilon \left[\begin{array}{cc}{v}_{21}& {v}_{23}\\ \text{}{v}_{22}& {v}_{24}\end{array}\right]\end{array}$ (4)

Since ${u}_{1i}={v}_{1i}$ and ${u}_{2i}={v}_{2i}$ ( $i=1,2,3,4$ ) and we assume that ${\text{e}}^{i{\theta }_{j}/2}={x}_{j},\text{}j=1,2,\cdots ,6$ , we can obtain eight equations below:

$\begin{array}{l}{a}_{i1}{x}_{1}{x}_{2}{x}_{3}+{a}_{i2}{x}_{1}{x}_{2}{x}_{3}^{-1}+{a}_{i3}{x}_{1}{x}_{2}^{-1}{x}_{3}+{a}_{i4}{x}_{1}^{-1}{x}_{2}{x}_{3}\\ +\text{\hspace{0.17em}}{a}_{i5}{x}_{1}^{-1}{x}_{2}^{-1}{x}_{3}+{a}_{i6}{x}_{1}{x}_{2}^{-1}{x}_{3}^{-1}+{a}_{i7}{x}_{1}{}^{-1}{x}_{2}{x}_{3}^{-1}+{a}_{i8}{x}_{1}^{-1}{x}_{2}^{-1}{x}_{3}^{-1}\\ ={b}_{i1}{x}_{4}{x}_{5}{x}_{6}+{b}_{i2}{x}_{4}{x}_{5}{x}_{6}^{-1}+{b}_{i3}{x}_{4}^{-1}{x}_{5}{x}_{6}+{b}_{i4}{x}_{4}{x}_{5}^{-1}{x}_{6}\\ \text{\hspace{0.17em}}\text{\hspace{0.17em}}+{b}_{i5}{x}_{4}{x}_{5}^{-1}{x}_{6}^{-1}+{b}_{i6}{x}_{4}^{-1}{x}_{5}^{-1}{x}_{6}+{b}_{i7}{x}_{4}^{-1}{x}_{5}{x}_{6}^{-1}+{b}_{i8}{x}_{4}^{-1}{x}_{5}^{-1}{x}_{6}^{-1}\text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{\hspace{0.17em}}\left(i=\text{1},\text{2,}\cdots ,\text{8}\right)\end{array}$ (5)

where ai and bi are known parameters determined by the structure of mechanical arm.

3.2. Eliminate x4, x5 and x6

Assume the right end of the eight Equations in (5) to be:

$\begin{array}{l}{y}_{1}={x}_{4}{x}_{5}{x}_{6};\text{\hspace{0.17em}}{y}_{2}={x}_{4}{x}_{5}{x}_{6}^{-1};\text{\hspace{0.17em}}{y}_{3}={x}_{4}{x}_{5}^{-1}{x}_{6};\\ {y}_{5}={x}_{4}^{-1}{x}_{5}{x}_{6};\text{\hspace{0.17em}}{y}_{4}={x}_{4}{x}_{5}^{-1}{x}_{6}^{-1};\text{\hspace{0.17em}}{y}_{6}={x}_{4}^{-1}{x}_{5}^{-1}{x}_{6};\\ {y}_{7}={x}_{4}^{-1}{x}_{5}{x}_{6}^{-1};\text{\hspace{0.17em}}{y}_{8}={x}_{4}^{-1}{x}_{5}^{-1}{x}_{6}^{-1}\end{array}$

$\begin{array}{l}{x}_{4}{x}_{5}{x}_{6}-{y}_{1}=0;\text{\hspace{0.17em}}{x}_{4}{x}_{5}-{x}_{6}{y}_{2}=0;\text{\hspace{0.17em}}{x}_{4}{x}_{6}-{x}_{5}{y}_{3}=0;\\ {x}_{5}{x}_{6}-{x}_{4}{y}_{4}=0;\text{\hspace{0.17em}}{x}_{4}-{x}_{5}{x}_{6}{y}_{5}=0;\text{\hspace{0.17em}}{x}_{6}-{x}_{4}{x}_{5}{y}_{6}=0;\\ {x}_{5}-{x}_{4}{x}_{6}{y}_{7}=0;\text{\hspace{0.17em}}1-{x}_{4}{x}_{5}{x}_{6}{y}_{8}=0.\end{array}$ (6)

Solve yi and we can get eight equations below:

$\begin{array}{l}{c}_{i1}{x}_{1}{x}_{2}{x}_{3}+{c}_{i2}{x}_{1}{x}_{2}{x}_{3}^{-1}+{c}_{i3}{x}_{1}{x}_{2}^{-1}{x}_{3}+{c}_{i4}{x}_{1}^{-1}{x}_{2}{x}_{3}+{c}_{i5}{x}_{1}{x}_{2}^{-1}{x}_{3}^{-1}\\ +\text{\hspace{0.17em}}{c}_{i6}{x}_{1}^{-1}{x}_{2}{x}_{3}^{-1}+{c}_{i7}{x}_{1}^{-1}{x}_{2}^{-1}{x}_{3}+{c}_{i8}{x}_{1}^{-1}{x}_{2}^{-1}{x}_{3}^{-1}={y}_{i}\text{\hspace{0.17em}}\text{\hspace{0.17em}}\text{\hspace{0.17em}}\left(i=1,2,\cdots ,8\right)\end{array}$ (7)

where ci are known parameters determined by the structure of mechanical arm.

In (7), we use lexicographic method to order, so ${x}_{4}>{x}_{5}>{x}_{6}$ , then we compute the Groebner base. Use algebraic system to perform symbolic operation, then we get 21 bases. However, 11 bases among them are eliminated because of containing x4, x5 and x6. Other ten lower-degree bases are selected as quadric bases:

$\begin{array}{l}-{y}_{\text{6}}\ast {y}_{\text{7}}+{y}_{\text{5}}\ast {y}_{\text{8}}=0;\text{\hspace{0.17em}}-{y}_{\text{4}}\ast {y}_{\text{6}}+{y}_{\text{3}}\ast {y}_{\text{8}}=0;\\ -{y}_{\text{4}}\ast {y}_{\text{7}}+{y}_{\text{2}}\ast {y}_{\text{8}}=0;\text{\hspace{0.17em}}-{y}_{\text{2}}\ast {\text{y}}_{\text{5}}+{y}_{\text{1}}\ast {y}_{\text{7}}=0;\\ -{y}_{\text{3}}\ast {y}_{\text{5}}+{y}_{\text{1}}\ast {y}_{\text{6}}=0;\text{\hspace{0.17em}}-{y}_{\text{2}}\ast {y}_{\text{3}}+{y}_{\text{1}}\ast {y}_{\text{4}}=0;\\ -\text{1}+{y}_{\text{4}}\ast {y}_{\text{5}}=0;\text{\hspace{0.17em}}-\text{1}+{y}_{\text{3}}\ast {y}_{\text{7}}=0;\\ -\text{1}+{y}_{\text{1}}\ast {y}_{\text{8}}=0;\text{\hspace{0.17em}}-\text{1}+{y}_{\text{2}}\ast {y}_{\text{6}}=0.\end{array}$ (8)

Substitute y1~y8 into Equation (8) and multiply ${x}_{1}^{2}{x}_{2}^{2}{x}_{3}^{2}$ to both sides, then we obtain 10 equations about ${x}_{1}^{2},{x}_{2}^{2},{x}_{3}^{2}$ whose degrees are no more than 4:

${d}_{i1}+{d}_{i2}{x}_{1}^{2}+\cdots +{d}_{i27}{x}_{1}^{4}{x}_{2}^{4}{x}_{3}^{4}=0\text{\hspace{0.17em}}\left(i=1,\cdots ,10\right)$ . (9)

For (9), assume ${x}_{1}^{2}={X}_{1}$ , ${x}_{2}^{2}={X}_{2}$ , ${x}_{3}^{2}={X}_{3}$ and simplify this equation by lexicographic Groebner base. Detailed steps are:

Use Mathematica on Intel Pentium IV, 2.93 GHz, RAM 1 G, PC and rank the Groebner bases in (9) according to ${X}_{1}>{X}_{2}>{X}_{3}$ . This process cost 5.657 seconds and we obtain 11 bases.

Seven of the 11 bases are shown below:

${e}_{i1}+{e}_{i2}{X}_{1}+{e}_{i3}{X}_{1}^{2}+{e}_{i4}{X}_{2}+{e}_{i5}{X}_{1}{X}_{2}+{e}_{i6}{X}_{2}^{2}+{e}_{i7}{X}_{2}^{3}\text{\hspace{0.17em}}\text{\hspace{0.17em}}\left(i=1,2,\cdots ,7\right)$ . (10)

Other four bases are abandoned because they have high degree of X1 and X2. The coefficients eij in (10) are 4, 3, 1, 3, 2, 2, 1 algebraic expression of X3.

The seven Equations in (10) can be transformed into matrix form:

${N}_{7×7}T=0$ (11)

where $T={\left[1,{X}_{1},{X}_{1}^{2},{X}_{2},{X}_{1}{X}_{2},{X}_{2}^{2},{X}_{2}^{3}\right]}^{\text{T}}$ .

In order to have solutions, the value of determinant of coefficient must be 0, that is

$\mathrm{det}\left({N}_{7×7}\right)=0$ . (12)

According to the analysis to the degree of X3, matrix N7×7 has 16 degrees about X3. So the degree of expansion is no more than 16.

According to (12), there is no need to extract any common factor and 16-degree input and output equations with single variable X3 can be obtained:

$\underset{i=0}{\overset{16}{\sum }}{s}_{i}{X}_{3}^{i}=0$ (13)

where si are real coefficients determined by input parameters. Solve (13), 16 solutions will be obtained and through:

${\theta }_{3}=\mathrm{ln}{X}_{3}/i$ (14)

value of ${\theta }_{3}$ is available.

Substitute x3 to (8) and extract eight equations to build an equation set; consider ${X}_{1},{X}_{1}^{2},{X}_{2},{X}_{1}{X}_{2},{X}_{2}^{2},{X}_{2}^{3}$ as unknown variables and solve it. Then we can get X1 and X2. Through (14), we can get θ1 and θ2. Input θ1 and θ2 to (7), yi can be solved. Then, by (6) and (14), θ4, θ5 and θ6 are also available.

3.3. Sample Analysis

Use the parameters in example by Q. Shuguang, which the structural parameters are shown in Table 1. According to the parameter above, we obtain the position and gesture matrix of the end of the mechanical arm, which has 16 groups of joint angles.

The results of back solution are shown in Table 2, in which there are four real solutions and the 14th solution equals to the initial value. In addition, the result in Table 2 is exactly the same as the result gained by program in   and  . In this way, this algorithm is correct.

4. Conclusion

Based on research of quaternion in matrix form, we apply it on the modeling of 6R robot. By using lexicographic Groebner base twice, we eliminate 3 variables

Table 1. Structural parameters of 6R robot.

Table 2. 16 groups of solutions of 6R robot.

and obtain 10 bases in the first time and 11 bases in the second time. Thus, the degree of variables decreases. Using 7 bases among them and resultant elimination, the analytical solution of the 16-degree equation is obtained and the numerical example gets the same result as the literature above. It is a convenient method which can be applied to inverse kinematics analysis of 6R robot in three dimensions, laying a new foundation for real application.

Conflicts of Interest

The authors declare no conflicts of interest.

  Liao, Q.Z. and Liang, C.G. (1993) Synthesizing Spatial 7R Mechanism with 16-Assembly Configurations. Mechanisms and Machine Theory, 28, 715-720. https://doi.org/10.1016/0094-114X(93)90010-S  Qiao, S., Liao, Q., Wei, S. and Su, H.J. (2010) Inverse Kinematic Analysis of the General 6R Serial Manipulators Based on Double Quaternions. Mechanism and Machine Theory, 45, 193-197. https://doi.org/10.1016/j.mechmachtheory.2009.05.013  Huang, X.G. and Liao, Q.Z. (2010) New Algorithm for Inverse Kinematics of 6R Serial Robot Mechanism. Journal of Beijing University of Aeronautics and Astronautics, 36, 295-298.  Gan, D., Liao, Q., Wei, S., Dai, J.S. and Qiao, S. (2008) Dual Quaternion-Based Inverse Kinematics of the General Spatial 7R Mechanism. Journal of Mechanical Engineering Science, 222, 1593-1598. https://doi.org/10.1243/09544062JMES1082  Goodson, G.R. (1997) The Inverse-Similarity Problem for Real Orthogonal Matrices. American Mathematical Monthly, 104, 223-230. https://doi.org/10.2307/2974787  Stephen, J., Wine, S. and Le Bihan, N. (2006) Quaternion Singular Value Decomposition Based on Bidiagonalization to a Real or Complex Matrix Using Quaternion Householder Transformations. Applied Mathematics and Computation, 128, 727-738.  Ni, Z.S., Liao, Q.Z., Wei, S.M. and Li, R.H. (2009) Dual Four Element Method for Inverse Kinematics Analysis of Spatial 6R Manipulator. Journal of Mechanical Engineering, 45, 25-29. https://doi.org/10.3901/JME.2009.11.025  Ni, Z.S. (2010) A Study of Geometric Algebro Method on Some Issues for Kinematrics of Mechanisms. Beijing University of Posts and Telecommunications, Beijing.

comments powered by Disqus

Copyright © 2020 by authors and Scientific Research Publishing Inc. This work and the related PDF file are licensed under a Creative Commons Attribution 4.0 International License.