Inverse Kinematics of a 5-DOF Hybrid Manipulator

Abstract

Control of any robotic system cannot be executed without a preliminary solution of the inverse kinematic problem. This problem implies determining the control actions of the actuators required to perform a given motion trajectory and embedded into the control system. The current study considers the inverse kinematics of a hybrid (parallel-serial) manipulator with five degrees-of-freedom (5-DOF). The article first briefly describes the manipulator structure, which includes 3-DOF parallel and 2-DOF serial parts, and then explains an algorithm for solving the inverse kinematics. The algorithm relies on the product-of-exponentials (PoE) formula applied to an equivalent manipulator with a serial structure. The proposed algorithm results in a closed-form solution with no assumptions about the manipulator geometry. A case study confirms the algorithm correctness. The method for solving the inverse kinematic problem can be adapted for other hybrid manipulators.

Appendices

APPENDIX A

This Appendix outlines the application of the PoE formula [9] for the kinematic analysis of robotic manipulators.

Let the output link of a manipulator be attached to its base by an open kinematic chain, which consists of n 1-DOF joints (we can represent any multi-DOF joint as a combination of 1-DOF ones). We can associate (unit) twist ξ_i ∈ ${{\mathbb{R}}^{6}}$ with the ith joint, i = 1, …, n:

