Angular Momentum Based Balance Control

Abstract

Maintaining balance under dynamic external disturbances and environmental conditions remains a key challenge of humanoid robots. As the centroidal dynamics of humanoid states that both linear and angular momenta must be regulated to completely control the balance, the momentum-based balance control approaches maintain the balance through controlling both the linear and angular momenta of a robot. In this setting, the joint motion of a humanoid robot is typically controlled to realize the desired momentum rate change while satisfying non-slip constraints for the support feet as well as some task objectives such as the desired posture. After reviewing related work, we establish the basic theories that compute centroidal momentum and its relation with the generalized coordinates of a humanoid robot. Then we introduce approaches to controlling momentum, such as the resolved momentum control and computed torque-based control. Subsequently, we present several momentum-based approaches to maintaining humanoid robots’ stationary balance in detail. Several ideas to set the desired angular momentum are presented as well. The chapter concludes with the discussion of the limitations and open questions for the momentum-based balance control.

Centroidal momentum, Angular momentum control, Humanoid balance control

1 Introduction

Even after several decades of research, balance maintenance has remained one of the most important issues of humanoid robots. Although the basic dynamics of balance are currently understood, robust and general controllers that can deal with discrete and non-level foot support as well as large, unexpected, and unknown external disturbances such as moving support, slip, and trip have not yet emerged. Especially, in comparison with the elegance and versatility of human balance, present-day robots appear quite deficient. In order for humanoid robots to coexist with humans in the real world, more advanced balance controllers that can deal with a broad range of environment conditions and external perturbations need to be developed.

Traditionally most balance control techniques have attempted to maintain balance by controlling only the linear motion of a robot’s center of mass (CoM) . For example, Kagami et al. [1] and Kudoh et al. [2] proposed methods to change the input joint angle trajectories to modify the position of the center of pressure (CoP) , a point within the robot’s support area through which the resultant ground reaction force (GRF) acts. When the CoP, computed from the input joint motion, leaves the support base, indicating a possible toppling of a foot, the motion is modified to bring the CoP back inside the support base, while the robot still follows the desired linear motion of the CoM. The rotational motion of the robot remains more or less ignored in these approaches.

However, rotational dynamics of a robot plays a significant role in balance [3]. Experiments on human balance control also show that humans tightly regulate angular momentum during gait [4], which suggests the strong possibility that angular momentum may be important in humanoid movements.

In fact, both angular and linear momenta must be regulated to completely control the CoP. The fundamental quantities and the relations between them are schematically depicted in Fig. 1 and described subsequently.

Fig. 1

The external forces and torques in (a) are solely responsible for the centroidal momentum rate change in (b). (c): Linear momentum rate change $\dot { \boldsymbol {l}}$ has a one-to-one correspondence with the GRF f. (d): The centroidal angular momentum rate change $\dot { \boldsymbol {k}}$ is determined by both f and CoP location p. (e): Conversely, p is determined by both $\dot { \boldsymbol {l}}$ and $\dot { \boldsymbol {k}}$

Figure 1a shows all the external forces that act on a freely standing humanoid: the GRF f, the ground reaction moment τ _n normal to the ground, and the weight mg of the robot, where m is the total robot mass and g is the acceleration due to gravity. The sums of external moments and external forces are equivalent to the rates of change of angular and linear momenta, respectively, of the robot, and the mathematical expressions for these relationships are given by Eqs. (1) and (2). Figure 1b depicts the robot’s rate of change of angular momentum about the CoM, $\dot {\boldsymbol {k}}$, and linear momentum, $\dot {\boldsymbol {l}}$, respectively:

$$\displaystyle \begin{aligned} \dot{\boldsymbol{k}} &= (\boldsymbol{p} - \boldsymbol{r}_G)\times\boldsymbol{f} + \boldsymbol{\tau}_n {} \end{aligned} $$

(1)

$$\displaystyle \begin{aligned} \dot{\boldsymbol{l}} &= m\boldsymbol{g} + \boldsymbol{f} {} \end{aligned} $$

(2)

In the above equations, r_G is the CoM location and p is the CoP location. Together k and l is a 6 × 1 vector that constitutes the spatial centroidal momentum h_G = [k^Tl^T]^T. In this chapter, we will call it spatial momentum or simply the momentum of the robot.

Indeed, as noted in [5], the (spatial) momentum rate change has a one-to-one relationship with the GRF and CoP. From Eq. (2) and as shown in Fig. 1c, $\dot {\boldsymbol {l}}$ is completely determined by f and vice versa. Furthermore, from Eq. (1) and Fig. 1d, a complete description of $\dot {\boldsymbol {k}}$ needs both f and p. Conversely, p depends on both $\dot {\boldsymbol {k}}$ and $\dot {\boldsymbol {l}}$, which is shown in Fig. 1e. (The normal torque τ_n also affects $\dot {\boldsymbol {k}}$ in the transverse plane. Actually f, p, and τ_n together constitute the 6 variables that correspond to the 6 variables of the spatial momentum. Usually τ_n is omitted in the discussion for simplicity because its magnitude is small.) This last sentence implies that a complete control of p is impossible without controlling both the momenta components.

The idea of the momentum-based balance control is thus to maintain the balance through controlling both the linear and angular momenta of a robot. In this setting, the joint motion of a humanoid robot is typically controlled to realize the desired momentum rate change while satisfying nonslip constraints for the support feet. On top of these, task objectives such as the desired posture or the desired configuration of the end effectors are pursued.

The challenging aspect of the momentum control originates from the fact that it can be achieved only by interaction from the external environment through contact. The amount of force that can be exerted this way is limited, due both to limitations of the biped’s actuators and the inherent limitations of exerting force on a relatively high CoM with a relatively narrow footprint. Despite the serious actuation limits, bipeds are capable of achieving remarkable tasks like walking, running, and jumping on restricted terrain, if they use appropriate controllers.

The next section describes related work, including a popular bipedal walking technique called ZMP-based control . Section 3 provides analysis and algorithms for computing the momentum of a humanoid robot. Section 4 presents methods to control a robot given the input momentum goals. Section 5 describes some exemplar momentum-based balance controllers. We conclude in Sect. 6 with a discussion and summary and ideas for future work.

2 Related Work

Starting from the early work of [6], researchers have developed numerous techniques for biped balance control using various approaches. Among these are joint control strategies using the ankle or hip [7, 8], whole-body control approaches [1, 9,10,11,12,13], methods that find optimal control policies [14, 15], and reflex controllers [16]. In this section we focus on the research relevant to momentum-based balance control.

The importance of angular momentum in humanoid walking was reported by Sano and Furusho as early as 1990 [7]. However, it was much later before its importance for balance maintenance of human and humanoid robots started to be seriously explored [4, 10, 17, 18]. Sano and Furusho [7] and Mitobe et al. [19] showed that it is possible to generate the desired angular momentum by controlling the ankle torque. Kajita et al. [10] included angular momentum criteria into the whole-body control framework for balance maintenance. After expressing desired linear and angular momenta as linear functions of the generalized velocities, they determined the joint velocities that achieved both momenta.

Komura et al. [3] presented a balance controller that can counteract rotational perturbations using the angular momentum pendulum model (AMPM) . This model augments the well-known 3D linear inverted pendulum model (LIPM) [20] with the additional capability of possessing centroidal angular momentum. Naksuk et al. [21] proposed an iterative method to compute joint trajectories of humanoid robots to satisfy the desired CoM trajectory and to minimize the centroidal angular momentum. Hyon et al. [22] suggested to add the time integral of horizontal component of CoM position to regulate the angular momentum. Other papers that deal with angular momentum for balance and gait include [23,24,25,26,27].

Abdallah and Goswami [28] defined balance control objectives in terms of CoM and CoP and achieved this goal by controlling the rate of change of linear and angular momenta of a reduced model humanoid robot. The joint accelerations to generate the target momentum rate change were resolved using the Moore-Penrose pseudoinverse .

Hofmann et al. [29] presented a method that controls CoM by modulating angular momentum under large external perturbations. It gives higher priority to controlling linear momentum over angular momentum to enhance the performance of the balance controller.

Table 1 illustrates how some of the existing methods treat momentum, GRF, and CoP in formulating balance and gait strategies. Robot gait planning methods using reduced models such as [11, 20] (Table 1a) compute the necessary CoM trajectory which ensures balance for a specified desired CoP trajectory. This is done using reduced models such as the LIPM. As can be seen from Fig. 1e CoP depends on both linear and angular momenta rate changes, so CoM cannot be uniquely determined solely from CoP. This was possible in [11, 20] because the reduced model used in those works approximated the robot as a point mass, which can only possess a zero angular momentum.

Table 1 The diagrams show how each method on balance control or gait pattern generation treats momenta (k, l), GRF (f), and CoP (p). A pair of solid lines determines the target value together. The dotted line shows the determination process of linear motion and force. The subscript “d” indicates a desired input to the controller, and superscript “∗” indicates that the quantity is used to determine control output such as joint torques

