Balance and Posture Control for Biped Robots

Abstract

This work presents an overview of a new approach for balance and posture control by regulating simultaneously the center of mass position and trunk orientation of a biped robot. After an unknown external perturbation deviates the robot from a desired posture, the controller computes a wrench (force and torque) required to recover the desired position and orientation, according to a compliance control law. This wrench is distributed to predefined supporting contact points at the feet. The forces at these points are computed via a constrained optimization problem, adopted from the grasping literature, which minimizes the contact forces while including friction restrictions and torque limits at each joint.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

8.1 Introduction

The goal of obtaining biped robots able to interact with humans in everyday tasks and environments, calls for a proper control system that allows the robot to balance (compliantly) in the presence of unknown external perturbations. Such balance, i.e. the control of the linear and angular momentum of the system, is achieved through the application of suitable contact forces to the ground, using the finite support area of the feet [12]. Traditional approaches use a dynamics based walking pattern generator that provides desired trajectories for the underlying position controllers. The execution of such trajectories requires the addition of force sensors in the feet for implementing a Zero Moment Point (ZMP) control loop. In this way, a large range of stepping and walking motions can be generated. Several position-based balance compensators have been developed, although in general they require the measurement of force at every expected point of interaction with the external environment, which increases the computational load and creates time delays in the controller [1, 7, 20, 22].

Joint torque sensing and control allows sensitive compliance and impedance control [14], but requires additional instrumentation in the drive units. Torque sensing has been applied explicitly in the hydraulic humanoid robot CB [3] and implicitly via serial elastic actuators in [17]. At DLR, joint torque sensors are integrated in an electrically driven biped robot based on the torque controlled drive units of the DLR-KUKA Light-Weight Robot (Fig. 8.1), which can be position or torque controlled [15].

Fig. 8.1

DLR-Biped: a biped walking machine with torque controlled joints

Passivity-based impedance and compliance controllers based on joint torque sensing have been traditionally applied to manipulation tasks [1, 14]. The application of such framework to biped balancing control was first proposed in [9]. This controller provides gravity compensation, making the robot compliant and thus facilitating physical interactions and adaptation to unknown external forces. It sets a ground reaction force able to compensate perturbations on the robot position, and transforms the desired force to joint torques directly. It is able to cope with an arbitrary number of interaction points with the environment, but does not require force measurement at such points and does not use inverse kinematics or dynamics. The controller was tested both in simulation and on a real humanoid [3], and has been extended to compensate for yaw perturbations and to provide adaptability to unknown rough terrain [8].

More recently, a dynamic balance force controller was proposed for determining full body joint torques based on the desired motion of the Center of Mass (COM), combined with some desired virtual task forces [21]. The approach controls the motion of the COM and the angular momentum of the robot by computing suitable contact forces via a quadratic optimization problem. The mapping of the contact forces to the joint torques is solved considering the nonlinear multi-body dynamics of the system. In addition to the force distribution, the control of internal forces during multicontact interaction tasks was studied in [19], based on the concept of a virtual-linkage model, which provides a representation of the internal and COM resultant forces with respect to reaction forces on the supporting surfaces. A balancing controller based on the independent control of the desired ground reaction force and center of pressure at each support foot was also proposed [11], which allows dealing with different ground geometry. The approach minimizes ankle torques while generating desired rates of change of momenta. The performance of the approach is shown with simulated experiments.

This work presents an overview of a new approach for a posture controller able to deal with unknown external perturbations by distributing the required balancing forces among predefined contact points [16]. The approach is strongly based on the observation that the problems of grasping an object and balancing a robot are fundamentally similar, in the sense that both try to achieve a desired wrench F _o (on the object in the grasping case, on the robot in the balancing case) based on the application of suitable forces at the contact points f _i (at the fingertips or at the feet). Figure 8.2 illustrates such similarity. By using the basic theory of grasping, the force required to counteract external perturbations is distributed to the contact points; the final solution is obtained via a constrained optimization problem. The approach is validated in simulation, and tested on the DLR biped.

Fig. 8.2

Force distribution is fundamentally similar in (a) Grasping and (b) Balancing

8.2 Grasping Basics