$${\boldsymbol{\xi }_{i}} = \left[ \begin{gathered} {{{\boldsymbol{\omega }}}_{i}} \\ {{{\boldsymbol{\upsilon }}}_{i}} \\ \end{gathered} \right] = \left\{ \begin{gathered} \left[ \begin{gathered} {{{{\mathbf{\hat {s}}}}}_{i}} \\ {{{\mathbf{r}}}_{i}} \times {{{{\mathbf{\hat {s}}}}}_{i}} + {{h}_{i}}{{{{\mathbf{\hat {s}}}}}_{i}} \\ \end{gathered} \right],\quad {\text{if}}\quad {{h}_{i}} \ne \infty , \hfill \\ \left[ \begin{gathered} {{{\mathbf{0}}}_{{3 \times 1}}} \\ {{{{\mathbf{\hat {s}}}}}_{i}} \\ \end{gathered} \right],\quad \quad \quad \,\,{\text{if}}\quad {{h}_{i}} = \infty , \hfill \\ \end{gathered} \right.$$

(A.1)

where ω_i ∈ ${{\mathbb{R}}^{3}}$ is a vector part of the twist; ${{{\boldsymbol{\upsilon }}}_{i}}$∈ ${{\mathbb{R}}^{3}}$ is a moment part of the twist; ${{{\mathbf{\hat {s}}}}_{i}}$ is a unit vector parallel to the twist axis; r_i is a vector that defines coordinates of an arbitrary point on the twist axis; h_i is a pitch of the twist.

Let SX_SY_SZ_S be a reference frame attached to the output link, and let matrix T_S ∈ SE(3) define its configuration relative to base reference frame OXYZ. Finally, let matrix M_S describe some initial configuration of the manipulator. In this configuration, we can associate twists ξ_i, i = 1, …, n, with the chain joints according to Eq. (A.1). Then, the following relation exists between matrices T_S and M_S [9, p. 120]:

$${{{\mathbf{T}}}_{S}} = \left( {\prod\limits_{i = 1}^n {{{e}^{{[{\boldsymbol{\xi }_{i}}]{{\theta }_{i}}}}}} } \right){{{\mathbf{M}}}_{S}},$$

(A.2)

where θ_i is a displacement in the ith joint; [ξ_i] is a matrix representation of twist ξ_i:

$$\begin{gathered} \text{[}{\boldsymbol{\xi }_{i}}] = \left[ {\begin{array}{*{20}{c}} {\Lambda ({{{\boldsymbol{\omega }}}_{i}})}&{{{{\boldsymbol{\upsilon }}}_{i}}} \\ {{{{\mathbf{0}}}_{{1 \times 3}}}}&0 \end{array}} \right] \in se(3), \\ \Lambda ({{{\boldsymbol{\omega }}}_{i}}) = \Lambda \left( {\left[ \begin{gathered} \omega _{i}^{x} \\ \omega _{i}^{y} \\ \omega _{i}^{z} \\ \end{gathered} \right]} \right) = \left[ {\begin{array}{*{20}{c}} 0&{ - \omega _{i}^{z}}&{\omega _{i}^{y}} \\ {\omega _{i}^{z}}&0&{ - \omega _{i}^{x}} \\ { - \omega _{i}^{y}}&{\omega _{i}^{x}}&0 \end{array}} \right] \in so(3). \\ \end{gathered} $$

(A.3)

Equation (A.2) represents the product of exponentials ${{e}^{{[{\boldsymbol{\xi }_{i}}]{{\theta }_{i}}}}}$:

$${{e}^{{[{\boldsymbol{\xi }_{i}}]{{\theta }_{i}}}}} = \left[ {\begin{array}{*{20}{c}} {{{e}^{{\Lambda ({{{\boldsymbol{\omega }}}_{i}}){{\theta }_{i}}}}}}&{({{{\mathbf{I}}}_{{3 \times 3}}}{{\theta }_{i}} + (1 - \cos {{\theta }_{i}})\Lambda ({{{\boldsymbol{\omega }}}_{i}}) + ({{\theta }_{i}} - \sin {{\theta }_{i}})\Lambda {{{({{{\boldsymbol{\omega }}}_{i}})}}^{2}}){{{\boldsymbol{\upsilon }}}_{i}}} \\ {{{{\mathbf{0}}}_{{1 \times 3}}}}&1 \end{array}} \right],$$

where ${{e}^{{\Lambda ({{{\boldsymbol{\omega }}}_{i}}){{\theta }_{i}}}}}$ corresponds to the rotation matrix about the axis defined by vector ω_i by angle θ_i:

$${{e}^{{\Lambda ({{{\boldsymbol{\omega }}}_{i}}){{\theta }_{i}}}}} = {{{\mathbf{I}}}_{{3 \times 3}}} + \sin {{\theta }_{i}}\Lambda ({{{\boldsymbol{\omega }}}_{i}}) + (1 - \cos {{\theta }_{i}})\Lambda {{({{{\boldsymbol{\omega }}}_{i}})}^{2}}.$$

Initial configuration M_S and corresponding twists ξ_i, i = 1, …, n, depend on the manipulator design and location of reference frames SX_SY_SZ_S and OXYZ, so these parameters are considered known for the kinematic analysis. Thus, Eq. (A.2) represents the relationship between joint displacements θ_i and the output link configuration defined by matrix T_S. We can use this equation not only for the forward kinematics (where it is applied generally [9]), but also for the inverse kinematics, which is demonstrated in the present article for the hybrid manipulator.

APPENDIX B

This Appendix contains coefficients of the equations, which are used for solving the inverse kinematic problem:

$${{a}_{1}} = n_{0}^{x}({{(s_{5}^{y})}^{2}} + {{(s_{5}^{z})}^{2}}) - n_{0}^{y}s_{5}^{x}s_{5}^{y} - n_{0}^{z}s_{5}^{x}s_{5}^{z},$$

$${{b}_{1}} = - n_{0}^{y}s_{5}^{x} + n_{0}^{z}s_{5}^{y},$$

$${{c}_{1}} = n_{0}^{x}{{(s_{5}^{x})}^{2}} + n_{0}^{y}s_{5}^{x}s_{5}^{y} + n_{0}^{z}s_{5}^{x}s_{5}^{z} - {{n}^{x}},$$

$$\begin{gathered} {{a}_{2}} = n_{0}^{x}(s_{5}^{x}s_{5}^{y}(1 - \cos {{q}_{6}}) + s_{5}^{z}\sin {{q}_{6}}) + n_{0}^{y}({{(s_{5}^{y})}^{2}}(1 - \cos {{q}_{6}}) + \cos {{q}_{6}}) \\ \, - n_{0}^{z}(s_{5}^{x}\sin {{q}_{6}} - s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})),\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\, \\ \end{gathered} $$

$$\begin{gathered} {{b}_{2}} = n_{0}^{x}(s_{5}^{x}s_{5}^{y}(\cos {{q}_{6}} - 1) + s_{5}^{y}\sin {{q}_{6}}) - n_{0}^{y}(s_{5}^{x}\sin {{q}_{6}} + s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})) \\ \, + n_{0}^{z}({{(s_{5}^{z})}^{2}}(\cos {{q}_{6}} - 1) - \cos {{q}_{6}}),\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\, \\ \end{gathered} $$

$${{c}_{2}} = - {{n}^{y}},$$

$$\begin{gathered} {{a}_{3}} = n_{0}^{x}(s_{5}^{x}s_{5}^{z}(1 - \cos {{q}_{6}}) - s_{5}^{y}\sin {{q}_{6}}) + n_{0}^{y}(s_{5}^{x}\sin {{q}_{6}} + s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})) \\ \, + n_{0}^{z}({{(s_{5}^{z})}^{2}}(1 - \cos {{q}_{6}}) + \cos {{q}_{6}}),\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\, \\ \end{gathered} $$