In the resolved momentum control approach [10], both desired linear and angular momenta are used to determine joint motion for posture change (Table 1b). As the CoP is not explicitly considered, the desired momenta should be carefully chosen to guarantee balance. The methods in Table 1c determine the desired angular momentum rate change given desired CoP and linear momentum rate change. Starting by setting the desired linear/angular momenta rate changes, the approach in Table 1d first determines admissible foot GRFs and CoPs and then computes corresponding admissible momenta rate changes.

Ugurlu and Kawamura have studied bipedal walking that specifically controls the centroidal angular momentum [25]. Relatively recently we have seen a comprehensive study of angular momentum during human gait [32].

3 Computing Momentum of a Humanoid Robot

After defining a humanoid robot model, this section presents mathematical tools to compute the momentum and the mapping between the momentum and the joint coordinates. A useful indicator point related with angular momentum is also presented.

3.1 Humanoid Robot Model

A humanoid can be modeled as a set of N + 1 links interconnected by N joints, of up to six degrees of freedom each, forming a tree-structure topology. The motion of the links is referenced to a fixed base (inertial frame) which is labeled 0, while the links are labeled from 1 through N. Numbering of the links may be done in any manner such that link i’s predecessor toward the root (link 0), indicated by p(i), is always less than i. Joints in the tree are numbered such that joint i connects link i to link p(i). A coordinate frame is attached to each link to provide a reference for quantities associated with the link.

An n_i × 1 vector $\dot {\boldsymbol {q}}_i$ relates the velocity of link i to the velocity of its predecessor, link p(i), where n_i is the number of degrees of freedom at the joint connecting the two links. The free modes of the joint are represented by the 6 × n_i matrix Φ_i, such that the spatial velocity of link i is given as follows:

(3)

where ω_i and v_i are the angular and linear velocities of link i, respectively, as referenced to the link coordinate frame. We will use spatial notation [33, 34] for describing rigid-body velocity, acceleration, inertia, etc., using 6D vectors and tensors. is a 6 × 6 spatial transform which transforms spatial motion vectors from p(i) to i coordinates. The matrix Φ_i depends on the type of joint [33]. For instance, $\boldsymbol {\varPhi }_i = \left [ 0 \, 0 \, 1 \, 0 \, 0 \, 0 \right ]^T$ for a revolute joint with the rotation axis being aligned with the z-axis.

In order to model a humanoid when in flight, one of the links is modeled as a floating base (typically the torso) and numbered as link 1. A fictitious six degree-of-freedom (DoF) joint is inserted between the floating base and fixed base. In this case, Φ₁ = 1_6×6 where 1_6×6 is the identity matrix. The total number of degrees of freedom in the humanoid is n where $n= \sum n_i$. Note that n includes the six degrees of freedom for the floating base.

The spatial transform may be composed from the position vector from the origin of coordinate frame p(i) to the origin of i and the 3 × 3 rotation matrix which transforms 3D vectors from coordinate frame p(i) to i:

(4)

The quantity S(p) is the skew-symmetric matrix that satisfies S(p) ω = p ×ω for any 3D vector ω.

It is possible to combine the equations for the velocity or momentum for all the links into a global set of equations. To do so, composite vectors and matrices are defined. Global notation is useful in developing a system Jacobian which leads to an expression for the centroidal momentum matrix.

Gathering all of the link velocities and joint velocities together, the system Jacobian J can be defined to give the relationship between the two:

$$\displaystyle \begin{aligned} \mathbf{v} = \boldsymbol{J} \, \dot{\boldsymbol{q}}, \end{aligned} $$

(5)

where

$$\displaystyle \begin{aligned} \mathbf{v} = \left[ {\mathbf{v}}_1^T, \, {\mathbf{v}}_2^T, \cdots {\mathbf{v}}_i^T, \cdots {\mathbf{v}}_N^T \right]^T. \end{aligned} $$

(6)

The elements of the system Jacobian are just the Jacobians for each of the links:

$$\displaystyle \begin{aligned} \boldsymbol{J} = \left[ \boldsymbol{J}_1^T, \, \boldsymbol{J}_2^T, \cdots \boldsymbol{J}_i^T, \cdots \boldsymbol{J}_N^T \right]^T. \end{aligned} $$

(7)

3.2 Multibody System Momenta

The spatial momentum of each link may be computed from the spatial velocity as follows (see Fig. 2):

$$\displaystyle \begin{aligned} \boldsymbol{h}_i = \left[ \begin{array}{c} \boldsymbol{k}_i \\ \boldsymbol{l}_i \end{array} \right] = \boldsymbol{I}_i \, {\mathbf{v}}_i, \end{aligned} $$

(8)

where k_i is the angular momentum, l_i is the linear momentum, and I_i is the spatial inertia for link i. The spatial inertia may be composed from the mass m_i, position vector c_i to the CoM of link i, and 3 × 3 rotational inertia $\bar {\boldsymbol {I}}_i$, all relative to coordinate frame i:

$$\displaystyle \begin{aligned} \boldsymbol{I}_i = \begin{bmatrix} \bar{\boldsymbol{I}}_i & m_i \, \boldsymbol{S}(\boldsymbol{c}_i) \\ m_i \, \boldsymbol{S}(\boldsymbol{c}_i)^T & m_i \, \mathbf{1} \end{bmatrix}, \end{aligned} $$

(9)

where

(10)

and $\bar {\boldsymbol {I}}^{cm}_i$ is the rotational inertia about the CoM. Recall that if the origin of coordinate frame i is chosen at the CoM, the off-diagonal blocks m_i S(c_i) reduce to zero. If, in addition, the axes of coordinate frame i are oriented along the principal axes of inertia, $\bar {\boldsymbol {I}}_i$ becomes a 3 × 3 diagonal matrix and I_i a 6 × 6 diagonal matrix.

Fig. 2

Schematic depiction of a single rigid body: spatial momentum contains the angular and linear momenta

The momenta of all the links in the system may be determined as the product of the system velocity vector v and the system inertia I; gathering all:

$$\displaystyle \begin{aligned} \boldsymbol{h} = \boldsymbol{I} \, \mathbf{v}, \end{aligned} $$

(11)

where h is the 6N × 1 system momentum vector $\boldsymbol {h} = \left [ \boldsymbol {h}_1^T, \, \boldsymbol {h}_2^T, \cdots \boldsymbol {h}_i^T, \cdots \boldsymbol {h}_N^T \right ]^T,$ and the 6N × 6N system inertia matrix is defined as $\boldsymbol {I} = \mathrm {diag} \left [ \boldsymbol {I}_1, \, \boldsymbol {I}_2, \cdots \boldsymbol {I}_i, \cdots \boldsymbol {I}_N \right ].$

3.3 Centroidal Momentum

The aggregate momentum of a humanoid may be obtained by summing up all of the angular and linear momenta contributed by the individual link segments. As defined, the spatial momentum of each link h_i is most naturally expressed in its own coordinate system. As a measure of dynamic stability or for control, it is useful to combine the momenta for the links by projecting the momenta to a common coordinate frame. A convenient frame is one set at the instantaneous CoM or the centroid of the system G and whose coordinate axes are parallel to those of the inertial coordinate frame 0. The 6 × 1 centroidal momentum vector h_G, which consists of the linear and centroidal angular momenta of the robot, is the aggregate momentum with respect to the abovementioned centroidal frame. Noting that the spatial momentum may be projected as any other force-type vector [33], the following equation may be used to calculate the spatial momentum at the centroid of the system (see Fig. 3):

(12)

where X_G is defined as the projection matrix, for motion vectors, from centroidal coordinates to link coordinates and is given as follows:

(13)

Fig. 3

Humanoid robot showing link and centroidal momentum vectors. The inertial frame is located at O and the position vector to the robot CoM, G, is given by r_G. The reference frame of link i is located at O_i. The centroidal momentum h_G can be obtained from either Eq. 15 or Eq. 13

The CoM is calculated by

$$\displaystyle \begin{aligned} \mathbf{CoM} = \frac{\sum_{i=1}^N m_i {\mathbf{r}}_i}{\sum_{i=1}^N m_i} {} \end{aligned} $$

(14)

where r_i denotes the position of the CoM of each link i. The centroidal momentum vector h_G is related to its n × 1 joint velocity vector $\dot {\boldsymbol {q}}$ as

$$\displaystyle \begin{aligned} \boldsymbol{h}_G = \boldsymbol{A}_G(\boldsymbol{q}) \, \dot{\boldsymbol{q}}. \end{aligned} $$

(15)

The 6 × n matrix A_G is called the centroidal momentum matrix , and A_G contains contributions both from the intersegmental joint variables of the robot and from the fictitious joint connecting the floating base of the humanoid to the inertial frame (detailed in Sect. 3.1). Note that A_G is identical to the large matrix in the RHS of Eq. 1 of [10], with only the linear and angular parts interchanged.

As shown in Eq. 15, the CMM gives the relationship between the joint rates and centroidal momentum. In order to find the relationship between this matrix and the link inertias and Jacobians, the concept of the system momentum matrix A is first presented. The system momentum matrix A expresses the relationship between the system momentum vector and the joint rates: $\boldsymbol {h} = \boldsymbol {A} \, \dot {\boldsymbol {q}}$.