An object is grasped for resisting external perturbations or to manipulate it in a dexterous way. The fulfillment of such tasks depends on the selection of suitable forces f _i applied at contact points P _i such that they produce a desired net wrench F _o on the object [13]. Each contact location is described by its relative position r _p and orientation R _p with respect to the object reference frame $\mathcal{O}$, commonly located at the COM of the object (Fig. 8.3).

Fig. 8.3

Object and contact coordinate frames

In general, a fingertip can only apply forces in certain directions, described by a contact model. The friction properties at the fingertip are commonly described using Coulomb’s friction model, which states that slippage is avoided when ${f^t} \le \mu {f^n}$, where ${f^n}$ and ${f^t}$ are the magnitudes of the normal and tangential components, respectively, and μ is the friction coefficient. Therefore, the set of allowable contact forces at the contact point is

$$ {\mathcal{F}_i} = \left\{ {\boldsymbol {f}_{i} \in {\mathbb {R} }^3}\left| {\sqrt {{f_{{{i_x}}}^2 + f_{{{i_y}}}^2}} \le \mu {f_{{{i_z}}}},{f_{{{i_z}}}} \ge 0} \right. \right\} $$

(8.1)

Geometrically, ${\mathcal{F}_i}$ represents a friction cone with axis along the surface normal and a semiangle of ϕ = atan (μ). Besides the friction constraints, the forces must also fulfill the positivity restriction, i.e. the fingers can push but cannot pull the object.

The generalized force F _i that can be applied at a contact point is described by F _i  = B _i f _i , with B _i being the wrench basis that characterizes the contact model. For instance, for a frictional point contact, the applied wrench is

$$ {\boldsymbol{F}}_{i} = \left[ {\begin{array}{cllclclc} 1 & 0 & 0 \\0 & 1 & 0 \\0 & 0 & 1 \\0 & 0 & 0 \\0 & 0 & 0 \\0 & 0 & 0 \\\end{array} } \right]\left[ {\begin{array}{clclcllc} {{f_{{{i_x}}}}} \\{{f_{{{i_y}}}}} \\{{f_{{{i_z}}}}} \\\end{array} } \right] = {\boldsymbol{B}}{_i}{\boldsymbol f}_{i} $$

(8.2)

The wrench exerted by a single contact on the object, expressed in the object coordinate frame, is given by

$$ {{\boldsymbol F}_{{{O_i}}}} = {\boldsymbol Ad}_{{op}}^T{\boldsymbol B}_{i}{\boldsymbol f}_{i} = {\boldsymbol{G}}{_i}{\boldsymbol {f}_{i}} $$

(8.3)

where ${\boldsymbol G}_i = {\boldsymbol Ad}_{{op}}^T{\boldsymbol B}_i$ is called the contact map. $\mathop{{\boldsymbol Ad}}\nolimits_{{op}}^T$ is the transpose of the adjoint matrix for the homogeneous transformation from the frame $\mathcal{P}$ to $\mathcal{O}$, given by

$$ \mathop {\boldsymbol{Ad}}\nolimits_{{op}}^T = \left( {\begin{array}{cclclcll} {{{\boldsymbol R}_p}} & 0 \\{\mathop{{\hat{\boldsymbol r}}}\nolimits_p {{\boldsymbol R}_p}} & {{\boldsymbol R}_p} \\ \end{array} } \right) $$

(8.4)

with $\mathop{{\hat{\boldsymbol r}}}\nolimits_p$ the cross product matrix for the vector ${{\boldsymbol r}_p} = \mathop{{\left( {{x_p}\;{y_p}\;{ z_p}} \right)}}\nolimits^T$, given by

$$ \mathop{{\hat{\boldsymbol r}}}\nolimits_p = \left( {\begin{array}{clclclclclclcl}0 & { - { z_p}} & {{y_p}} \\{{z_p}} & 0 & { - {x_p}} \\{ - {y_p}} & {{x_p}} & 0 \\\end{array} } \right) $$

(8.5)

The total wrench F _O on the object is the sum of the contributions from each one of the η contacts, expressed in the same coordinate frame $\mathcal{O}$,