$$\begin{gathered} {{b}_{3}} = n_{0}^{x}(s_{5}^{x}s_{5}^{y}(1 - \cos {{q}_{6}}) + s_{5}^{z}\sin {{q}_{6}}) + n_{0}^{y}({{(s_{5}^{y})}^{2}}(1 - \cos {{q}_{6}}) + \cos {{q}_{6}}) \\ \, - n_{0}^{z}(s_{5}^{x}\sin {{q}_{6}} - s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})),\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\,\, \\ \end{gathered} $$

$${{c}_{3}} = - {{n}^{z}},$$

$${{a}_{4}} = s_{4}^{x},$$

$$\begin{gathered} {{b}_{4}} = p_{{S0}}^{x}(({{(s_{5}^{y})}^{2}} + {{(s_{5}^{z})}^{2}})(\cos {{q}_{6}} - 1) + 1) + p_{{S0}}^{y}(s_{5}^{x}s_{5}^{y}(1 - \cos {{q}_{6}}) - s_{5}^{z}\sin {{q}_{6}}) \hfill \\ \,\,\,\,\, + p_{{S0}}^{z}(s_{5}^{x}s_{5}^{z}(1 - \cos {{q}_{6}}) + s_{5}^{y}\sin {{q}_{6}}) + r_{5}^{x}({{(s_{5}^{y})}^{2}} + {{(s_{5}^{z})}^{2}})(1 - \cos {{q}_{6}}) \hfill \\ \,\,\,\,\, + r_{5}^{y}(s_{5}^{x}s_{5}^{y}(\cos {{q}_{6}} - 1) + s_{5}^{z}\sin {{q}_{6}}) + r_{5}^{z}(s_{5}^{x}s_{5}^{z}(\cos {{q}_{6}} - 1) - s_{5}^{y}\sin {{q}_{6}}) - p_{S}^{x}, \hfill \\ \end{gathered} $$

$${{a}_{5}} = 1,$$

$$\begin{gathered} {{b}_{5}} = p_{{S0}}^{x}(s_{5}^{x}s_{5}^{y}(1 - \cos {{q}_{6}})\cos \varphi + s_{5}^{x}s_{5}^{z}(\cos {{q}_{6}} - 1)\sin \varphi + s_{5}^{y}\sin {{q}_{6}}\sin \varphi + s_{5}^{z}\sin {{q}_{6}}\cos \varphi ) \hfill \\ \,\,\,\, + p_{{S0}}^{y}( - s_{5}^{x}\sin {{q}_{6}}\sin \varphi + {{(s_{5}^{y})}^{2}}(1 - \cos {{q}_{6}})\cos \varphi + s_{5}^{y}s_{5}^{z}(\cos {{q}_{6}} - 1)\sin \varphi + \cos {{q}_{6}}\cos \varphi ) \hfill \\ \,\,\,\, + p_{{S0}}^{z}( - s_{5}^{x}\sin {{q}_{6}}\sin \varphi + s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})\cos \varphi + {{(s_{5}^{z})}^{2}}(\cos {{q}_{6}} - 1)\sin \varphi + \cos {{q}_{6}}\cos \varphi ) \hfill \\ \,\,\,\, + {{q}_{5}}(s_{4}^{y}\cos \varphi - s_{4}^{z}\sin \varphi ) + r_{3}^{y}(1 - \cos \varphi ) + r_{3}^{z}\sin \varphi \hfill \\ \,\,\,\, + r_{5}^{x}(s_{5}^{x}(\cos {{q}_{6}} - 1)(s_{5}^{y}\cos \varphi - s_{5}^{z}\sin \varphi ) - s_{5}^{y}\sin {{q}_{6}}\sin \varphi - s_{5}^{z}\sin {{q}_{6}}\cos \varphi ) \hfill \\ \,\,\,\, + r_{5}^{y}(s_{5}^{x}\sin {{q}_{6}}\sin \varphi + ({{(s_{5}^{y})}^{2}}\cos \varphi - s_{5}^{y}s_{5}^{z}\sin \varphi - \cos \varphi )(\cos {{q}_{6}} - 1)) \hfill \\ \,\,\,\, + r_{5}^{z}(s_{5}^{x}\sin {{q}_{6}}\cos \varphi + (s_{5}^{y}s_{5}^{z}\cos \varphi - {{(s_{5}^{z})}^{2}}\sin \varphi + \sin \varphi )(\cos {{q}_{6}} - 1)) - p_{S}^{y}, \hfill \\ \end{gathered} $$