Substituting the expression for the system velocity in Eq. 5 into Eq. 11, and using the definition of the system momentum matrix, we can write

$$\displaystyle \begin{aligned} \boldsymbol{A} = \boldsymbol{I} \; \boldsymbol{J}. \end{aligned} $$

(16)

The system momentum matrix is just the product of the system inertia matrix and the system Jacobian. It includes the momentum matrix for each link and is of size 6N × n:

$$\displaystyle \begin{aligned} \boldsymbol{A} = \left[ \begin{array}{c} \boldsymbol{A}_1^T, \, \boldsymbol{A}_2^T, \, \cdots \boldsymbol{A}_i^T, \, \cdots \boldsymbol{A}_N^T \end{array} \right]^T, \end{aligned} $$

(17)

with A_i = I_i J_i. The centroidal momentum may also be expressed as a function of the system momentum matrix A:

$$\displaystyle \begin{aligned} \boldsymbol{h}_G = {\boldsymbol X}_G^T \, \boldsymbol{A} \, \dot{\boldsymbol{q}}. \end{aligned} $$

(18)

Noting Eqs. 15 and 18, and using the expression for A in Eq. 16, the CMM, A_G, may then be defined as

$$\displaystyle \begin{aligned} \boldsymbol{A}_G = {\boldsymbol X}_G^T \, \boldsymbol{A} = {\boldsymbol X}_G^T \, \boldsymbol{I} \, \boldsymbol{J}, \end{aligned} $$

(19)

which shows the relationship between A_G and the system inertia and Jacobian. Furthermore, time differentiation of Eq. 15 results in the following relation which forms the basis of our momentum-based balance controller presented later in this chapter:

$$\displaystyle \begin{aligned} \dot{\boldsymbol{h}}_G = \boldsymbol{A}_G \, \ddot{\boldsymbol{q}} + \dot{\boldsymbol{A}}_G \, \dot{\boldsymbol{q}}. \end{aligned} $$

(20)

Finally, since $\mathbf {f}=\dot {\boldsymbol {h}}_G$ (Newton’s equations of motion) where f is the net external force/moment on the system, then it may be noted that A_G gives the relationship between the net external force/moment and the joint accelerations. The vector $\dot {\boldsymbol {A}}_G \, \dot {\boldsymbol {q}}$ can be computed efficiently by using the fact that it is equivalent to $\dot {\boldsymbol {h}}_G$ when $\ddot {\boldsymbol {q}}=0$, i.e., $\dot {\boldsymbol {A}}_G \, \dot {\boldsymbol {q}}$ can be set to the rate of change of the momentum (or, equivalently, the net force and torque) of a humanoid robot when $\ddot {\boldsymbol {q}}=0$.

3.4 Centroidal Moment Pivot (CMP) Point

The CoM and the CoP are widely used feature points that describe the centroidal linear dynamics of a humanoid robot. The centroidal moment pivot (CMP) point [35] is the point on the ground from which the GRF would have to emanate if it were to produce no torque about the CoM. Therefore, the distance of CMP from CoP is closely related with the rate of change of the angular momentum: the larger it is, the more torque is applied by the GRF, thereby increasing the centroidal angular momentum. The CMP is the point from which the observed net ground reaction force vector would have to act in order to generate no torque about the CoM. Thus, it is that point where a line parallel to the GRF vector, passing through the CoM, intersects with the ground, as shown in Fig. 4.

Fig. 4

(Left) If there is no moment about the CoM (r_G), the GRF (f) points from the CoP (p) to the CoM position. (Right) If there is a moment about the CoM, the CoP and CMP diverge, where the separation distance is the moment arm associated with the vertical force, f_y

Let us derive the relations between CMP, CoM, and CoP. We designate the horizontal axes as x and z and the vertical axis as y (positive direction is upward). Given the CoM r_G = (r_Gx, r_Gy, r_Gz) and GRF f = (f_x, f_y, f_z), the position of the CoP along these axes, p_x and p_z, can be expressed by expanding Eq. (1):

$$\displaystyle \begin{aligned} \begin{aligned} p_{x} = r_{Gx} - \frac{f_{x}}{f_{y}} r_{Gy} + \frac{\tau_z}{f_{y}}\\ p_{z} = r_{Gz} - \frac{f_{z}}{f_{y}} r_{Gy} - \frac{\tau_x}{f_{y}} {} \end{aligned} \end{aligned} $$

(21)

where τ_x and τ_z are horizontal torque components generated by the GRF.

Let us denote the CMP by s. Since s −r_G is parallel to f, i.e., $\left ( \boldsymbol {s} - \boldsymbol {r}_{G} \right ) \times \boldsymbol {f} = 0$, expanding this cross product yields

$$\displaystyle \begin{aligned} \begin{aligned} s_x = r_{Gx} - \frac{f_{x}}{f_{y}} r_{Gy}\\ s_z = r_{Gz} - \frac{f_{z}}{f_{y}} r_{Gy} {} \end{aligned}\end{aligned} $$

(22)

Note that because f_x and f_z are the net horizontal forces on the CoM, this relation can be used to compute horizontal CoM force as a function of CoM position, CMP point location, and vertical GRF. Such horizontal forces are critical for maintaining bipedal stability since they can be applied to change CoM state to desired values.

By combining Eqs. 21 and 22, we obtain a relation between CoP and CMP:

$$\displaystyle \begin{aligned} \begin{aligned} s_x &=p_x - \frac{\tau_z}{f_y} \\ s_z &=p_z + \frac{\tau_x}{f_y} {} \end{aligned} \end{aligned} $$

(23)

Equation (23) shows that when there is no horizontal moment about the CoM, the CMP and CoP coincide. In this case, the GRF vector points directly to the CoM, as shown in Fig. 4 (left). Conversely, when there is a horizontal moment about the CoM, the CMP and CoP diverge. The horizontal separation distance between these points is the moment arm for the CoM moment due to vertical force, f_y, as shown in Fig. 4 (right). Note that as the CMP and CoP diverge, the CoP must remain within the support base, but the CMP may leave the region of support.

The relationship between the CoM and CMP indicates the specific effect that the net GRF has on CoM translation. Because the observed net GRF always operates at the CoP which is within the support base, whenever the net GRF generates no torque about the CoM, then the CoP and CMP coincide. If the net GRF generates torque, however, then the CMP and CoP differ in location, and, in particular, the CMP may be outside the support base.

It is sometimes desirable to have the CMP and CoP diverge as shown in Fig. 4 (right) so that horizontal CoM forces can be more effectively controlled. In this case the CMP can be displaced from the CoP which reflects the increased ability of the net GRF to affect translation of the CoM. The associated moment about the CoM generally produces undesirable effects, such as loss of upright orientation of the upper body. In many cases, these effects are temporary, can be managed, and are well worth the overall positive effect on CoM position and velocity. For example, a tightrope walker will tolerate temporary angular instability if this means that he won’t fall off the tightrope.

The use of the CMP is demonstrated in Fig. 5, which depicts recovery from a lateral disturbance. This sequence shows an initial disturbance that pushes the biped to the right. To compensate, the system takes control actions involving rotation of the body and swing leg, which move its CMP to the right, creating a lateral compensating force to the left. Because the disturbance is significant, the CMP moves beyond the edge of the support polygon, and thus, it does not coincide with the CoP. This compensating action corresponds to a clockwise torque about the CoM, which is manifested by clockwise rotation of the torso and right leg .

Fig. 5

Recovery from a lateral disturbance using CMP

4 Methods to Control Momentum

Using the relations between the momentum and the joint angles (Eqs. (15) and (20)), we can control the robot to achieve the desired momentum. In this section, we introduce two methods, one for determining the joint velocities to create the desired momentum and the other for computing the joint torques to create the given rate of change of the momentum.

4.1 Resolved Momentum Control

Kajita et. al [10] developed the resolved momentum control method , which determines the joint velocities that achieves the desired linear and angular momenta as well as additional constraints. Let us assume that the desired momentum h_d is given by a planner, and the robot must satisfy the constraints expressed as follows:

$$\displaystyle \begin{aligned} \boldsymbol{v}_c = {\boldsymbol J}_c \dot{\boldsymbol{q}}. {} \end{aligned} $$

(24)

For example, v_c may specify the desired velocities of the end effectors, i.e., zero velocity for the support foot, and J_c is an appropriate Jacobian matrix. The original method proposed in [10] divides joints into two parts, one for satisfying Eq. (24) (e.g., leg joints) and the rest. After determining joint velocities for the first part from Eq. (24), the velocities of the rest of the joints are determined to satisfy $\boldsymbol {h}_d = {\boldsymbol A}_G \dot {\boldsymbol {q}}$ Eq. (15).

Let us rewrite the method in a more general setting, without dividing the joints. The joint angles $\dot {\boldsymbol {q}}$ are determined as