$$ {\boldsymbol {F}_{O}} = \left[ {{\boldsymbol {G}_{1}} \ldots {\boldsymbol {G}_{\eta }}} \right]\left[ {\begin{array}{clclclclclclcl}{{{\boldsymbol {f}_{1}}}} \\ {\vdots} \\{{{\boldsymbol {f}_{\eta }}}} \\ \end{array}}\right] = {\boldsymbol {G}{\boldsymbol {f}_{C}}} $$

(8.6)

where G is the grasp map, given by

$$ {\boldsymbol G} = \left[ {{\boldsymbol Ad}_{{op{_{-1}}}}^T{\boldsymbol B}_1} \cdots \mathop{{\boldsymbol Ad}}\nolimits_{{o{p_{\eta }}}}^T {\boldsymbol B}_{\eta } \right] $$

(8.7)

Assuming η frictional point contacts, the grasp map is further simplified to

$$ {\boldsymbol {G}} = \left( {\begin{array}{clclclclclclcl}{{{\boldsymbol R}_{{{p_1}}}}} & \cdots & {{{\boldsymbol R}_{{{p_{\eta }}}}}} \\{\mathop{{\hat{\boldsymbol r}}}\nolimits_{{{p_1}}} {{\boldsymbol R}_{{{p_1}}}}} & \cdots & {\mathop{{\hat{\boldsymbol r}}}\nolimits_{{{p_{\eta }}}} {\boldsymbol R}_{{{p_{\eta }}}}}\end{array}} \right) $$

(8.8)

8.3 Dynamic Model

The unconstrained dynamics of a biped robot can be described using the velocity vector ${\boldsymbol \upsilon = \mathop{{({{\boldsymbol \nu}^T},{\bf\omega^T},\mathop{{\dot{\boldsymbol q}}}\nolimits^T )}}\nolimits^T }$, where $\boldsymbol\nu \in {\mathbb {R}^3}$ and $\boldsymbol\omega \in {\mathbb {R}^3}$ are the linear and angular velocities of a frame $\cal B$ attached to a base link (e.g. trunk) with respect to the world coordinate frame $\cal W$, and $\dot{\boldsymbol q} \in {\mathbb {R}^n}$ are the angular velocities for the n joints of the robot. Let ${{\boldsymbol r}_b} \in {\mathbb {R}^3}$ and ${{\boldsymbol R}_b} \in SO(3)$ be the position and orientation of the base frame $\cal B$; then, the dynamical model is

$$ {\boldsymbol M}({\boldsymbol q})\dot{\boldsymbol\upsilon} + {\boldsymbol C}({\boldsymbol q,\boldsymbol\upsilon}){\boldsymbol\upsilon} + {\boldsymbol p}({\boldsymbol q,{R_{\it b}}}) = \left( {\begin{array}{clclclclclclcl}{\mathbf{0}} \\{\mathbf{0}} \\\boldsymbol\tau \\\end{array} } \right) + \sum\limits_{{k = \{ r,l\} }} {{\boldsymbol J}_k}{({\boldsymbol q})^T}{{\boldsymbol F}_k} $$

(8.9)

where M(q), C(q, υ)υ, and p(q, R _b ) are the robot’s inertia matrix, the vector of centrifugal and Coriolis terms, and the vector of gravity terms, respectively; $\boldsymbol\tau \in {\mathbb {R}^n}$ is the vector of actuator torques, and ${{\boldsymbol F}_k} \in {\mathbb {R}^6}$ are the body wrenches acting at the robot’s right (k = r) and left foot (k = l). Moreover, ${{\boldsymbol J}_k}({\boldsymbol q}) = [{\boldsymbol A}{{\boldsymbol d}_{{kb}}}({\boldsymbol q}){\boldsymbol J}_k^b({\boldsymbol q})]$, with Ad _kb (q) being the adjoint matrix for the homogeneous transformation between the feet and the base link, and ${\boldsymbol J}_k^b({\boldsymbol q})$ is the body Jacobian for the feet [13].

This dynamical model is further simplified if the COM velocity $\mathop{{\dot{\boldsymbol r}}}\nolimits_C$ is used instead of the velocity of the base link [23]. Then, υ is replaced by ${{\boldsymbol\upsilon}_C} = {(\mathop{{\dot{\boldsymbol r}}}\nolimits_C^T, {\boldsymbol\omega^T},\mathop{{\dot{\boldsymbol q}}}\nolimits^T )^T}$ via

