Balancing of humanoid robot using contact force/moment control by task-oriented whole body control framework

Abstract

Balancing control of humanoid robots is of great importance since it is a necessary functionality not only for maintaining a certain position without falling, but also for walking and running. For position controlled robots, the for-ce/torque sensors at each foot are utilized to measure the contact forces and moments, and these values are used to compute the joint angles to be commanded for balancing. The proposed approach in this paper is to maintain balance of torque-controlled robots by controlling contact force and moment using whole-body control framework with hierarchical structure. The control of contact force and moment is achieved by exploiting the full dynamics of the robot and the null-space motion in this control framework. This control approach enables compliant balancing behavior. In addition, in the case of double support phase, required contact force and moment are controlled using the redundancy in the contact force and moment space. These algorithms are implemented on a humanoid legged robot and the experimental results demonstrate the effectiveness of them.

Appendix: Proportional distribution of contact force/moment

The resultant contact force/moment $^{O}F_{c}$ in frame O can be recalculated with Eq. (34) using commanding torque. The vector $^{O}F_{c}$ is known value and the contact force/moment of each foot can be calculated by the Eq. (35). This equation can be described as

$$\begin{aligned} ^{O}F_{x}= & {} ^{O}F_{x,r} + ^{O}F_{x,l}, \end{aligned}$$

(55)

$$\begin{aligned} ^{O}F_{y}= & {} ^{O}F_{y,r} + ^{O}F_{y,l}, \end{aligned}$$

(56)

$$\begin{aligned} ^{O}F_{z}= & {} ^{O}F_{z,r} + ^{O}F_{z,l}, \end{aligned}$$

(57)

$$\begin{aligned} ^{O}M_{x}= & {} ^{O}M_{x,r} + ^{O}M_{x,l} - \left( z_{r} \, ^{O}F_{y,r}+z_{l} \, ^{O}F_{y,l}\right) \nonumber \\&+ \left( y_{r} \, ^{O}F_{z,r}+y_{l} \, ^{O}F_{z,l}\right) , \end{aligned}$$

(58)

$$\begin{aligned} ^{O}M_{y}= & {} ^{O}M_{y,r} + ^{O}M_{y,l} + \left( z_{r} \, ^{O}F_{x,r}+z_{l} \, ^{O}F_{x,l}\right) \nonumber \\&\, -\left( x_{r} \, ^{O}F_{z,r}+x_{l} \, ^{O}F_{z,l}\right) , \end{aligned}$$

(59)

$$\begin{aligned} ^{O}M_{z}= & {} ^{O}M_{z,r} + ^{O}M_{z,l} + \left( x_{r} \, ^{O}F_{y,r}+x_{l} \, ^{O}F_{y,l}\right) \nonumber \\&\, -\left( y_{r} \, ^{O}F_{x,r}+y_{l} \, ^{O}F_{x,l}\right) , \end{aligned}$$

(60)

where $^{O}F_{x,r}$, $^{O}F_{y,r}$, $^{O}F_{z,r}$, $^{O}M_{x,r}$, $^{O}M_{y,r}$, $^{O}M_{z,r}$ are contact force/moment of the right foot at frame O and $^{O}F_{x,l}$, $^{O}F_{y,l}$, $^{O}F_{z,l}$, $^{O}M_{x,l}$, $M_{y,l}^{O}$, $^{O}M_{z,l}$ are contact force/moment of the left foot at frame O. To re-distribute these contact force/moment to each foot, assume every force/moment terms are divided in same ratio $\eta$ as

$$\begin{aligned} \eta =\frac{^{O}F_{z,r}}{^{O}F_{z}}. \end{aligned}$$

(61)

The coefficient $\eta$ has the value between zero and one, since $^{O}F_{z} < ^{O}F_{z,r} < 0$. Now we can rewrite the Eqs. (55–60) as,

$$\begin{aligned}&{}^{O}F_{x,r}= \eta ^{O}F_{x},\quad ^{O}F_{x,l}=(1-\eta ) ^{O}F_{x}, \end{aligned}$$

(62)

$$\begin{aligned}&{}^{O}F_{y,r}= \eta ^{O}F_{y},\quad ^{O}F_{y,l}=(1-\eta ) ^{O}F_{y}, \end{aligned}$$

(63)

$$\begin{aligned}&{}^{O}F_{z,r}= \eta ^{O}F_{z}, \quad ^{O}F_{z,l}=(1-\eta ) ^{O}F_{z}, \end{aligned}$$

(64)

$$\begin{aligned}&{}^{O}M_{x,r} = \eta \left( ^{O}M_{x,r} + ^{O}M_{x,l}\right) , \nonumber \\&{}^{O}M_{x,l} = (1-\eta ) \left( ^{O}M_{x,r} + ^{O}M_{x,l}\right) , \end{aligned}$$

(65)