$$\displaystyle \begin{aligned} \dot{\boldsymbol{q}} = \arg\min_{\dot{\boldsymbol{q}}} || \boldsymbol{h}_d - {\boldsymbol A}_G \dot{\boldsymbol{q}} || \quad \text{subject to }\quad \boldsymbol{v}_c = {\boldsymbol J}_c \dot{\boldsymbol{q}} {} \end{aligned} $$

(25)

The general solution for $\dot {\boldsymbol {q}}$ satisfying the constraint is $\dot {\boldsymbol {q}} = {\boldsymbol J}_c^\dagger \boldsymbol {v}_c + Null({\boldsymbol J}_c)\boldsymbol {\lambda }$ where ${\boldsymbol J}_c^\dagger$ is J_c’s pseudoinverse. Thus λ is determined from

$$\displaystyle \begin{aligned} \boldsymbol{\lambda} = \arg\min_{\boldsymbol{\lambda}} || \boldsymbol{h}_d - {\boldsymbol A}_G \left( {\boldsymbol J}_c^\dagger \boldsymbol{v}_c + Null({\boldsymbol J}_c)\boldsymbol{\lambda} \right) || \end{aligned} $$

(26)

Since the null space of J_c is not trivial to obtain, a practical way to compute Eq. (25) is to use the optimization algorithms for quadratic programming with linear equality constraints.

4.2 Momentum-Based Computed Torque Control

Equation (20) and inverse dynamics algorithm allow us to compute the joint torques to create the desired momentum rate changes under some constraints. Here we introduce the framework presented by [30, 31], which is based on the method of [5]. Figure 6 shows the block diagram for the controller.

Fig. 6

A momentum-based computed torque controller framework. $\boldsymbol {\ddot {\theta }}_d^u$ is the desired joint accelerations for the upper body. T_d,i and v_d,i are the desired configuration and spatial velocity of each foot (i = r, l). Subscripts d and a imply “desired” and “admissible,” respectively

Let us assume that the desired momentum rate change is given, but it may not always be physically realizable due to several constraints on the foot-ground contact: The CoP corresponding to the desired momentum rate change may lie outside the robot’s support base, and the GRF may violate the unilateral constraint () or the friction limit of the foot-ground surface.

Therefore, the next step determines the admissible values of GRF and CoP that will create the desired momentum rate change as close as possible while being physically realizable. Specifically, in order to make the controller robustly applicable to non-level and nonstationary ground, admissible foot GRFs and foot CoPs are directly determined, without using more conventional net GRF and net CoP of the robot. Assuming planar contact between the ground and each foot, the foot GRF is the ground reaction force acting on an individual foot, and foot CoP is the location where its line of action intersects the foot support plane. There can be many strategies to determine the admissible foot GRFs and foot CoPs depending on the objectives and preferences chosen by a controller. Using the values of admissible foot GRF and foot CoP, momentum rate changes are recalculated – these are the admissible momentum rate changes $\dot {\boldsymbol {h}}_a=(\dot {\boldsymbol {k}}_a,\dot {\boldsymbol {l}}_a)$.

Given the admissible foot GRFs and CoPs as well as corresponding admissible momentum rate change, we can compute the admissible joint accelerations and torques. The constraint equations (e.g., due to ground contacts) and the joint space equations of motion of the robot are as follows:

$$\displaystyle \begin{aligned} \boldsymbol{v}_c &= {\boldsymbol J}_c \dot{\boldsymbol{q}}{} \end{aligned} $$

(27)

$$\displaystyle \begin{aligned} \boldsymbol{\tau} &= {\boldsymbol H} \ddot{\boldsymbol{q}} + {\boldsymbol C}\dot{\boldsymbol{q}} + \boldsymbol{\tau}_g - {\boldsymbol J}_c^T\boldsymbol{f}_c {} \end{aligned} $$

(28)

where $\boldsymbol {\tau } \in \mathbb {R}^{n}$ denotes the generalized forces, H is the joint space inertia matrix, ${\boldsymbol C}\dot {\boldsymbol {q}}$ includes Coriolis and centrifugal terms, and τ_g is the gravity torque. f_c is a vector representing external “constraint” forces from the environment, determined by foot GRFs and CoPs, and the Jacobian ${\boldsymbol J}_c\in \mathbb {R}^{c\times n}$ transforms f_c to the generalized forces. The number of constraint equations c depends on the nature of constraint at the foot-ground contact. For example, when both the linear and angular motions of the support foot are constrained due to foot-ground contact, c = 6 for single support and c = 12 for double support. In this case, ${\boldsymbol J}_c\dot {\boldsymbol {q}}$ denotes the linear and angular velocities of the support foot given $\dot {\boldsymbol {q}}$. For a stationary ground, v_c is set to zero.

Since the robot base is free-floating, the first six elements of τ are zero, i.e., $\boldsymbol {\tau }^T = [ \boldsymbol {0}^T~ \boldsymbol {\tau }_s^T ]$. Hence, we can divide Eq. (28) into two parts, one corresponding to the base, denoted by the subscript 0, and the other, subscripted with s, for the joints.

$$\displaystyle \begin{aligned} {\boldsymbol{0}} &= {\boldsymbol H}_0 \ddot{\boldsymbol{q}} + {\boldsymbol C}_0\dot{\boldsymbol{q}} + \boldsymbol{\tau}_{g,0} - {\boldsymbol J}_{c,0}^T \boldsymbol{f}_c {} \end{aligned} $$

(29)

$$\displaystyle \begin{aligned} \boldsymbol{\tau}_s &= {\boldsymbol H}_s \ddot{\boldsymbol{q}} + {\boldsymbol C}_s\dot{\boldsymbol{q}} + \boldsymbol{\tau}_{g,s} - {\boldsymbol J}_{c,s}^T\boldsymbol{f}_c {} \end{aligned} $$

(30)

Note that Eq. (29) is equivalent to Eq. (20). Equation (29) refers to the global dynamics expressed with respect to the base link, whereas Eq. (20) is expressed with respect to the CoM frame. Therefore, we rewrite Eqs. (27) and (28) as follows:

$$\displaystyle \begin{aligned} \dot{\boldsymbol{v}}_c &= {\boldsymbol J}_c\ddot{\boldsymbol{q}} + \boldsymbol{\dot{J}}_c \dot{\boldsymbol{q}} {} \end{aligned} $$

(31)

$$\displaystyle \begin{aligned} \dot{\boldsymbol{h}}_G & = \boldsymbol{A}_G \ddot{\boldsymbol{q}} + \dot{\boldsymbol{A}}_G \dot{\boldsymbol{q}} {} \end{aligned} $$

(32)

$$\displaystyle \begin{aligned} \boldsymbol{\tau}_s &= {\boldsymbol H}_s \ddot{\boldsymbol{q}} + {\boldsymbol C}_s\dot{\boldsymbol{q}} + \boldsymbol{\tau}_{g,s} - {\boldsymbol J}_{c,s}^T\boldsymbol{f}_c {} \end{aligned} $$

(33)

where Eq. (31) is the time derivative of Eq. (27).

Due to the high DoFs of humanoid robots, balance controllers usually solve an optimization problem. However, the computational cost of the optimization increases rapidly as the dimension of the search space increases. Even the simplest optimization problem such as the least-squares problem has order O(n³) time complexity. Therefore, aiming for computational efficiency, we divide the balance control problem into three smaller subproblems, which can be solved serially. The balance controller determines the control input τ_s through the following steps by sequentially determining f_c, $\ddot {\boldsymbol {q}}$, and τ_s, in that order:

Step 1::

Admissible foot GRFs and foot CoPs (hence f_c) are computed from the desired momentum rate change.

Step 2::

Joint accelerations $\ddot {\boldsymbol {q}}$ are determined such that they satisfy both Eq. (31) and Eq. (32), with $\dot {\boldsymbol {h}}_G$ being set to $\dot {\boldsymbol {h}}_a$. Equation (31) may include not only the foot constraints but also additional constraints such as the hand movement. In general, if the total number of robot DoFs is greater than or equal to c + 6, a solution to $\ddot {\boldsymbol {q}}$ exists.

One method for computing the output accelerations $\ddot {\boldsymbol {q}}$ is to make it minimize the following objective function:

$$\displaystyle \begin{aligned}\begin{aligned} \ddot{\boldsymbol{q}}_a = \underset{\ddot{\boldsymbol{q}}}{\operatorname{argmin}}~w_b|| \dot{\boldsymbol{h}}_a - \boldsymbol{A}_G\ddot{\boldsymbol{q}} - \dot{\boldsymbol{A}}_G \dot{\boldsymbol{q}} || + (1-w_b)|| \boldsymbol{\ddot{\theta}}^u_d - \boldsymbol{\ddot{\theta}}^u || \\ \text{s.t. } {\boldsymbol J}\ddot{\boldsymbol{q}} + \boldsymbol{\dot{J}}\dot{\boldsymbol{q}} = \dot{\boldsymbol{v}_c} \quad \text{and} \quad \boldsymbol{\ddot{\theta}}_l \le \boldsymbol{\ddot{\theta}} \le \boldsymbol{\ddot{\theta}}_u, \end{aligned} {} \end{aligned} $$

(34)