$$ \boldsymbol\upsilon = \left( {\begin{array}{clclclclclclcl}\boldsymbol\nu \\\boldsymbol\omega \\{\dot{\boldsymbol q}} \\\end{array} } \right) = \underbrace{{\left[ {\begin{array}{clclclclclclcl}{{\boldsymbol R}_b^T} & {^B\mathop{{\hat{{\boldsymbol r}}}}\nolimits_C ({\boldsymbol q})} & { - {{\boldsymbol J}_{{BC}}}({\boldsymbol q})} \\{\mathbf{0}} & {\boldsymbol I} & {\mathbf{0}} \\{\mathbf{0}} & \mathbf{0} & {\boldsymbol I} \\\end{array} } \right]}}_{\boldsymbol A}\underbrace{{\left( {\begin{array}{clclclclclclcl}{\mathop{{\dot{\boldsymbol r}}}\nolimits_C } \\\boldsymbol\omega \\{\dot{\boldsymbol q}} \\\end{array} } \right)}}_{{{\boldsymbol\upsilon_C}}} $$

(8.10)

where ${{\boldsymbol J}_{{BC}}}({\boldsymbol q}) = {\partial^B}{{\boldsymbol r}_C}(\boldsymbol{q})/\partial q$, and $^B{{\boldsymbol r}_C}({\boldsymbol q})$ is the COM position represented in the coordinate frame. With these new coordinates, the dynamic model is transformed to

$$ \begin{array}{clclcl}\left[{\begin{array}{clclclclclclcl}{m{\boldsymbol I}} & {\mathbf{0}}\\{\mathbf{0}} & {\bar{\boldsymbol M}(\boldsymbol q)} \\\end{array}} \right]\mathop{{\dot{\boldsymbol\upsilon}}}\nolimits_C + \left({\begin{array}{clclclclclclcl}{\mathbf{0}}\\{\bar{\boldsymbol{C}}({\boldsymbol{q}},{{\boldsymbol\upsilon}_C}) {{\boldsymbol\upsilon}_C}}\\ \end{array}} \right) + \left({\begin{array}{clclclclclclcl}{m\boldsymbol g} \\{\mathbf{0}}\\\end{array} } \right) = \left({\begin{array}{clclclclclclcl}{\underline {\mathbf{0}} } \\{\mathbf{0}} \\\boldsymbol\tau \\\end{array} } \right) + \sum\limits_{{k = \{ r,l\} }} \mathop{{\bar{{\boldsymbol J}}}}\nolimits_k {({\boldsymbol q})^T}{{\boldsymbol F}_k}\end{array}$$

(8.11)

where m is the total mass, and ${{ {\bar{\boldsymbol{M}}}}}(\boldsymbol q)$ and ${{ {\bar{\boldsymbol{C}}}}}(\boldsymbol q,{\boldsymbol\upsilon_C}){\boldsymbol\upsilon_C}$ are terms resulting from the coordinate transformation (8.10). The resulting Jacobian matrices are given by ${{\mathop{{\bar{{\boldsymbol J}}}}\nolimits_k {({\boldsymbol q}) = \mathop{{{\boldsymbol J}}}\nolimits_k (\boldsymbol q)A}}}$, with k = {r, l}, and can be partitioned as

$$ {\mathop{{\bar{{\boldsymbol J}}}}\nolimits_k {({\boldsymbol q}) = \left[ {\left( {\begin{array}{clclclclclclcl}{{\boldsymbol R}_k^T} \\{\mathbf{0}} \\\end{array} } \right)\ \ \mathop{{\boldsymbol {Ad}}}\nolimits_{{kb}} \left( {\begin{array}{clclclclclclcl}{^B\mathop{{\hat{{\boldsymbol r}}}}\nolimits_C } \\{\boldsymbol I} \\\end{array} } \right)\ \ \underbrace{{{\boldsymbol J}_k^b({\boldsymbol q}) - {{\boldsymbol R}_{{kb}}}{{\boldsymbol J}_{{BC}}}({\boldsymbol q})}}_{{{\boldsymbol J_{{\it ck}}}(\boldsymbol {q})}}} \right]}} $$

