ZMP support area

When a legged robot walks over a regular terrain, we can simplify its dynamics to a reduced model like the linear inverted pendulum where the center of mass (CoM) is controlled via the zero-tilting moment point (ZMP) of contact forces with the ground. To be physically feasible, the ZMP must lie in a support area, also known as support polygon, which is roughly "the area between the feet" in simple cases. In more complex cases, such as a hand on a wall or a knee on the ground, the area is a bit more complex but it is still computable.

Modeling

The constrained equations of motion describe the dynamics of our articulated system without simplification:

\begin{equation*} \begin{array}{rcl} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd & = & \bfS^\top \bftau + \bftau_g(\bfq) + \bftau_\mathit{ext} + \bfJ(\bfq)^\top \bff \\ \bfF(\bfq) \bff & \leq & \bfzero \end{array} \end{equation*}

The configuration \(\bfq\) consists of its \(n\) actuated degrees of freedom and 6 unactuated floating base coordinates. Contact forces from the environment are stacked in the vector \(\bff\) and constrained by the matrix \(\bfF(\bfq)\) of Coulomb friction cones. The interest in switching to a simplified model like the linear inverted pendulum (LIP) is to reduce the dimension of this equation. The constrained equations of motion for the LIP are line-by-line similar to those of the whole-body:

\begin{equation*} \begin{array}{rcl} \bfpdd_G & = & \omega^2 (\bfp_G - \bfp_Z) \\ \bfA(\bfp_G) \bfp_Z & \leq & \bfb(\bfp_G) \end{array} \end{equation*}

The configuration \(\bfp_G\) consists of the the 2D horizontal position of the center of mass. Contact forces from the environment are aggregated in the position \(\bfp_Z\) of the ZMP and constrained by the second line of half-space inequalities. The matrix \(\bfA(\bfp_G)\) and vector \(\bfb(\bfp_G)\) define the support area, which is a polygon as soon as we take into account actuation limits of the robot. They can be computed from the contact geometry and friction information encoded in \(\bfJ(\bfq)\) and \(\bfF(\bfq)\) by polyhedral projection. Further technical details on how to do this are given in Section IV of this paper.

Simplification on flat floors

In pre-2010 robotics papers, we often see the ZMP support area defined as the "convex hull of ground contact points", where \(\bfA\) and \(\bfb\) do not depend on \(\bfp_G\) any more and can be readily computed from ground contact points by a convex hull algorithm (a particular case of polyhedral projection). This simplification is only valid if we make two assumptions:

  • All ground contact points are coplanar.
  • The coefficient of friction between robot feet and the ground is infinite.

If we follow the general algorithm from the previous section, which takes the CoM position \(\bfp_G\) as an input, it is not clear how the ZMP support area becomes independent from \(\bfp_G\) under these two conditions. Let us detail this step by step.

Infinite friction

Let us consider for simplicity of calculations that our flat ground contact plane is horizontal (othogonal to gravity). In the absence of friction constaints, our friction cone inequalities are reduced to contact unilaterality constraints:

\begin{equation*} \forall \textrm{ contact } i, \ f_i^z > 0 \end{equation*}

This means we can exert arbitrary horizontal forces \(f_i^x\) and \(f_i^y\) at every contact point \(i\).

Newton-Euler equations

Our contact forces are bound to the CoM and ZMP by Newton and Euler equations. First, Newton equation gives us:

\begin{equation*} \sum_{\textrm{contact } i} \bff_i = \bff = \frac{m g}{h} (\bfp_G - \bfp_Z) \end{equation*}

with \(m\) the total mass, \(g\) the gravity constant and \(h\) the CoM height above ground. Second, Euler equation (with no angular momentum at the center of mass since we are in a pendular model) gives us:

\begin{equation*} \sum_{\textrm{ contact } i} \bfp_i \times \bff_i = \bfp_G \times \bff \end{equation*}

Let's unwrap it to understand better what it says. On flat floor, we have \(z_i = 0\) for all contact points \(i\), so that the vector cross products expand to:

\begin{equation*} \begin{array}{rcl} \sum_{i} y_i f_i^z & = & y_G f^z - h f^y \\ \sum_{i} -x_i f_i^z & = & -x_G f^z + h f^x \\ \sum_{i} (x_i f_i^y - y_i f_i^x) & = & x_G f^y - y_G f^x \end{array} \end{equation*}

Since we are not limited by friction, the third equation always has a solution and we can trim it out. In the other two equations, we can replace \(\bff\) by its expression \(mg (\bfp_G - p_Z) / h\) from the linear inverted pendulum model, for instance:

\begin{equation*} \begin{array}{rcl} y_G f^z & = & y_G m g (h - 0) / h = y_G m g \\ h f^y & = & m g (y_G - y_Z) \\ y_G f^z - h f^y & = & m g y_Z \\ mg y_Z & = & \sum_i {f_i^z} y_i \end{array} \end{equation*}

and similarly for the other equation. Defining \(\alpha_i = \frac{f_i^z}{mg}\), we get:

\begin{equation*} \begin{array}{rcl} \bfp_Z & = & \sum_{i} \alpha_i \bfp_i \\ \forall i, \alpha_i & \geq & 0 \end{array} \end{equation*}

which is the vertex representation of the convex hull of ground contact points \(\bfp_i\).

To go further

ZMP support areas can be derived in any multi-contact configurations, but in this case they depend (nonlinearly) on the position of the center of mass. Simplified ZMP support areas can be applied for walking over both horizontal and inclined. For instance, they have been used by biped robots to climb stairs and quadruped robots to trot over inclined surfaces. Joint torque limits can also be included to compute actuation-aware ZMP support polygons.

© Stéphane Caron — All content on this website is licensed under the CC BY 4.0 license.
π