where $\boldsymbol {\ddot {\theta }} (\subset \ddot {\boldsymbol {q}})$ denotes the joint accelerations except those of the float base and the superscript u denotes the joints of the upper body. Because there can be infinitely many solutions for $\boldsymbol {\ddot {\theta }}_a$ that create $\dot {\boldsymbol {h}}_a$, Eq. (34) adds an additional optimality criterion, which is to follow the desired joint acceleration of the upper body $\boldsymbol {\ddot {\theta }}^u_d$ as closely as possible. One can set $\boldsymbol {\ddot {\theta }}^u_d$ to specify an upper-body task or set $\boldsymbol {\ddot {\theta }}^u_d={\boldsymbol {0}}$ to minimize the movement. The parameter w_b(0 < w_b < 1) controls the relative importance between the balance objective (the first term) and the prescribed motion objective associated with the kinematic task (the second term). It is to be noted that w_b should be close to 1 in order to create admissible momentum rate, but it cannot be exactly 1 because in this case, Eq. (34) becomes indeterminate. Equation (34) can be easily converted to a least-squares problem with linear equality and bound constraints, and many solvers, e.g., [36], are available for this type of problem.

Step 3::

the required joint torques τ_s satisfying Eq. (33) are computed from f_c and $\ddot {\boldsymbol {q}}$ using an inverse dynamics algorithm. Note that, by computing f_c and $\ddot {\boldsymbol {q}}$ first, we have all the necessary information for inverse dynamics. Therefore, we can use efficient linear time algorithms for inverse dynamics, such as Featherstone’s hybrid dynamics [34], without having to compute the joint space equations of motion Eq. (28) which have a quadratic time complexity.

Overall torque input is determined by adding feedback terms:

$$\displaystyle \begin{aligned} \boldsymbol{\tau}_s &= \boldsymbol{\tau}_{ff} + \boldsymbol{\tau}_{fb} {} \end{aligned} $$

(35)

$$\displaystyle \begin{aligned} \boldsymbol{\tau}_{fb} &= \boldsymbol{\varGamma}_p ( \boldsymbol{\theta}^\ast - \boldsymbol{\theta} ) + \boldsymbol{\varGamma}_d ( \boldsymbol{\dot{\theta}}^\ast - \boldsymbol{\dot{\theta}}), \end{aligned} $$

(36)

where Γ_p = diag(γ_p,i) and Γ_d = diag(γ_d,i) are proportional and derivative gains, respectively. Position and velocity commands θ^∗, $\boldsymbol {\dot {\theta }}^\ast$ are determined from the time integration of $\boldsymbol {\ddot {\theta }}_a$.

5 Approaches to Momentum-Based Balance Control

This section describes approaches to momentum-based balance control . Although they emphasize different aspects, they are complementary in that they both control momentum of the humanoid.

The first, described in Sect. 4.2, accepts a direct input of the desired rate of change of linear and angular momentum, and desired movement of the torso and feet, and moves the humanoid so that these desired values are achieved. It can interface directly to a higher-level motion planner that generates trajectories for desired momentum.

The second controller, described in Sect. 5.2, uses a feedback linearization technique to make the complex nonlinear biped appear to be a set of loosely coupled linear systems. Then, PID control laws can be applied to quantities of interest, such as the CoM, or reaction points of interest in the kinematic chain, such as ones on the torso, head, feet, or hands. Thus, balance task goals can be specified in terms of desired position and velocity of the CoM, which is directly related to system linear momentum, and goals for reaction point movement (linear and angular). By default, system angular momentum is minimized. With this approach, it is easy to specify tasks like balancing in single or double support modes and also dynamic walking tasks, which alternate between single and double support modes.

Setting the desired linear momentum is relatively straightforward because it is directly related with CoM trajectory, but the issue of setting the desired angular momentum largely remains an open problem. In the last Sect. 5.3, we present some approaches proposed so far to set the desired angular momentum.

5.1 Stationary Balance Using Momentum-Based Computed Torque Control

In this section, we show how the momentum control framework introduced in Sect. 4.2 is employed to create stationary balance. The objective of the controller is to maintain the balanced pose under external perturbations of relatively short durations. The momentum controller in Sect. 4.2 requires two types of specifications; one is the specification of the desired momentum rate change, and the other is the strategy to resolve the admissible foot GRFs and foot CoPs. Given the two specifications, the momentum controller outputs the joint torques to realize the desired momentum.

5.1.1 Determination of the Desired Momentum

The overall behavior of the robot against external perturbations is determined by the desired momentum rate change. For the stationary balance, the following feedback control policy can be used:

$$\displaystyle \begin{aligned} \dot{\boldsymbol{k}}_d &= \boldsymbol{\varGamma}_{11} (\boldsymbol{k}_d - \boldsymbol{k}) {} \end{aligned} $$

(37)

$$\displaystyle \begin{aligned} \dot{\boldsymbol{l}}_d/m &= \boldsymbol{\varGamma}_{21}(\boldsymbol{\dot{r}}_{G,d}-\boldsymbol{\dot{r}}_G) + \boldsymbol{\varGamma}_{22}(\boldsymbol{r}_{G,d}-\boldsymbol{r}_G) {} \end{aligned} $$

(38)

where $\dot {\boldsymbol {k}}_d$ and $\dot {\boldsymbol {l}}_d$ are the independently specified desired rates of change of centroidal angular and linear momenta. Additionally, r_G,d is the desired CoM position. Γ_ij represents 3×3 diagonal matrix of feedback gain parameters. Note that unlike the linear position feedback term in Eq. (38), an angular position feedback is omitted in Eq. (37). This is because a physically meaningful angular “position” cannot be defined corresponding to angular momentum [37]. For postural balance maintenance experiments, we set k_d and $\boldsymbol {\dot {r}}_{G,d}$ to zero and r_G,d to be above the midpoint of the geometric centers of the two feet. For other cases, these values may be determined from the desired motion.

5.1.2 Strategy for the Admissible Momentum Rate Change, Foot GRFs, and Foot CoPs

Given the desired momentum rate change, admissible foot GRF and foot CoP are determined such that the resulting momentum rate change is as close as possible to the desired value. If the desired GRF and CoP computed from $\dot {\boldsymbol {k}}_d$ and $\dot {\boldsymbol {l}}_d$ violate physical constraints (e.g., GRF being outside friction cone, normal component of GRF being negative, or CoP being outside support base), it is not possible to generate those $\dot {\boldsymbol {k}}_d$ and $\dot {\boldsymbol {l}}_d$. In this case we must strike a compromise and decide which quantity out of $\dot {\boldsymbol {k}}_d$ and $\dot {\boldsymbol {l}}_d$ is more important to preserve.

Figure 7 illustrates one case where the desired CoP, p_d, computed from the desired momentum rate change is outside the support base, indicating that it is not admissible. Two different solutions are possible. The first solution, shown in Fig. 7, left, is to translate the CoP to the closest point of the support base while keeping the magnitude and line of action of the desired GRF f_d unchanged. In this case the desired linear momentum is attained, but the desired angular momentum is compromised. The behavior emerging from this choice is characterized by a trunk rotation. This strategy can be observed in the human when the trunk yields in the direction of the push to maintain balance. Alternatively, in addition to translating the CoP to the support base, as before, we can rotate the direction of the GRF, as shown in Fig. 7, right. In this case the desired angular momentum is attained, while the desired linear momentum is compromised. With this strategy the robot must move linearly along the direction of the applied force due to the residual linear momentum, making it necessary to step forward to prevent falling.

Fig. 7

When the desired GRF, f_d, and the desired CoP, p_d, computed from the desired momentum rate change are not simultaneously admissible, as indicated by p_d being outside the support base, momenta objectives need to be compromised for control law formulation. Two extreme cases are illustrated. Left: linear momentum is respected while angular momentum is compromised. Right: angular momentum is respected while linear momentum is compromised

For the stationary balance, it is natural to give higher priority to preserving linear momentum over angular momentum because it increases the capability of postural balance without involving a stepping. Ideally, a smart controller should be able to choose optimal strategies depending on the environment conditions and the status of the robot. The approaches that give higher priority to linear momentum and sacrifice angular momentum can also be found in the literature [13, 29] and in the work on stepping [38].

Foot GRFs and CoPs are determined differently depending on the support configurations.

Single Support Case:

Dealing with single support case is straightforward because the foot GRF and CoP are uniquely determined from the desired momentum rate change, from Eqs. (1) and (2):

$$\displaystyle \begin{aligned} \boldsymbol{f}_d &= \dot{\boldsymbol{l}}_d - m\boldsymbol{g} {} \end{aligned} $$

(39)

$$\displaystyle \begin{aligned} p_{d,x} &= r_{Gz} - \frac{1}{\dot{\boldsymbol{l}}_{d,y} - mg} ( f_{d,x}\: r_{Gy} - \dot{k}_{d,z} ){} \end{aligned} $$

(40)

$$\displaystyle \begin{aligned} p_{d,z} &= r_{Gz} - \frac{1}{\dot{\boldsymbol{l}}_{d,y} - mg} ( f_{d,z}\: r_{Gy} + \dot{k}_{d,x} ) {} \end{aligned} $$