(8.12)

The COM dynamics emerges from the first three equations in (8.11), i.e.

$$ m\mathop{{\ddot{\boldsymbol r}}}\nolimits_C + \;m{\boldsymbol g} = \sum\limits_{{k = r,l}} {{\boldsymbol R}_k}{{\boldsymbol f}_k} $$

(8.13)

and it is influenced by the force components f _k from the contact wrenches F _k  = ( f _k , t _k ). The last n equations in (8.11) provide a kinetostatic mapping of the contact wrenches F _r , F _l at the right and left foot to the joint torques τ via

$$ \boldsymbol\tau = {\boldsymbol J}_{{cr}}{(\boldsymbol q)}^{T}{{\boldsymbol{F}_r}} + {{\boldsymbol {J}_{{cl}}}}(\boldsymbol {q})^{T}{{\boldsymbol F}_l} $$

(8.14)

The representation provided in (8.11), which gives the interaction of the isolated COM dynamics with the remaining multibody dynamics, was previously used for the design of a force based COM balancing controller [9]. In the following section, we extend the controller from [9] by adding a posture controller for the base orientation, and using the force distribution framework described in Sect. 8.2.

8.4 Balancing Controller

This section presents a balancing controller which regulates the position of the robot’s total COM in $\cal W$, ${{\boldsymbol r}_C} \in {\mathbb {R}^3}$, and the orientation R _b of the base link. Figure 8.4 shows the structure of the proposed controller. Basically, given a desired equilibrium position ${\boldsymbol r}_C^d$ for the COM position and a desired orientation R _d of the trunk, we compute a desired wrench F _GA to be applied to the robot. That wrench is distributed to forces ${{\boldsymbol f}_C} \in {\mathbb {R}^{{3\eta }}}$ at the η supporting contact points at the feet. In order to realize the desired contact forces, we compute the resulting contact wrenches F _r and F _l at the right and left foot, and map these contact wrenches to corresponding joint torques τ _d via (8.14), thus avoiding the use of inverse kinematics or dynamics. The resulting joint torques are commanded to an underlying joint torque controller [1]. Further details are provided below.

Fig. 8.4

Overview of the balancing controller

8.4.1 Object Force Generation

A biped robot with multiple contacts with the ground can be analyzed as a series of contact forces applied at the contact points, which generate a net wrench on the robot according to (8.6). The force component of the net wrench is called the ground reaction force f _GR . For the balancing controller, it is more convenient to consider a desired wrench that must be applied at the COM of the robot, which must be generated through forces applied at the contact points. The net force that must be generated is called ground applied force f _GA , defined as f _GA  = −f _GR .

To keep the desired COM position, we obtain a desired ground applied force ${\boldsymbol f}_{{GA}}^d$, given by the task of recovering the initial position (according to a compliance control law) while providing gravity compensation, i.e.

$$ {\boldsymbol f}_{{GA}}^d = m({\boldsymbol g} + \mathop{{\ddot{{\boldsymbol r}}}}\nolimits_C^d ) - {{\boldsymbol K}_P}\left( {{{\boldsymbol r}_C} - {\boldsymbol r}_C^d} \right) - {{\boldsymbol D}_P}\left( {\mathop{{\dot{{\boldsymbol r}}}}\nolimits_C - \mathop{{\dot{{\boldsymbol r}}}}\nolimits_C^d } \right) $$

(8.15)

where m is the total mass of the robot, g is the gravity vector, K _P , D _P  > 0 are the proportional and differential gain matrices, and ${\boldsymbol r}_C^d$, $\mathop{{\dot{\boldsymbol r}}}\nolimits_C^d$, and $\mathop{{\ddot{\boldsymbol r}}}\nolimits_C^d$ are the desired position, velocity and acceleration of the COM.