$${{a}_{6}} = 1,$$

$$\begin{gathered} {{b}_{6}} = p_{{S0}}^{x}(s_{5}^{x}s_{5}^{y}(1 - \cos {{q}_{6}})\sin \varphi + s_{5}^{x}s_{5}^{z}(1 - \cos {{q}_{6}})\cos \varphi - s_{5}^{y}\sin {{q}_{6}}\cos \varphi + s_{5}^{z}\sin {{q}_{6}}\sin \varphi ) \hfill \\ \,\,\,\, + p_{{S0}}^{y}(s_{5}^{x}\sin {{q}_{6}}\cos \varphi + {{(s_{5}^{y})}^{2}}(1 - \cos {{q}_{6}})\sin \varphi + s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})\cos \varphi + \cos {{q}_{6}}\sin \varphi ) \hfill \\ \,\,\,\, + p_{{S0}}^{z}( - s_{5}^{x}\sin {{q}_{6}}\sin \varphi + s_{5}^{y}s_{5}^{z}(1 - \cos {{q}_{6}})\sin \varphi + {{(s_{5}^{z})}^{2}}(1 - \cos {{q}_{6}})\cos \varphi + \cos {{q}_{6}}\cos \varphi ) \hfill \\ \,\,\,\, + {{q}_{5}}(s_{4}^{y}\sin \varphi + s_{4}^{z}\cos \varphi ) - r_{3}^{y}\sin \varphi + r_{3}^{z}(1 - \cos \varphi ) \hfill \\ \,\,\,\, + r_{5}^{x}(s_{5}^{x}(\cos {{q}_{6}} - 1)(s_{5}^{y}\sin \varphi + s_{5}^{z}\cos \varphi ) + s_{5}^{y}\sin {{q}_{6}}\cos \varphi - s_{5}^{z}\sin {{q}_{6}}\sin \varphi \hfill \\ \,\,\,\, + r_{5}^{y}( - s_{5}^{x}\sin {{q}_{6}}\cos \varphi + ({{(s_{5}^{y})}^{2}}\sin \varphi + s_{5}^{y}s_{5}^{z}\cos \varphi - \sin \varphi )(\cos {{q}_{6}} - 1)) \hfill \\ \,\,\,\, + r_{5}^{z}(s_{5}^{x}\sin {{q}_{6}}\sin \varphi + (s_{5}^{y}s_{5}^{z}\sin \varphi + {{(s_{5}^{z})}^{2}}\cos \varphi - \cos \varphi )(\cos {{q}_{6}} - 1)) - p_{S}^{z}, \hfill \\ \end{gathered} $$

$${{a}_{7}} = - \frac{{p_{{Aj}}^{y} - p_{{Bj}}^{y}}}{{p_{{Aj}}^{z} - p_{{Bj}}^{z}}},$$

$${{b}_{7}} = \frac{{{{{(p_{{Aj}}^{y})}}^{2}} + {{{(p_{{Aj}}^{z})}}^{2}} - {{{(p_{{Bj}}^{y})}}^{2}} - {{{(p_{{Bj}}^{z})}}^{2}} - l_{{AjCj}}^{2} + l_{{BjCj}}^{2}}}{{2(p_{{Aj}}^{z} - p_{{Bj}}^{z})}},$$

$${{a}_{8}} = 1 + a_{7}^{2},$$

$${{b}_{8}} = - 2p_{{Aj}}^{y} - 2{{a}_{7}}(p_{{Aj}}^{z} - {{b}_{7}}),$$

$${{c}_{8}} = {{(p_{{Aj}}^{z} - {{b}_{7}})}^{2}} - l_{{AjCj}}^{2},$$

where $p_{S}^{x}$, $p_{S}^{y}$, $p_{S}^{z}$ and n^x, n^y, n^z are the corresponding components of vectors p_S and ${\mathbf{\hat {n}}}$; $p_{{S0}}^{x}$, $p_{{S0}}^{y}$, $p_{{S0}}^{z}$ and $n_{0}^{x}$, $n_{0}^{y}$, $n_{0}^{z}$ are the same components in the initial configuration of the manipulator (defined by matrix M_S in Eq. (3)); $s_{4}^{x}$, …, $s_{5}^{z}$ are the corresponding components of vectors ${{{\mathbf{\hat {s}}}}_{4}}$ and ${{{\mathbf{\hat {s}}}}_{5}}$.