(41)

If and p_d computed above are valid, then these values are directly used. Otherwise, as mentioned previously, higher priority is given to linear momentum, which is achieved by unchanging and projecting p_d inside the support base. In the case that is outside the friction cone, it is projected onto the friction cone to prevent foot slipping. Admissible momentum rate change $\dot {\boldsymbol {h}}_a=[\dot {\boldsymbol {k}}_a^T~\dot {\boldsymbol {l}}_a^T]^T$ is computed from the modified foot GRF and CoP using Eqs. (1) and (2).

Double Support Case:

As there are multiple contact parts, the mapping from the foot GRF/CoP to the momentum rate change is many to one, which means that there are multiple solutions for foot GRFs and CoPs given the input momentum rate change. One can utilize this indeterminacy to pursue secondary objectives, e.g., minimizing ankle torques. We refer the readers to the original paper [31] for details.

5.1.3 Simulation Experiment

Figure 8 shows a simulation experiment of the balance controller for a full-sized humanoid robot whose total mass is about 50 kg and each leg has 6 DoFs. The control command was generated every 1 ms. When the push magnitude is small, the desired GRF and CoP computed from the desired momentum rate change are both admissible, and thus the robot can achieve the desired values for both linear and angular momentum. When the perturbation is larger, the desired values are different from the admissible values, and in order to maintain balance without stepping, the controller tries to preserve the CoM location by modulating angular momentum by rotating the upper body. The resulting motion of the robot is similar to that of a human rotating the trunk in the direction of the push to maintain balance.

Fig. 8

Top row: Given a forward push (120 N, 0.1 s), the balance controller controls both linear and angular momenta and generates a motion comparable to human’s balance control behavior. The robot is standing on stationary level platforms. (a–d): Trajectory of important physical properties of the experiment. Small circles in each figure indicate the start and end of the push. The dotted line in (g) indicates the front and rear borders of the safe region of the support, which is set to a few millimeters inside of the edge of the foot base. Left foot CoP is very similar to the right foot CoP

The top row of Fig. 8 shows a series of snapshots illustrating this when the robot is subjected to an external push (120 N, 0.1 s) applied at the CoM in the forward direction (Fig. 8a). Before 0.2 s and after 0.65 s in the test, the admissible foot GRF (Fig. 8d) and foot CoP (Fig. 8g) can be determined such that they create the desired momentum rate change, so the admissible momentum rate change during that time period is nearly identical to its desired value (Figs. 8c, f). However, from 0.2 to 0.65 s, the admissible foot CoP (Fig. 8g) is kept on the front border of the safe region of the support, marked with dotted line. The controller gives higher priority to linear momentum (Fig. 8b) so the admissible linear momentum rate change is still same as the desired value, while the angular momentum objective is compromised (Fig. 8e), as shown by the difference between the desired and admissible values of angular momentum rate change in Fig. 8f.

Figure 8g shows the measured foot CoP, which is calculated using the contact force information during the simulation. Ideally it should be the same as the admissible foot CoP, but actually they are slightly different because of the inclusion of the prescribed motion objective in Eq. (34) as well as the numerical error of the simulation.

5.1.4 Application to a Real Robot and Extension to Stepping Controller

The controller strategy has been modified and applied to a real torque-controlled robot. The readers are referred to [39]. The effectiveness of the control framework in Sect. 5.1 has not only been demonstrated for the postural balance control but also for the stepping controller. Yoon and Goswami [38] developed a momentum-based balance controller that generates a stepping motion under strong perturbations. The stepping motion is generated using a rimless wheel model. Choi et al. [40] developed a similar momentum-based stepping controller, in which the stepping motion is generated without preplanned trajectories.

5.2 Angular Momentum-Exploiting Balance Control

The balance controller in Sect. 5.1 explicitly specifies the desired and admissible values of the angular momentum for the purpose of maintaining postural balance. Another approach we introduce here is achieving the similar result by dealing with angular momentum implicitly. The idea is to let the controller exploit the angular momentum to enhance the postural control. The control goals are specified as desired state (position and velocity) of the reaction points, combined with corresponding PD control law gains. The controller outputs joint accelerations and corresponding torques to achieve the movement goals. The original controller has been developed for a seven-link humanoid robot [29]. Here, we present the method in the form available to a robot with higher DoFs.

5.2.1 Controller Architecture

The controller uses a model-based input-output linearization algorithm [41] to linearize and decouple the plant. The input-output linearization approach is augmented with a slack variable relaxation technique to accommodate actuation constraints and prioritize goals. This feature is important because it is not always possible to achieve all control goals simultaneously. Actuation constraints, such as the requirement that the CoP must remain well inside the support base in the case where foot roll is undesirable, may cause the overall system to become over-constrained, in which case some goals must be deferred. To address this problem, the controller incorporates a goal prioritization algorithm that automatically sacrifices lower-priority goals when the system becomes over-constrained in this way. For example, the system may temporarily sacrifice goals of maintaining upright posture in order to achieve CoM state goals. We now describe the linearization and goal prioritization components of the controller in more detail.

Desired motion behavior for the outputs is specified in a simple, straightforward way, using a linear proportional-differential (PD) law:

$$\displaystyle \begin{aligned} \ddot{\boldsymbol{y}}_d = \boldsymbol{\varGamma}_p \left( \boldsymbol{y}_d - \boldsymbol{y}\right) - \boldsymbol{\varGamma}_d \dot{\boldsymbol{y}} {} \end{aligned} $$

(42)

where y denotes the vector of outputs to be controlled. For instance, y may include the CoM position, orientation of the upper body, as well as the position and orientation of a swing foot. y_d and $\dot {\boldsymbol {y}}_d$ are position and velocity setpoint vectors, and Γ_p and Γ_d are proportional and damping gains. Such a control law can be represented as a set of virtual spring-damper elements attached to the output points being controlled so that the controlled outputs move as if they were point masses attached to these spring-damper systems. A geometric transform, x, is used to convert from the joint state to the workspace (output) state representation, according to

$$\displaystyle \begin{aligned} \boldsymbol{y} = \boldsymbol{x} \left( \boldsymbol{q} \right) {} \end{aligned} $$

(43)

where q is the joint position vector and y is the output vector.

The controller uses a feedback linearizing transformation to convert desired workspace variable accelerations, $\ddot {\boldsymbol {y}}$, into corresponding joint torques. The linearization is accomplished using a two-stage process; Given a desired output acceleration vector, $\ddot {\boldsymbol {y}}_{d}$, which is computed by the PD law, we first compute the corresponding joint acceleration vector, $\ddot {\boldsymbol {q}}_{d}$, using a geometric transformation. Then, we compute the joint torque vector, τ, which achieves $\ddot {\boldsymbol {q}}_{d}$, using an inverse dynamics transformation.

In order to obtain a relation between $\ddot {\boldsymbol {y}}_{d}$ and $\ddot {\boldsymbol {q}}_{d}$, we differentiate Eq. 43 twice to obtain

$$\displaystyle \begin{aligned} \begin{array}{rcl} \dot{\boldsymbol{y}} = \frac{\partial \boldsymbol{x}}{\partial \boldsymbol{q}} \dot{\boldsymbol{q}} ={\boldsymbol J} \dot{\boldsymbol{q}} \end{array} \end{aligned} $$

(44)

$$\displaystyle \begin{aligned} \begin{array}{rcl} \ddot{\boldsymbol{y}} ={\boldsymbol J} \ddot{\boldsymbol{q}} + \dot{{\boldsymbol J}} \dot{\boldsymbol{q}} ={\boldsymbol J} \ddot{\boldsymbol{q}} + \varPsi {} \end{array} \end{aligned} $$

(45)

where J is the Jacobian matrix. The matrix J and the vector Ψ are functions of joint state. The inverse dynamics computation is of the form

$$\displaystyle \begin{aligned} {\boldsymbol H} \left( \boldsymbol{q} \right) \ddot{\boldsymbol{q}} + {\boldsymbol C} \left( \boldsymbol{q}, \dot{\boldsymbol{q}} \right) + \boldsymbol{\tau}_g \left( \boldsymbol{q} \right) = \boldsymbol{\tau} {} \end{aligned} $$

(46)

where ${\boldsymbol H} \left ( \boldsymbol {q} \right )$ is a matrix of inertial terms, ${\boldsymbol C} \left ( \boldsymbol {q}, \dot {\boldsymbol {q}} \right )$ is a matrix of velocity-related terms, and $\boldsymbol {\tau }_g \left ( \boldsymbol {q} \right )$ is a vector of gravitational terms. Note that Eq. 46 corresponds to Eq. 30, but with the external forces omitted (external forces could be included as well but are omitted for simplicity).

Therefore, for a particular joint state $\left [ \boldsymbol {q}^T, \dot {\boldsymbol {q}}^T \right ]$, Eq. 45 represents a linear relation between $\ddot {\boldsymbol {q}}$ and $\ddot {\boldsymbol {y}}$. To achieve the linearization we combine Eqs. 46 and 45:

$$\displaystyle \begin{aligned} \begin{bmatrix} {\mathbf{1}}_{12 \times 12} & {\mathbf{0}}_{12 \times 12} & {\mathbf{0}}_{12 \times 12} \\ {\mathbf{1}}_{12 \times 12} & -{\boldsymbol J} & {\mathbf{0}}_{12 \times 12} \\ {\mathbf{0}}_{12 \times 12} & {\boldsymbol H} & - {\mathbf{1}}_{12 \times 12} \end{bmatrix} \begin{bmatrix} \ddot{\boldsymbol{y}} \\ \ddot{\boldsymbol{q}} \\ \boldsymbol{\tau} \end{bmatrix} = \begin{bmatrix} \ddot{\boldsymbol{y}}_{d} \\ \varPsi \\ - {\boldsymbol C} \end{bmatrix} {} \end{aligned} $$

(47)

The system represented by Eq. 47 can be over-constrained if inequality constraints, which represent actuation limits, are introduced, and these constraints become active. An important constraint of this type is the requirement to keep the stance foot flat on the ground during single support; while balancing on one leg, it is undesirable for the stance foot to roll, particularly on its lateral edge. This particular constraint is accomplished by requiring the CoP to be inside the edge of the support envelope. Note that this constraint is distinct from the physical constraint that the CoP not be outside the support base. If the CoP is on the edge of the support envelope, the foot will begin to roll [35]. Hence, in order to avoid foot roll, we employ linear inequality constraints to keep the CoP inside the edge of the support envelope. For the humanoid model, the CoP is given by expanding Eq. 21, or

$$\displaystyle \begin{aligned} x_{CoP} &= \frac{\sum m_i r_{xi} \left( \ddot{r}_{zi} + g\right) - \sum m_i r_{zi} \ddot{r}_{xi} - \sum \tau_{yi}} {\sum m_i \left( \ddot{r}_{zi} + g\right)} \end{aligned} $$

(48)

$$\displaystyle \begin{aligned} y_{CoP} &= \frac{\sum m_i r_{yi} \left( \ddot{r}_{zi} + g\right) - \sum m_i r_{zi} \ddot{r}_{yi} + \sum \tau_{xi}} {\sum m_i \left( \ddot{r}_{zi} + g\right)} \end{aligned} $$

(49)

$$\displaystyle \begin{aligned} \boldsymbol{\tau}_{xi} &= {\mathbf{1}}_{Gi} \dot{\boldsymbol{\omega}_{xi}} \end{aligned} $$

(50)

$$\displaystyle \begin{aligned} \boldsymbol{\tau}_{yi} &= {\mathbf{1}}_{Gi} \dot{\boldsymbol{\omega}_{yi}}{} \end{aligned} $$

(51)

where i is the segment index; r_xi, r_yi, and r_zi denote the CM position of segment i; 1_Gi is the inertia matrix of segment i; and ω_xi and ω_yi are the angular velocities of segment i about the x- and y-axes, respectively. The moments, τ_xi and τ_yi, are about the segment i CM in the x and y axes, respectively. These equations are transformed into a set of linear inequality constraints by replacing x_CoP and y_CoP with min and max terms, reflecting the bounds, so that these become constants: The constraint of the boundedness of CoP can be expressed as a set of linear inequality constraints:

$$\displaystyle \begin{aligned} {\boldsymbol H}_r \ddot{\boldsymbol{y}}_r \leq {\mathbf{K}}_r {} \end{aligned} $$

(52)

for some suitably chosen matrix H_r and vector K_r. where

$$\displaystyle \begin{gathered} {\boldsymbol H}_r = \begin{bmatrix} - {\mathbf{m}}^T \bullet {\mathbf{r}}_z^T & {\mathbf{0}}_{1 \times 6} & \left( -{\mathbf{x}}_{max} + {\mathbf{m}}^T \bullet {\mathbf{r}}_x^T \right) & {\mathbf{0}}_{1 \times 6} & - {\mathbf{1}}_y \\ {\mathbf{m}}^T \bullet {\mathbf{r}}_z^T & {\mathbf{0}}_{1 \times 6} & \left( {\mathbf{x}}_{min} - {\mathbf{m}}^T \bullet {\mathbf{r}}_x^T \right) & {\mathbf{0}}_{1 \times 6} & {\mathbf{1}}_y \\ {\mathbf{0}}_{1 \times 6} & - {\mathbf{m}}^T \bullet {\mathbf{r}}_z^T & \left( -{\mathbf{y}}_{max} + {\mathbf{m}}^T \bullet {\mathbf{r}}_y^T \right) & {\mathbf{1}}_x & {\mathbf{0}}_{1 \times 6} \\ {\mathbf{0}}_{1 \times 6} & {\mathbf{m}}^T \bullet {\mathbf{r}}_z^T & \left( {\mathbf{y}}_{min} - {\mathbf{m}}^T \bullet {\mathbf{r}}_y^T \right) & - {\mathbf{1}}_x & {\mathbf{0}}_{1 \times 6} \end{bmatrix} {} \end{gathered} $$

(53)

$$\displaystyle \begin{gathered} \ddot{\mathbf{y}}_r = \begin{bmatrix} \ddot{\mathbf{r}}_x^T & \ddot{\mathbf{r}}_y^T & \ddot{\mathbf{r}}_z^T & \dot{\boldsymbol{\omega}}_x^T & \dot{\boldsymbol{\omega}}_y^T \end{bmatrix}^T {} \end{gathered} $$

(54)

$$\displaystyle \begin{gathered} {\mathbf{K}}_r = \begin{bmatrix} g \left( m_{tot} x_{copmax} - {\mathbf{m}}^T \bullet {\mathbf{r}}_x^T \right)^T \\ -g \left( m_{tot} x_{copmin} - {\mathbf{m}}^T \bullet {\mathbf{r}}_x^T \right)^T \\ g \left( m_{tot} y_{copmax} - {\mathbf{m}}^T \bullet {\mathbf{r}}_y^T \right)^T \\ -g \left( m_{tot} y_{copmin} - {\mathbf{m}}^T \bullet {\mathbf{r}}_y^T \right)^T \\ \end{bmatrix} {} \end{gathered} $$

(55)

$$\displaystyle \begin{gathered} {\mathbf{x}}_{max} = x_{copmax} {\mathbf{m}}^T \end{gathered} $$

(56)

$$\displaystyle \begin{gathered} {\mathbf{x}}_{min} = x_{copmin} {\mathbf{m}}^T \end{gathered} $$

(57)

$$\displaystyle \begin{gathered} {\mathbf{y}}_{max} = y_{copmax} {\mathbf{m}}^T \end{gathered} $$

(58)

$$\displaystyle \begin{gathered} {\mathbf{y}}_{min} = y_{copmin} {\mathbf{m}}^T {} \end{gathered} $$

(59)

where m is a vector of masses for the segments; r_x, r_y, and r_z are vectors of the segments’ CM x, y, and z positions; 1_x and 1_y are vectors of the segments’ inertias about the x- and y-axes; and x_copmin, x_copmax, y_copmin, and y_copmax are the CoP limits. The operator • represents element-wise multiplication.

Now, r_x, r_y, r_z, 1_x, and 1_y are kinematic functions of q, and m is a constant. y_r is a vector of kinematic variables related with CoP. Therefore, Eq. 52 is linear with respect to the current joint state. Furthermore, $\ddot {\boldsymbol {y}}_r$ is related to the joint acceleration vector through a linear function similar to Eq. 45:

$$\displaystyle \begin{aligned} \ddot{\boldsymbol{y}}_r ={\boldsymbol J}_r \ddot{\boldsymbol{q}} + \varPsi_r {} \end{aligned} $$

(60)

If we combine the inequality constraints of Eq. 60 with 52, we have an overall system that may be over-constrained. One way to deal with this over-constrained problem is to use “slack” variables that provide flexibility to the overall system. Thus, the controller output, $\ddot {\boldsymbol {y}}_{contout}$, is given by

$$\displaystyle \begin{aligned} \ddot{\boldsymbol{y}}_{d} = \ddot{\boldsymbol{y}}_{contout} + \ddot{\boldsymbol{y}}_{slack} {} \end{aligned} $$

(61)

where $\ddot {\boldsymbol {y}}_{slack}$ is the vector of slack variables. The goal of the overall control system is then to minimize $\ddot {\boldsymbol {y}}_{slack}$, taking into account the relative importance of each element.

This minimization is accomplished by formulating the control problem as a quadratic program (QP) and then using a QP optimizer to solve

$$\displaystyle \begin{aligned} \min \ddot{\boldsymbol{y}}^T_{slack} {\boldsymbol W} \ddot{\boldsymbol{y}}_{slack} \quad \text{subject to Eqs. (47) and (52)} \end{aligned} $$

(62)