To deal with the orientation perturbations, let δ and $\boldsymbol\epsilon$ be the scalar and vector part of the quaternion representation of ${{\boldsymbol R}_{{db}}} = {\boldsymbol R}_d^T{{\boldsymbol R}_b}$, with ${{\boldsymbol R}_b}$ and ${{\boldsymbol R}_d}$ the current and desired trunk orientation. The required torque for a torsional spring acting to align ${{\boldsymbol R}_b}$ to ${{\boldsymbol R}_d}$ can be derived from a potential function ${V_O} = 2{ \boldsymbol\epsilon^T}{{\boldsymbol K}_r} \boldsymbol\epsilon$, and is given by ${\boldsymbol\tau_r} = - 2\left( {\delta {\boldsymbol I}{\mathbf{+ }}\hat{ \boldsymbol\epsilon }} \right){{\boldsymbol K}_r} \boldsymbol\epsilon$, with ${{\boldsymbol K}_r} \in{\mathbb {R}^{{3 \times 3}}}$ a symmetric and positive definite stiffness matrix. This relation can be verified by using the principle of virtual work [2]. Thus, a PD-like orientation controller for the trunk is given by

$$ \boldsymbol\tau_{{GA}}^d = {{\boldsymbol R}_{{wb}}}({\boldsymbol\tau_r} - {{\boldsymbol D}_r}(\boldsymbol\omega - {\boldsymbol\omega^d})) $$

(8.16)

with ${{\boldsymbol D}_r} \in {\mathbb {R}^{{3 \times 3}}}$ being a symmetric and positive definite damping matrix.

Finally, the desired force and torque from (8.15) and (8.16) are combined to get the net desired wrench ${{\boldsymbol F}_{{GA}}} = {({\boldsymbol f}_{{GA}}^d,\boldsymbol\tau_{{GA}}^d)^T}$.

8.4.2 Contact Force Distribution

Consider a biped robot with η contact points with the ground (Fig. 8.5), and let ${{\boldsymbol r}_i} = {[{x_i},{y_i},{z_i}]^T}$ with $i = 1,...,\eta$ be the position of the contact points with respect to the COM (i.e. in the coordinate frame). The robot only interacts with the environment through forces f _i at the η frictional contact points. Each contact force is described as ${{\boldsymbol f}_i} = {[{f_{{{i_x}}}},{f_{{{i_y}}}},{f_{{{i_z}}}}]^T}$, and all the contact forces are stacked in the contact force vector ${\boldsymbol f}_C \in {\mathbb {R}^{{3\eta }}}$.

Fig. 8.5

Contact positions and forces for a biped robot: (a) Location of the COM; (b) Forces

The desired net wrench on the robot, F _GA , must be generated through suitable contact forces f _C ; the relation between them is given by (8.6) (${{\boldsymbol F}_{{GA}}} = {{\boldsymbol G}_C}{{\boldsymbol f}_C}$), with G _C the contact map (i.e. the grasp map applied to walking robots). For instance, for the case of a robot standing on flat ground, the coordinate frame at each contact point can be chosen parallel to the world frame $\cal W$, and ${{\boldsymbol G}_C} \in {\mathbb {R}^{{6 \times 3\eta }}}$ gets the simplified form

$$ {{\boldsymbol G}_C} = \left( {\begin{array}{clclclclclclcl}{{{\boldsymbol I}_{{3 \times 3}}}} & \cdots & {{{{\boldsymbol I}_{{3 \times 3}}}}} \\{\mathop{{\hat{{\boldsymbol r}}}}\nolimits_{{{p_1}}} } & \cdots & {\mathop{{\hat{{\boldsymbol r}}}}\nolimits_{{{p_{\eta }}}} } \\\end{array} } \right) $$

(8.17)

For a given F _GA , the corresponding contact forces can be computed as

