Solution of the Forward Kinematics of Parallel Robots Based on Constraint Curves

Abstract

This paper presents a method to solve the forward kinematic problem of parallel robots, obtaining all real solutions. The method is illustrated with the 3UPS-PU parallel robot, and consists in eliminating one unknown to obtain an equation that constrains the admissible values of the remaining unknowns. This constraint defines a curve in the plane of the remaining unknowns, which contains all real solutions. The proposed method samples and scans this constraint curve in order to find all real solutions, using only analytical operations and k-dimensional trees. This method is able to find all real solutions and performs at least one order of magnitude faster than other numerical methods, while avoiding the complicated and lengthy symbolic expressions that traditional elimination procedures involve. The proposed method is also extended to the complex domain and another parallel robot (the 3RRR robot).

Appendices

Appendix A

This appendix derives the expressions of all coefficients $\omega _i$ of the quartic polynomial of Eq. (22). The next demonstration can also be used for obtaining all coefficients $\omega _i$ of the quartic polynomial of Eq. (29), by swapping every $\alpha$ and $\beta$ appearing in this appendix. First, let us rewrite Eq. (21) as follows, with the denominator already omitted:

$$\begin{aligned} \det \left( \mathbf {U}\cdot t_\beta ^2 + \mathbf {V}\cdot t_\beta + \mathbf {W} \right) = 0 \end{aligned}$$

(36)

where:

$$\begin{aligned} \mathbf {U} = \mathbf {Y}_\alpha - \mathbf {C}_\alpha , \quad \mathbf {V} = 2 \cdot \mathbf {S}_\alpha , \quad \mathbf {W} = \mathbf {Y}_\alpha + \mathbf {C}_\alpha \end{aligned}$$

(37)

Since $\mathbf {U}$, $\mathbf {V}$, $\mathbf {W}$ are $2\times 2$ matrices, the expansion of the left-hand side of Eq. (36) yields the quartic polynomial of Eq. (22), i.e.:

$$\begin{aligned} \det \left( \mathbf {U}\cdot t_\beta ^2 + \mathbf {V}\cdot t_\beta + \mathbf {W} \right) = \omega _4 t_\beta ^4 + \omega _3 t_\beta ^3 + \omega _2 t_\beta ^2 + \omega _1 t_\beta + \omega _0 \end{aligned}$$

(38)

The next objective is to obtain the expressions of the coefficients $\omega _i$ as simple functions of the matrices $\mathbf {U}$, $\mathbf {V}$, and $\mathbf {W}$. Note that $\omega _0$ can be directly obtained by substituting $t_\beta =0$ into the previous equation, which yields:

$$\begin{aligned} \omega _0 = \det \left( \mathbf {W}\right) \end{aligned}$$

(39)

To obtain $\omega _1$, we first differentiate Eq. (38) once with respect to $t_\beta$:

$$\begin{aligned} \hbox {tr}\left[ \hbox {adj}\left( \mathbf {U}\cdot t_\beta ^2 + \mathbf {V}\cdot t_\beta + \mathbf {W} \right) \left( 2 \mathbf {U}\cdot t_\beta + \mathbf {V} \right) \right] = 4 \omega _4 t_\beta ^3 + 3 \omega _3 t_\beta ^2 + 2\omega _2 t_\beta + \omega _1 \end{aligned}$$

(40)

where Jacobi’s formula for the derivative of a determinant has been used when differentiating the left-hand side of Eq. (38):

(41)

where “$\hbox {adj}(\mathbf {Q})$” is the adjugate of $\mathbf {Q}$, which satisfies: $\mathbf {Q} \cdot \hbox {adj}(\mathbf {Q})=\det (\mathbf {Q}) \cdot \mathbf {I}$ ($\mathbf {I}$ is the identity matrix with the same size as $\mathbf {Q}$). Inserting $t_\beta =0$ into (40) yields:

$$\begin{aligned} \omega _1 = \hbox {tr}\left[ \hbox {adj}(\mathbf {W})\cdot \mathbf {V} \right] \end{aligned}$$

(42)

Following the previous process, one might continue differentiating Eq. (40) repeatedly and substituting $t_\beta =0$ to obtain the remaining coefficients ($\omega _2$, $\omega _3$, $\omega _4$). However, this task becomes too tedious and error-prone due to the many terms generated by repeatedly differentiating the determinant. Alternatively, the remaining coefficients can be obtained more easily as explained next.

First, it can be checked that reversing the order of the matrix coefficients of the polynomial appearing inside the determinant on the left-hand side of Eq. (38) (i.e., swapping $\mathbf {U}$ and $\mathbf {W}$) reverses the order of the coefficients $\omega _i$ on the right-hand side of the same equation, obtaining:

$$\begin{aligned} \det \left( \mathbf {W}\cdot t_\beta ^2 + \mathbf {V}\cdot t_\beta + \mathbf {U} \right) = \omega _0 t_\beta ^4 + \omega _1 t_\beta ^3 + \omega _2 t_\beta ^2 + \omega _3 t_\beta + \omega _4 \end{aligned}$$

(43)

Accordingly, one can compute $\omega _4$ and $\omega _3$ by swapping $\mathbf {U}$ and $\mathbf {W}$ in the formulas already derived for $\omega _0$ and $\omega _1$, respectively, obtaining:

$$\begin{aligned} \omega _4 = \det \left( \mathbf {U}\right) , \quad \omega _3 = \hbox {tr}\left[ \hbox {adj}(\mathbf {U})\cdot \mathbf {V} \right] \end{aligned}$$

(44)

Finally, after all $\omega _i$ except $\omega _2$ are known, the central coefficient $\omega _2$ can be obtained by substituting $t_\beta =1$ into Eq. (38) and solving for $\omega _2$:

$$\begin{aligned} \omega _2 = \det \left( \mathbf {U} + \mathbf {V} + \mathbf {W} \right) - \omega _4 - \omega _3 - \omega _1 - \omega _0. \end{aligned}$$

(45)

Appendix B

This appendix shows how to solve $\beta$ (respectively $\alpha$) from Eq. (34) when $\alpha$ (respectively $\beta$) is given. If $\alpha$ is given, Eq. (34) can be rewritten as follows:

$$\begin{aligned} C\cos \beta + S\sin \beta + I=0 \end{aligned}$$

(46)

where the coefficients C, S, I have the following expressions:

$$\begin{aligned} C = 2 h_2 ( - b_3 s_{(\alpha +\theta _3)} + a_1 s_{\theta _1} - a_3 s_{\theta _3} - n_3 ) \end{aligned}$$

(47)

$$\begin{aligned} S = 2 h_2 ( b_3 c_{(\alpha + \theta _3)} - a_1 c_{\theta _1} + a_3 c_{\theta _3} + m_3 ) \end{aligned}$$

(48)

$$\begin{aligned}&I = - 2 a_1 b_3 c_{(\alpha + \theta _3-\theta _1)} - 2 a_1 a_3 c_{(\theta _3-\theta _1)} + 2 a_3 b_3 c_{\alpha } \nonumber \\&\qquad + 2 b_3 \left[ c_{\theta _3} ( m_3 c_{\alpha } + n_3 s_{\alpha } ) + s_{\theta _3} ( n_3 c_{\alpha } - m_3 s_{\alpha } ) \right] - 2 a_1 ( m_3 c_{\theta _1} + n_3 s_{\theta _1} ) \nonumber \\&\qquad \qquad \qquad + 2 a_3 ( m_3 c_{\theta _3} + n_3 s_{\theta _3} ) + a_1^2 + a_3^2 - b_1^2 + b_3^2 + m_3^2 + n_3^2 + h_2^2 \end{aligned}$$

(49)

Equation (46) can be converted to a quadratic polynomial using the substitution of Eq. (19), which gives: $(I-C)t_\beta ^2 + (2S)t_\beta + (I+C)=0$. This quadratic equation is easily solved.

Similarly, if $\beta$ is given, Eq. (34) can also be rewritten as “$C\cos \alpha + S\sin \alpha + I=0$”, where the coefficients C, S, I are:

$$\begin{aligned} C = 2 b_3 \left( h_2 s_{(\beta -\theta _3)} - a_1 c_{(\theta _1-\theta _3)} + m_3 c_{\theta _3} + n_3 s_{\theta _3} + a_3 \right) \end{aligned}$$

(50)

$$\begin{aligned} S = 2 b_3 ( - a_1 s_{(\theta _1-\theta _3)} - h_2 c_{(\beta -\theta _3)} + n_3 c_{\theta _3} - m_3 s_{\theta _3} ) \end{aligned}$$

(51)

$$\begin{aligned}&I = - 2 a_1 a_3 c_{(\theta _3-\theta _1)} + 2 a_1 h_2 s_{(\theta _1-\beta )} + 2 a_3 h_2 s_{(\beta -\theta 3)} \\&\qquad - 2 a_1 (m_3 c_{\theta _1} + n_3 s_{\theta _1}) + 2 a_3 ( m_3 c_{\theta _3} + n_3 s_{\theta _3}) + 2 h_2 ( m_3 s_{\beta } - n_3 c_{\beta } ) \\&\qquad \qquad \qquad \qquad \qquad \qquad + a_1^2 + a_3^2 - b_1^2 + b_3^2 + m_3^2 + n_3^2 + h_2^2. \end{aligned}$$

(52)