The relative importance of the slack variables is expressed in the cost function for the QP via a diagonal matrix W. This causes the optimizer to prioritize goals by first minimizing the slack variables for the most important outputs and, therefore, setting $\ddot {\boldsymbol {y}}_{contout}$ to be as close as possible to $\ddot {\boldsymbol {y}}_{d}$ for these outputs. For example, slack variables associated with the CoM position output are given higher cost than those associated with trunk and swing leg orientation. The slack variable costs were determined empirically. Their precise value is not crucial; as long as the slack costs for CoM position are higher than the slack costs for the other outputs, desired behavior is achieved.

5.2.2 Balancing on a Single Support

Here, we show how the controller automatically sacrifices angular momentum goals, temporarily, in order to achieve balance goals. This test was performed with the humanoid model in single support. Initial conditions were such that the ground projection of the CoM was outside the support polygon, and all velocities were set to zero. For such initial conditions, the CoM cannot be stabilized by stance ankle torques alone without the foot rolling and the model going unstable. Simple reference trajectories consisting of single, time-invariant set points were selected for the controller. These set points specified the desired equilibrium positions and velocities of the model’s CoM and swing leg foot. Because the desired final equilibrium posture was to stand on one leg assuming a static pose, all set point velocities were set to zero.

Figure 9 shows the system’s recovery from an initial displacement in the lateral (positive y) direction. From the model’s perspective, the leftmost edge of the foot support polygon is at 0.05m. As is shown in B, the CoP remains within the foot support polygon, while the laterally displaced CoM position begins outside the stance foot, but is brought quickly to zero by the controller. Part C shows the desired, actual, and slack values for the lateral CoM acceleration in the optimal controller. Note how the slack goes to zero quickly, due to its high penalty. Part D shows the roll angle of the body. Because roll angle is less tightly controlled (penalty on slack variable is less than for CoM position), the angle converges, but more slowly than the lateral CoM position.

Fig. 9

Lateral disturbance recovery. In (a), several frames of the model are shown, starting from the maximally displaced CoM posture (left most image) to the final static equilibrium posture (rightmost image). From the perspective of the model, the right leg is the swing leg and the left the stance leg. In (b), the lateral direction CoM (dotted line) and the CoP (solid line) are plotted versus time. In (c), the desired CoM acceleration (solid line), the actual CoM acceleration (heavy dashed line), and the slack value (dotted line) are plotted, showing the stabilization of the model’s CoM. Finally the body roll is plotted in (d), showing the corrective measures taken by the controller

5.3 Specifying Nonzero Desired Angular Momentum

In the momentum control framework, specifying a suitable momentum objective is crucial for achieving the given task of the controller. Setting the desired linear momentum is relatively straightforward because it is directly related with the CoM trajectory, which is dealt by almost every existing controllers. What is not straightforward is setting the desired angular momentum.

A widely selected strategy is to set it to zero, as shown in Sect. 5.1. For a postural balance, setting it to zero is reasonable because the controller should drive the robot to have a static pose. Besides the static balance, zero angular momentum objective has been adopted to create a hopping motion [42] and a walking motion. However, it is to be noted that the general hopping and walking motion should involve nonzero angular momentum, so setting them zero should be enforced as a weak objective that preferably tries to keep the kinetic energy of the motion low.

A large repertoire of motions that a humanoid robot needs to generate necessarily involves nonzero angular momentum. For example, even a simple case like a robot swings his arms while standing still requires nonzero angular momentum; if the controller enforces the zero angular momentum, the robot will create some awkward motion to compensate for the angular momentum created by the swing motion. However, despite the various studies on angular momentum in humanoid motions, the issue of how to set the desired angular momentum for more complex motions such as locomotion has not been fully explored and remains an important future work. Here we present two methods to determine desired angular momentum.

5.3.1 Controlling Only Partial Directions of Momentum

In their original resolved momentum control approach, Kajita et al. [10] noted that specifying desired momentum in all six directions may result in unnatural motion and sometime instabilities (presumably if the over-constrained objectives are not resolved well). A simple example is to make a kicking motion; if the controller attempts to maintain the angular momentum in all three directions zero, the robot will bend its upper body to compensate for the leg-lifting motion. A proposed solution is to specify the desired values for the momentum to only a subset of directions, not all six directions. The authors obtained natural kicking and walking motions by setting the desired values to the linear momentum and the yaw direction of the angular momentum, while setting angular momentum in pitch and roll angles free.

5.3.2 Setting from a Kinematic Reference Motion

Another interesting approach has been proposed by Herzog et al. [43], who set the desired angular momentum from a kinematic reference motion. Specifically, they generate whole-body trajectories from a simple inverse kinematic integration, from which the desired linear and angular momentum trajectories are computed by using Eq. (15). The reference kinematic motion is generated from the trajectories of the CoM and the end effectors. Of course, this angular momentum is not truly desired value and may even be dynamically infeasible. But still it takes into account the given task (e.g., leg swing) and may better serve the purpose than the zero desired angular momentum .

6 Discussion and Open Questions

The last section of the chapter concludes with the discussion on the motion planning for the dynamic walking controller and open questions.

6.1 Balance Control for Dynamic Walking

Although balance control, as shown in the results of the previous sections, is an end goal for many typical bipedal locomotion tasks, this does not imply that full balance control must be maintained throughout such tasks. With typical dynamic walking, the single support or “stepping” phase does not satisfy this constraint. For example, consider the case where the biped is in right single support, with the left leg stepping. The right leg pushes the CoM laterally to the opposite (left) side. Toward the end of this phase, the right leg no longer has the ability to push the CoM laterally back to the near (right) side; there is too much momentum to the left and not enough of a support base. This is usually ok because the subsequent double support phase “catches” the system. Thus, it is not uncommon for full balance control to be intermittent, and any control system should take this into account. Further, as discussed above, it is often desirable to sacrifice some posture goals in favor of balance control goals. Such sacrifice will typically be intermittent, in response to disturbances, allowing upright posture to be restored by the end of the task. The controller should be intelligent enough to take advantage of such flexibility in the task specification.

This suggests that the overall bipedal walking system should be equiped with some “foresight,” allowing it to verify that end goals are achievable, even if there are intermittent periods of very limited control. Such foresight is provided by a motion planner, which outputs goals to be input to the momentum controllers. A typical motion planner produces a reference trajectory, which the controller then tries to follow. For example, reference trajectories for system linear and angular momentum produced by a motion planner would be input directly to a momentum controller such as the one described in Sect. 4.2.

A motion planner need not, necessarily, produce a detailed reference trajectory. It could, instead, produce a coarser set of waypoints that define regions in state space to be achieved at the end of a single or double support phase. Such an approach works well with the type of controller described in Sect. 5.2. This type of motion planning utilizes reach set analysis to generate the waypoints (goal sets) for the controllers. This corresponds to generation of flow tubes, families of trajectories that lead to the goal sets [44, 45]. This has the potential advantage of improved robustness because there are, effectively, multiple reference trajectories for the controller to follow, allowing the controller more options in reacting to disturbances.

A detailed discussion of motion planning for bipeds is beyond the scope of this chapter. However, it is important to note that any motion planner formulations that properly handle both linear and angular momenta should be based on the controller formulations described here, but expanded over some time horizon. The primary purpose for the time horizon is to guarantee that the next control action does not lead the system into an undesired state at the end of the horizon. This allows the biped to accept uncontrollable balance single support phases if the subsequent double support phase is balance controllable. It allows the biped to know that a temporary sacrifice of postural goals (a temporary utilization of the angular momentum reservoir) is possible in response to a disturbance.

Note that the time horizon need not be long. Guaranteeing that the next double support phase is balance controllable requires only planning one step in the future. In general, detailed planning of many steps into the future is not necessary .

6.2 Open Questions

The momentum-based balance controller framework attempts to enhance a robot’s balance maintainability by controlling the linear and angular momenta, of which importance is manifested from the fundamental fact that the GRF and CoP have one-to-one mapping relation with the rate of change of linear and angular momentum.

The physical meaning of the linear momentum, as the product of the total mass and the velocity of the CoM, is very intuitive, and its importance is obvious. On the other hand, the notion of the angular momentum brings about some nontrivial issues. First, it is difficult to grasp the physical intuition of the angular momentum of a humanoid robot. Unlike linear momentum, from which the position of CoM is obtained by the time integration, the notion of rotational position is unknown for the angular momentum: For a rotating rigid body about a fixed axis, the rotated angle can be measured by integrating its angular momentum. However, we cannot easily obtain the rotated angle from even the rotating single rigid body because its rotational inertia varies depending on the rigid body’s orientation. As can be seen from [46], which visualizes the rotational inertia of a humanoid robot, the rotational inertia of a humanoid robot varies greatly according to the robot’s pose, and thus it is even more difficult to apprehend the notion of the rotational position.

Therefore, it is not trivial to set the desired value for angular momentum to the controller, whereas the desired linear momentum can be straightforwardly obtained by differentiating the CoM trajectory. One strategy is to estimate it from the kinematic motion generated by the motion planner, as proposed by [39]. Another viable strategy, largely left as an interesting venue for future work, is to assess the desired angular momentum from the dynamic motion of a reduced model, possibly generated by a motion planner, which maintains the characteristics of the angular momentum of the full physical robot.