Optimization for Whole Body Reaching Motion Without Singularity

Abstract

This paper presents an algorithm optimizing the whole-body reaching motion of the humanoid robot. After the humanoid robot reaching to the target object, the robot will perform a task such as lifting up the object, opening the door, closing the valve, etc. In order to perform such tasks, the arm configuration space of the robot should not be in the singular configuration that the robot loses its ability to move the end-effector in some direction no matter how it moves its joints. Hence, the robot should generate reaching motion without singularity for a given target point. To yield a solution for this issue, we divided singular cases into two cases through the Jacobian analysis for the 7 DOF arm of our humanoid robot which of the wrist joint has a spherical joint structure that three joint axes intersect one point. By means of self-motion of the redundant arm, one is able to escape is defined as ‘escapable singularity’ and the contrary is defined as ‘inescapable singularity’. The escapable singularity is addressed by the proposed low computing singularity avoidance control. Since the inescapable singularity cannot be avoided by self-motion, the proposed approach used pelvis motion to control shoulder position itself which is the base frame of the arm. To optimize the pelvis motion, we formulate a minimization problem for a simple kinematic model representing pelvis motion with newly defined a geometric constraint. The geometric constraint is selected by the result of the Monte Carlo method for four upper joints because the inescapable singularity is occurred by the four upper joints space only. To demonstrate the feasibility of the proposed optimization, the randomly stacked debris is put in front of the humanoid robot, and the robot would reach the debris.

Appendix

Decoupling the manipulator Jacobian is derived as follows. The joint space and the task spaces can be derived by the forward kinematics $\mathrm{FK}$. The Jacobian matrix is obtained by the first derivative from the forward kinematics.

$${X}_{E}=\mathrm{FK}\left(\Theta \right)\, \mathrm{with\, } \Theta={\left[{\theta }_{1} {\theta }_{2} {\theta }_{3} {\theta }_{4} {\theta }_{5} {\theta }_{6} {\theta }_{7}\right]}^{T}$$

$${\dot{X}}_{E}=\frac{\partial \mathrm{FK}\left(\Theta \right)}{\partial\Theta }\cdot \dot{\Theta }=\mathrm{J}\dot{\Theta }$$

Since the all joints of the DRC-HUBO + arm are revolute, the analytic Jacobian matrix is