$$ {{\boldsymbol {f}_{C}} = {\boldsymbol {G}_{C}}^{\# }{{\boldsymbol {F}_{{GA}}}}} $$

(8.18)

with ${\boldsymbol G}_C^{\# } = {\boldsymbol G}_C^T{({{\boldsymbol G}_C}{\boldsymbol G}_C^T)^{{ - 1}}}$ the pseudoinverse of the contact map. This solution minimizes the Euclidean norm of the contact forces under the constraint (8.6). The solution to this problem has also been considered in the grasping community as the minimization of the grasping forces that ensure a stable grasp [6, 18]. These optimization procedures include the friction cone restrictions, and guarantee that the equilibrium (8.6) is always fulfilled. However, in the case of the balancing problem the wrench F _GA on the object is a control command resulting from (8.15) and (8.16), which might not be exactly met. In order to ensure that all constraints are fulfilled, a different approach is required for this optimization problem.

8.4.3 Force Distribution Using Unilateral Constraints

To get a desired net wrench F _GA , the distribution of contact forces according to (8.18) in general does not guarantee that the positivity restriction and the friction constraints (8.1) at the contact points are fulfilled. As an alternative, the contact forces can be computed with a constrained multi-objective optimization problem. The main objective is to achieve a desired force on the COM, which can be formulated as the minimization of the cost function ${J_1}({{\boldsymbol f}_C}) = ||[{\boldsymbol I}\;{\mathbf{0}}]({{\boldsymbol F}_{{GA}}} - {{\boldsymbol G}_C}{{\boldsymbol f}_C})||_2^2$. Getting the object torque is a secondary objective, with an objective function ${J_2}({{\boldsymbol f}_C}) = ||[{\mathbf{0}}\;\boldsymbol I]({{\boldsymbol F}_{{GA}}} - {{\boldsymbol G}_C}{{\boldsymbol f}_C})||_2^2$. Additionally, a third objective is the minimization of the Euclidean norm of the contact forces, i.e. ${J_3}({{\boldsymbol f}_C}) = {\boldsymbol f}_C^T{{\boldsymbol f}_C}$. The three objectives are combined in a single objective function J( f _C )

$$ J({{\boldsymbol f}_C}) = {\alpha_1}{J_1}({{\boldsymbol f}_C}) + {\alpha_2}{J_2}({{\boldsymbol f}_C}) + {\alpha_3}{J_3}({{\boldsymbol f}_C}) $$

(8.19)

where ${\alpha_1},{\alpha_2},{\alpha_3} > 0$ are the corresponding weights for the three objectives. By choosing ${\alpha_3} < < {\alpha_2} < < {\alpha_1}$, the first objective is selected as the main priority task and the third objective acts mainly as a regularization of the Hessian for the objective function. The minimization of J(f _C ) subject to restrictions arising from a polyhedral approximation to the friction cone (8.1) and from the positivity constraints represents a quadratic optimization problem, given by

$$ min{_{{{{\boldsymbol f}_C}}}}{\boldsymbol f}_C^TQ{{\boldsymbol f}_C} + {{\boldsymbol p}^T}{{\boldsymbol f}_C} $$

(8.20)

where

$${ \boldsymbol Q} = {\alpha_3}{\boldsymbol I} + {\boldsymbol G}_C^T\left( {\begin{array}{clclclclclclcl}{{\alpha_1}{\boldsymbol I}} & 0 \\0 & {{\alpha_2}{\boldsymbol I}} \\\end{array} } \right){{\boldsymbol G}_C} $$

(8.21)

$$ {\boldsymbol p} = - {\boldsymbol G}_C^T\left( {\begin{array}{clclclclclclcl}{{\alpha_1}{\boldsymbol I}} & 0 \\0 & {{\alpha_2}{\boldsymbol I}} \\\end{array} } \right){{\boldsymbol F}_{{GA}}} $$

(8.22)

Note that the positivity and friction constraints force the ZMP to lie within the support polygon. Based on (8.14), additional restrictions such as torque limits at each joint can also be included in the problem statement.

The proposed approach assumes that all the predefined contacts are active at every moment, and therefore distributes the desired wrench to all the contact points. To guarantee that each contact is always active on the real robot, a lower limit for the contact force can be preset to some positive value.

8.5 Experiments

The balancing controller was tested in simulations using OpenHRP3 [10], and in experiments with the DLR Biped [15] (Fig. 8.1). The DLR Biped has six degrees of freedom per leg, a 6-DOF force-torque sensor (FTS) in each foot, position and torque sensors at each joint, and an inertial measurement unit (IMU). The FTS information is not used, since the proposed control scheme does not require any measurement of the contact forces at the feet.

8.5.1 Implementation Details

The origin of the world coordinate frame $\mathcal{W}$ was chosen to be in the middle point between the two feet. The trunk orientation and angular velocity are measured via the IMU. As the IMU shows considerable drift in the yaw rotation, we approximate the yaw angle between the world frame and the trunk by comparing the trunk orientation to the baseline between the right and left foot.

The proportional gain matrix K _p for the controller was chosen as a diagonal matrix with stiffness values of k _h  = 900 N/m in the horizontal (x and y) directions, and k _∨  = 3,000 N/m in the vertical (z) direction. The damping gain matrix D _p was chosen as a diagonal matrix with the elements set to ${d_h} = \sqrt {(} m{k_h})2 \cdot 0.8$ for the horizontal components, and ${d_\vee} = \sqrt {(} m{k_\vee})2 \cdot 0.2$ for the vertical component. The rotational stiffness and damping matrices were set to K _r  = 100I[Nm/rad] and D _r  = 50I[Nms/rad]. The same controller gains were used in the simulations and in the experiments.

The constrained optimization problem from Sect. 8.4.3 was solved using the open source software qpOASES [5]. The values of the weights for the multi-objective optimization are chosen as ${\alpha_1} = 1$, ${\alpha_2} = {10^{{ - 3}}}$ and ${\alpha_3} = {10^{{ - 6}}}$. With $\eta = 8$frictional contact points, the computation time for the optimization of $3\eta = 24$ components in f _C took less than 200 μs on the onboard 2.8 GHz mobile CPU running under the real-time operating system VxWorks.

8.5.2 Simulation

As an example of the behavior of the controller, a simulation of an external perturbation was performed. The desired COM velocity is set to $\mathop{{\dot{\boldsymbol r}}}\nolimits_C^d = 0$, and a lateral force of 70 N was applied during 50 ms at the hip (Fig. 8.6). This force applies also a torque on the base link, since it is not exerted at the COM. Figure 8.7a shows the COM error resulting from this simulation. The components of the contact forces are displayed in Fig. 8.7b. At t ≈ 0.25s the vertical forces at the right foot (in blue) reach their lower limit of 4 N set in the optimization algorithm from Sect. 8.4.3.

Fig. 8.6

Balancing experiment in OpenHRP

Fig. 8.7

Results for the simulated lateral force disturbance: (a) COM error in x (blue), y (black), and z (red); (b) Contact force components at the right (blue) and left (red) feet

8.5.3 Experimental Evaluation

An experiment of physical interaction with a human is shown in Fig. 8.8, where the trunk of the robot is pushed to create different perturbations in position and orientation. Figure 8.9a shows the corresponding COM error for pushes in x and y. Figure 8.9b shows the corresponding contact forces generated by the balancing controller. In this example, the non-negativity constraints on the vertical force components act over a longer duration compared to the simulated experiment, as the human interaction acts as a low frequency disturbance.

Fig. 8.8

Compensatory motions for different perturbations applied to the robot: (a) in x; (b) in y; (c) in z; (d) in yaw. Filled rectangles are overimposed as an aid to perceive the displacement of the robot

Fig. 8.9

Results for the experiment of physical human interaction: (a) COM error in x (blue), y (black), and z (red); (b) Contact force components at the right (blue) and left (red) feet

Finally, note that in the derivation of the controller there was no assumption for the robot to be on flat ground. When the global trunk orientation can be obtained from an onboard IMU, the controller can deal with uneven terrain. Figure 8.10 presents a time sequence of the behavior of the robot mounted on top of a balancing board, and subjected to perturbations coming from a human.

Fig. 8.10

Behavior of the robot mounted on top of a balancing board

8.6 Summary

This chapter has presented an optimization based balancing algorithm for biped robots, which regulates the position of the COM and the orientation of the trunk. It allows to distribute a net wrench, required to recover a desired posture, onto a predefined set of contact points. The approach was verified in simulation and in experiments with a torque controlled robot. The balancing controller, including the approach for distributing the contact force, is general enough to be applied to a biped robot in different contact situations, such as single or double support phases. It can also be applied to multi-legged robots or to multi-point contact situations, for instance, to balance forces created when a humanoid robot manipulates an object or interacts with the environment.