$$\begin{aligned}&{}^{O}M_{y,r} = \eta \left( ^{O}M_{y,r} + ^{O}M_{y,l}\right) , \nonumber \\&{}^{O}M_{y,l}= (1-\eta ) \left( ^{O}M_{y,r} + ^{O}M_{y,l}\right) , \end{aligned}$$

(66)

$$\begin{aligned}&{}^{O}M_{z,r} = \eta \left( ^{O}M_{z,r} + ^{O}M_{z,l}\right) , \nonumber \\&{}^{O}M_{z,l}=(1-\eta ) \left( ^{O}M_{z,r} + ^{O}M_{z,l}\right) . \end{aligned}$$

(67)

This assumption can be applied since every contact condition in Eqs. (30–33) are proportional to the normal force $F_{z}$. So when the resultant contact force/moment $^{O}F_{c}$ satisfies the contact conditions, distributed contact forc-e/moment can also satisfy the conditions for each.

The contact force can always maintain contact satisfying the conditions (29), (30), when $\eta$ has the value between 0 and 1 because forces are independent with moments. On the other hand, contact moments are related with forces at each foot. So the contact condition for moments (31–33) can be violated depending on $\eta$. The Eq. (58) can be described as the relation between $^{O}M_{x,r}$ and $\eta$ by substituting Eqs. (56–64).

$$\begin{aligned} ^{O}M_{x,r}= & {} \left\{ (z_{r}- z_{l}) \, ^{O}F_{y} - (y_{r} - y_{l}) \, ^{O}F_{z} \right\} \eta ^{2} \nonumber \\&+ \left( ^{O}M_{x} + z_{l} \, ^{O}F_{y} - y_{l} \, ^{O}F_{z}\right) \eta , \end{aligned}$$

(68)

and contact condition (58) can be expressed as

$$\begin{aligned} |^{O}M_{x,r}| < |^{O}F_{z}\eta | \frac{l_{y}}{2}, \end{aligned}$$

(69)

Substitute Eq. (68) to the condition (69) to find the boundary of the $\eta$.

$$\begin{aligned} a^{2}\eta ^{2} + 2ab\eta + \left( b^{2}-c^{2}\right) < 0, \end{aligned}$$

(70)

where

$$\begin{aligned}&a = (z_{r}- z_{l}) \, ^{O}F_{y} - (y_{r} - y_{l}) \, ^{O}F_{z}, \end{aligned}$$

(71)

$$\begin{aligned}&b = ^{O}M_{x} + z_{l} \, ^{O}F_{y} - y_{l} \, ^{O}F_{z}, \end{aligned}$$

(72)

$$\begin{aligned}&c = \frac{l_{y}}{2} \, ^{O}F_{z}. \end{aligned}$$

(73)

Boundary of $\eta$ about condition for $M_{y}^{O}$ Eq. (33) can be achieved by the same process using Eqs. (55), (57), (58), (62) and (64). The structure of the equation is the same as that of Eq. (70) and the following terms are different as

$$\begin{aligned}&a = -(z_{r}- z_{l}) \, ^{O}F_{x} + (x_{r} - x_{l}) \, ^{O}F_{z}, \end{aligned}$$

(74)

$$\begin{aligned}&b = ^{O}M_{y} - z_{l} \, ^{O}F_{x} + x_{l} \, ^{O}F_{z}, \end{aligned}$$

(75)

$$\begin{aligned}&c = \frac{l_{x}}{2} \, ^{O}F_{z}. \end{aligned}$$

(76)

Boundary of $\eta$ about condition for $M_{z}^{O}$ (31) also can be calculated by the same process by Eqs. (55), (56), (60), (62) and (63). The structure of the equation is the same as Eq. (70) and the following terms are different as

$$\begin{aligned}&a = - (x_{r}- x_{l}) \, ^{O}F_{y} + (y_{r} - y_{l}) \, ^{O}F_{x}, \end{aligned}$$

(77)

$$\begin{aligned}&b = ^{O}M_{z} - x_{l} \, ^{O}F_{y} + y_{l} \, ^{O}F_{x}, \end{aligned}$$

(78)

$$\begin{aligned}&c = \mu _{s} \, ^{O}F_{z}. \end{aligned}$$

(79)

Boundary of feasible $\eta$ can be determined by Eq. (70) about conditions for $^{O}M_{x,r}$, $^{O}M_{y,r}$ and $^{O}M_{z,r}$. In this research we minimize the magnitude of $M_{x,r}$ and $M_{x,l}$ because the foot width $l_{y}$ is relatively small than the length of the foot $l_{x}$ and static friction coefficient $\mu _s$. Coefficient $\eta$ to make $M_{x,r}= 0$ can be determined in Eq. (68). If the calculated $\eta$ is out of boundary, then nearest value of the boundary can be selected as $\eta$. Finally, distribution of the contact force/moment can be determined by $\eta$ in Eqs. (62–67).