$$\mathrm{J}=\left[\begin{array}{c}{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{1}\\ {\overrightarrow{z}}_{1}\end{array} \begin{array}{c}{\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{2}\\ {\overrightarrow{z}}_{2}\end{array} \cdots \begin{array}{c}{\overrightarrow{z}}_{7}\times {\overrightarrow{p}}_{7}\\ {\overrightarrow{z}}_{7}\end{array}\right]$$

The vector from joint origin to end-effector ${\overrightarrow{p}}_{i}$ can be divided into following two vectors.

$$\begin{aligned}&{\overrightarrow{p}}_{i}={\overrightarrow{p}}_{W}+{\overrightarrow{p}}_{iW}\\ &\mathrm{where }\,\,{\overrightarrow{p}}_{i}={O}_{E}-{O}_{i}, \,\,{\overrightarrow{p}}_{W}={O}_{E}-{O}_{W}, \,\,{\overrightarrow{p}}_{iW}={O}_{W}-{O}_{i}\end{aligned}$$

${\overrightarrow{p}}_{W}$ is a position vector from wrist to end-effector and the ${\overrightarrow{p}}_{iW}$ is a position vector from ith joint origin to wrist. Then the Jacobian matrix of the 1–4th columns are divided into two terms by superposition rule.

$$\left[{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{1} {\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{2} {\overrightarrow{z}}_{3}\times {\overrightarrow{p}}_{3} {\overrightarrow{z}}_{4}\times {\overrightarrow{p}}_{4}\right]=\left[{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{W} {\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{W} {\overrightarrow{z}}_{3}\times {\overrightarrow{p}}_{W} {\overrightarrow{z}}_{4}\times {\overrightarrow{p}}_{W}\right]+\left[{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{1W} {\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{2W} {\overrightarrow{z}}_{3}\times {\overrightarrow{p}}_{3W} {\overrightarrow{z}}_{4}\times {\overrightarrow{p}}_{4W}\right]$$

Left parts are simplified by skew symmetric matrix $-[{\overrightarrow{p}}_{W}\sim ]$ of the ${\overrightarrow{p}}_{W}$.

$$\left[{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{1} {\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{2} {\overrightarrow{z}}_{3}\times {\overrightarrow{p}}_{3} {\overrightarrow{z}}_{4}\times {\overrightarrow{p}}_{4}\right]=-\left[{\overrightarrow{p}}_{W}\sim \right]\left[{\overrightarrow{z}}_{1} {\overrightarrow{z}}_{2} {\overrightarrow{z}}_{3} {\overrightarrow{z}}_{4}\right]+\left[{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{1W} {\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{2W} {\overrightarrow{z}}_{3}\times {\overrightarrow{p}}_{3W} {\overrightarrow{z}}_{4}\times {\overrightarrow{p}}_{4W}\right]$$

where

$$\left[{\overrightarrow{p}}_{W}\sim \right]=\left[\begin{array}{ccc}0& -{p}_{Wz}& {p}_{Wy}\\ {p}_{Wz}& 0& -{p}_{Wx}\\ {-p}_{Wy}& {p}_{Wx}& 0\end{array}\right]$$

As shown in above Fig. 2e, since the wrist joints cross the wrist position ${\overrightarrow{p}}_{i}={\overrightarrow{p}}_{W} (i=5{-}7)$, the right upper parts of the Jacobian can also be substituted by skew symmetric matrix as above equation.

$$\left[{\overrightarrow{z}}_{5}\times {\overrightarrow{p}}_{5} {\overrightarrow{z}}_{6}\times {\overrightarrow{p}}_{6} {\overrightarrow{z}}_{7}\times {\overrightarrow{p}}_{7}\right]=-\left[{\overrightarrow{p}}_{W}\sim \right]\left[{\overrightarrow{z}}_{5} {\overrightarrow{z}}_{6} {\overrightarrow{z}}_{7}\right]$$

Thus, the Jacobian matrix of the 7 DOFs manipulator which of configuration likes can be expressed by four sub matrix ${\mathrm{J}}_{A}$, ${\mathrm{J}}_{B}, {\mathrm{J}}_{C}, {\mathrm{J}}_{D}$.

$$\mathrm{J}=\left[\begin{array}{cc}{\mathrm{J}}_{A}{\mathrm{J}}_{B}+{\mathrm{J}}_{C}& {\mathrm{J}}_{A}{\mathrm{J}}_{D}\\ {\mathrm{J}}_{B}& {\mathrm{J}}_{D}\end{array}\right]$$

where ${\mathrm{J}}_{A}=-\left[{\overrightarrow{p}}_{W}\sim \right]$, ${\mathrm{J}}_{B}=\left[{\overrightarrow{z}}_{1} {\overrightarrow{z}}_{2} {\overrightarrow{z}}_{3} {\overrightarrow{z}}_{4}\right]$, ${\mathrm{J}}_{C}=\left[{\overrightarrow{z}}_{1}\times {\overrightarrow{p}}_{1W} {\overrightarrow{z}}_{2}\times {\overrightarrow{p}}_{2W} {\overrightarrow{z}}_{3}\times {\overrightarrow{p}}_{3W} {\overrightarrow{z}}_{4}\times {\overrightarrow{p}}_{4W}\right]$, ${\mathrm{J}}_{D}=\left[{\overrightarrow{z}}_{5} {\overrightarrow{z}}_{6} {\overrightarrow{z}}_{7}\right]$.

Finally, this formulation divided into an upper block triangular matrix and a lower block triangular matrix.

$$\begin{aligned}\mathrm{J}&=\left[\begin{array}{cc}{\mathrm{J}}_{A}{\mathrm{J}}_{B}+{\mathrm{J}}_{C}& {\mathrm{J}}_{A}{\mathrm{J}}_{D}\\ {\mathrm{J}}_{B}& {\mathrm{J}}_{D}\end{array}\right]\\ &=\left[\begin{array}{cc}{\mathrm{I}}_{3}& {\mathrm{J}}_{A}\\ {0}_{3\times 3}& {\mathrm{I}}_{3}\end{array}\right]\left[\begin{array}{cc}{\mathrm{J}}_{C}& {0}_{3\times 3}\\ {\mathrm{J}}_{B}& {\mathrm{J}}_{D}\end{array}\right]\\ &={\mathrm{U}}_{6}\cdot {\mathrm{J}}_{W} \end{aligned}$$

The manipulability which is defined by [1] of this Jacobian is obtained.

$$\begin{aligned}\mathrm{det}\left(\mathrm{J}{\cdot \mathrm{J}}^{T}\right)&=\mathrm{det}\left({\mathrm{U}}_{6}\cdot {\mathrm{J}}_{W}\cdot {\mathrm{J}}_{W}^{T}\cdot {\mathrm{U}}_{6}^{T}\right)\\ &=\mathrm{det}({\mathrm{U}}_{6}) \cdot \mathrm{det}({\mathrm{J}}_{W}\cdot {\mathrm{J}}_{W}^{T}) \cdot \mathrm{det}({\mathrm{U}}_{6}^{T})\\ &=\mathrm{det}({\mathrm{J}}_{W}\cdot {\mathrm{J}}_{W}^{T})\end{aligned}$$

Consequently, when we observe that the determinant of the ${\mathrm{J}}_{W}$ is zero, the original Jacobian matrix is also singularity (Corollary 1).