Gait Generation using Intrinsically Stable MPC in the Presence of Persistent Disturbances

TL;DR

This paper enhances an existing model predictive control method for humanoid robots to maintain balance despite persistent disturbances by incorporating a disturbance observer into the control framework.

Contribution

It introduces a disturbance observer into the Intrinsically Stable MPC to handle persistent disturbances, ensuring stability and balance in humanoid gait generation.

Findings

Validated through MATLAB simulations with LIP model

Demonstrated in dynamic simulations of NAO humanoid in DART

Achieved improved balance robustness under disturbances

Abstract

Maintaining balance while walking is not a simple task for a humanoid robot because of its complex dynamics. The presence of a persistent disturbance makes this task even more challenging, as it can cause a loss of balance and ultimately lead the the robot to a fall. In this paper, we extend our previously proposed Intrinsically Stable MPC (IS-MPC), which guarantees boundedness of the CoM with respect to the ZMP, to the case of persistent disturbances. This is achieved by designing a disturbance observer whose estimate is used to compute a modified stability constraint included in the QP problem formulation. The method is validated by MATLAB simulations for the LIP as well as dynamic simulations for a NAO humanoid in DART.

Equations

$${\ddot{x}}_c={\eta }^2(x_c-x_z),$$

$$\begin{array}{rl}{x}_u& =x_c+{\dot{x}}_c/\eta \\ x_s& =x_c-{\dot{x}}_c/\eta .\end{array}$$

$${\dot{x}}_u=\eta (x_u-x_z).$$

$$x_u^k=\eta {\int }_{t_k}^{\infty }{e}^{-\eta (\tau -t_k)}{x}_z(\tau )d\tau .$$

$$\left(\begin{array}[]{c}\dot{x}_{c}\\ \ddot{x}_{c}\\ \dot{x}_{z}\end{array}\right)=\left(\begin{array}[]{ccc}0&1&0\\ \eta^{2}&0&-\eta^{2}\\ 0&0&0\end{array}\right)\left(\begin{array}[]{c}x_{c}\\ \dot{x}_{c}\\ x_{z}\end{array}\right)+\left(\begin{array}[]{c}0\\ 0\\ 1\end{array}\right)\dot{x}_{z},$$

$$R_{j}^{T}\left(\begin{array}[]{c}\delta\sum_{l=k}^{k+i-1}\dot{x}_{z}^{l}-x_{f}^{j}\\[10.0pt] \delta\sum_{l=k}^{k+i-1}\dot{y}_{z}^{l}-y_{f}^{j}\end{array}\right)\leq\frac{1}{2}\left(\begin{array}[]{c}f_{x}\\[5.0pt] f_{y}\end{array}\right)-R_{j}^{T}\left(\begin{array}[]{c}x_{z}^{k}\\[5.0pt] y_{z}^{k}\end{array}\right),$$

$$\sum _{i=0}^{C-1}{e}^{-i\eta \delta }{\dot{x}}_z^{k+i}=-\sum _{i=C}^{\infty }{e}^{-i\eta \delta }{\dot{\tilde{x}}}_z^{k+i}+\frac{\eta }{1-e^{-\eta \delta }}(x_u^k-x_z^k).$$

$${\dot{X}}_z^k$$

$${\dot{Y}}_z^k$$

$${\ddot{x}}_c={\eta }^2(x_c-x_z)+d.$$

$$d_x=\left(1-{\eta }^2\frac{z_c}{{\ddot{z}}_c+g}\right){\ddot{x}}_c-{\eta }^2\frac{{\dot{L}}_y}{m({\ddot{z}}_c+g)}+\frac{F_{\mathrm{ext}}}{m},$$

$$\left(\!\begin{array}[]{c}\dot{x}_{c}\\ \ddot{x}_{c}\\ \dot{x}_{z}\end{array}\!\right)\!=\!\left(\!\begin{array}[]{ccc}0&1&0\\ \eta^{2}&0&-\eta^{2}\\ 0&0&0\end{array}\!\right)\!\!\left(\!\begin{array}[]{c}x_{c}\\ \dot{x}_{c}\\ x_{z}\end{array}\!\right)+\left(\!\begin{array}[]{c}0\\ 0\\ 1\end{array}\!\right)\dot{x}_{z}+\left(\!\begin{array}[]{c}0\\ 1\\ 0\end{array}\!\right)d.$$

$${\dot{x}}_u=\eta (x_u-x_z)+d/\eta .$$

$$x_u^k=\eta {\int }_{t_k}^{\infty }{e}^{-\eta (\tau -t_k)}{x}_z(\tau )d\tau -\frac{1}{\eta }{\int }_{t_k}^{\infty }{e}^{-\eta (\tau -t_k)}d(\tau )d\tau .$$

$$\sum _{i=0}^{C-1}{e}^{-i\eta \delta }{\dot{x}}_z^{k+i}=-\sum _{i=C}^{\infty }{e}^{-i\eta \delta }{\dot{\tilde{x}}}_z^{k+i}+\frac{\eta }{1-e^{-\eta \delta }}(x_u^k-x_z^k+{\Delta }_d^k),$$

$${\Delta }_d^k=\frac{1}{\eta }{\int }_{t_k}^{\infty }{e}^{-\eta (\tau -t_k)}d(\tau )d\tau .$$

$$d(t)=d^i+{\dot{d}}^i(t-t_i),t\in [t_i,t_{i+1})$$

$${\Delta }_d^k=\frac{1-e^{-\eta \delta }}{{\eta }^3}\sum _{i=0}^{\infty }{e}^{-i\eta \delta }{\dot{d}}^{k+i}+\frac{d^k}{{\eta }^2}.$$

$$x_c-x_z=\frac{{\bar{F}}_{\mathrm{ext}}}{m{\eta }^2}.$$

$${\Delta }_d^k={\Delta }_d=\frac{{\bar{d}}_k}{{\eta }^2}=\frac{{\bar{F}}_{\mathrm{ext}}}{m{\eta }^2},$$

$$\ddot{d}=0,$$

$$\begin{split}A=&\left(\begin{array}[]{ccccc}0&1&0&0&0\\ \eta^{2}&0&-\eta^{2}&1&0\\ 0&0&0&0&0\\ 0&0&0&0&1\\ 0&0&0&0&0\\ \end{array}\right)\ \ \ B=\left(\begin{array}[]{c}0\\ 0\\ 1\\ 0\\ 0\\ \end{array}\right)\\ C=&\left(\begin{array}[]{ccccc}1&0&0&0&0\\ 0&0&1&0&0\\ \end{array}\right).\end{split}$$

$$\dot{\hat{x}}=A\hat{x}+Bu+G(C\hat{x}-y)$$

$${\Delta }_d^k=\frac{{\hat{d}}^k}{{\eta }^2}.$$

Full text

I Introduction

Interest in humanoid robotics has considerably increased in the last decade, leading to major improvements both on the constructive side, with more reliable platforms being available, and on the control side.

Maintaining balance while walking is still a challenging task. The problem is usually approached by controlling the position of the Zero Moment Point (ZMP) [1], i.e., the point on the ground for which the horizontal components of the contact moments become zero. In order to guarantee dynamic balance, the ZMP has to be kept inside the support polygon of the robot at all times during the gait. Most position controlled humanoids adapt the ZMP through the Center of Mass (CoM) of the robot.

The dynamics relating the ZMP to the CoM is nonlinear, however standard simplifying assumptions lead to the well-known Linear Inverted Pendulum (LIP) model which is the basis for many popular approaches to humanoid gait generation. The influential paper [2] uses the Cart Table (CT), inverse of the LIP, and derives a preview controller. Constraints were added in [3] using a Model Predictive Control (MPC) scheme. This formulation was also extended in order to perform automatic footstep placement in [4].

For humanoids, since the area of the footprint is small, a push or an external persistent force may easily throw the robot off balance. The first case, push recovery, has been addressed for example with the Capture Point [5] and MPC schemes have shown to be also very effective [3, 6] just to cite a few. However disturbances and uncertainties can have a detrimental effect on an MPC-controlled system, if not properly accounted for. One possibility is to adopt a robust approach and design a controller that is able to withstand bounded unknown disturbances [7, 8, 9]. In the context of humanoid locomotion, these ideas have been developed in [10] leading to the precise computation of safety margins to cope with a given set of uncertainties.

When dealing with strong persistent disturbances, this approach may result very conservative. Another possibility is to introduce a disturbance observer and design a controller which is able to counteract the disturbance. An first notable example is [11] where the external force is estimated evaluating its effect on the humanoid. Other examples include [12] which incorporates an observer in a preview formulation, or approaches based on the Divergent Component of Motion such as [13] and [14]. Similar ideas are employed to deal with unmodeled dynamics during walking, such as in [15] and [16].

In [6] we proposed an MPC scheme, adding a stability constraint, in which the CoM trajectory is guaranteed to be bounded with respect to the ZMP. The resulting scheme is defined as Intrinsically Stable MPC (IS-MPC). The control scheme was further developed in [17] where we also proved how, using available preview information (e.g. coming from a footstep planner), the stability constraint can make the MPC scheme recursively feasible, which means that feasibility (i.e., the existence of a solution under the imposed constraints) at one iteration implies feasibility at the next iteration.

In this paper we extend the stability constraint of the IS-MPC to counteract the effect of a persistent disturbance (e.g., a humanoid carrying an unknown moving payload as illustrated in Fig. 1). This is done with the aid of an observer and tested in a range of different situations using dynamic simulations.

The paper is organized as follows. In the next section we briefly recall IS-MPC in the absence of disturbances (nominal case). Section III introduces the perturbed LIP model. The ideal case of known disturbance together with the corresponding modified stability constraint are discussed in Sect. IV. Removing the known disturbance hypothesis, a disturbance observer is introduced in Sect. V together with its use in the resulting observer-based IS-MPC. Simulations on the LIP and dynamic simulations on a NAO humanoid robot are also performed to validate the proposed approach. Section VI offers a few concluding remarks.

II IS-MPC: The Nominal Case

In this section we provide a brief review of the IS-MPC gait generation method. See [6, 17] for further details.

Assume that the humanoid is walking on flat horizontal ground, and denote the position of the humanoid Center of Mass (CoM) and Zero Moment Point (ZMP) as $(x_{c},y_{c},z_{c})$ and $(x_{z},y_{z},0)$, respectively. The dynamic equation relating the CoM and the ZMP can be derived by balancing moments around the ZMP, e.g., see [18]. Assuming the CoM height $z_{c}$ to be constant and neglecting angular momentum contributions around the CoM leads to the Linear Inverted Pendulum (LIP) model, where the $x$-axis (sagittal) and $y$-axis (coronal) dynamics are linear, identical and decoupled. For illustration, consider only the sagittal motion

[TABLE]

with $\eta=\sqrt{g/\bar{z}_{c}}$, where $g$ is the gravity acceleration and $\bar{z}_{c}$ is the constant CoM height. Note that the ZMP position $x_{z}$ acts as an input in this model.

The LIP is decomposed into a stable and an unstable subsystem by using the following change of coordinates:

[TABLE]

The dynamics of $x_{u}$, also known as divergent component of motion [19] or capture point [5], is

[TABLE]

Although this dynamics is unstable, $x_{u}$ (and hence $x_{c}$) will not diverge with respect to $x_{z}$ provided that

[TABLE]

Equation (4), called the stability condition in the following, is a relationship between the value of $x_{u}$ at the current time $t_{k}$, denoted by $x_{u}^{k}$, and the future values of the input $x_{z}$, and is therefore non-causal [20].

Intrinsically Stable MPC (IS-MPC) is a scheme for humanoid gait generation that uses a causal stability constraint derived from condition (4) in order to guarantee that the gait is internally stable, i.e., that the CoM remains bounded with respect to the ZMP. The prediction model is a dynamically extended LIP

[TABLE]

with the ZMP velocity $\dot{x}_{z}$ now acting as input. IS-MPC uses piecewise-constant inputs, i.e., $\dot{x}_{z}(t)=\dot{x}_{z}^{i}$ for $t\in[t_{i},t_{i+1}]$, with $t_{i+1}-t_{i}=\delta$ the duration of sampling intervals. The MPC control horizon is $C\cdot\delta$.

Although IS-MPC can perform automatic footstep placement (AFS), in this paper we will for simplicity consider the case of given footsteps (a simulation with AFS is included however in Section V-C). In this case, only the ZMP and the stability constraints must be enforced.

The ZMP constraint guaranteeing dynamic balance is expressed as

[TABLE]

where $R_{j}^{T}$ is the rotation matrix associated to the orientation of the $j$-th footstep, $(x_{f}^{j},y_{f}^{j})$ is its position, and $f_{z}$, $f_{y}$ are the dimensions of a rectangular region approximating the footprint. The above is the expression of the constraint during single support; the double support constraint can be expressed in a similar way.

The stability constraint is derived from (4) using the fact that the ZMP is piecewise-linear:

[TABLE]

Here, the left-hand side gathers the ZMP velocities $\dot{x}_{z}^{k},\dots,\dot{x}_{z}^{k+C-1}$ within the control horizon, which are are the MPC decision variables. The right-hand side depends on the system state at $t_{k}$ as well as on the tail, i.e., the conjectured values $\dot{\tilde{x}}_{z}^{k+C},\dot{\tilde{x}}_{z}^{k+C+1},\dots$ of the ZMP velocities after the control horizon. This conjecture, which is needed to obtain a causal constraint, can be made using the available preview information on the footstep plan (anticipative tail). More details on ZMP velocity tails are given in [17].

Collecting the MPC decision variables in

[TABLE]

the generic MPC iteration at $t_{k}$ consists in solving the following Quadratic Programming (QP) problem:

\left\{\begin{minipage}{433.62pt} $$\min_{\dot{X}_{z}^{k},\dot{Y}_{z}^{k}}\|\dot{X}_{z}^{k}\|^{2}+\|\dot{Y}_{z}^{k}\|^{2}$$ \centerline{\hbox{subject to:}} \par\IEEEitemize\par\itemize@item@ZMP constraints (\ref{eq:ZMPcon}); \par\itemize@item@stability constraints (\ref{eq:StabConstr}) for x$and$y. \par\endIEEEitemize \end{minipage}\right.

Once the problem is solved, the first sample $\dot{x}_{z}^{k}$ of the optimal input sequence is used to integrate the LIP dynamics along $x$ (analogously for $y$). This results in a CoM reference trajectory that can be tracked by the humanoid robot using a standard kinematic controller.

III The Perturbed Model

Consider now a disturbance111In general, the disturbance will be a vector $(d_{x},d_{y})$ and will include a component acting along $y$. However, in the following we focus on the $x$ dynamics and therefore we shall simply write $d$ in place of $d_{x}$. $d$ acting on the LIP dynamics as follows:

[TABLE]

This disturbance can represent external forces acting as well as unmodeled dynamics and uncertainties. For example, starting from the general CoM dynamics [10] one can write $d$ as

[TABLE]

where $m$ is the total mass of the robot, $L_{y}$ its centroidal angular momentum around the $y$-axis and $F_{\rm ext}$ is (the $x$ component of) the resultant of external forces acting on the CoM. The first two terms of this expression are unmodeled dynamics, whereas the third is an actual disturbance.

The prediction model (5) is therefore modified to include the disturbance as

[TABLE]

Applying the same change of variables (2), the unstable component $x_{u}$ is now found to be affected by $d$:

[TABLE]

To guarantee boundedness of the CoM with respect to the ZMP in the perturbed case, the stability condition (4) must be modified accordingly:

[TABLE]

A causal implementation of this condition would require, in addition to the conjecture on the ZMP velocities after the control horizon, also knowledge of $d$ from $t_{k}$ up to infinity. In the next section, we will temporarily assume that knowledge is indeed available in order to devise an IS-MPC for the perturbed case. This hypothesis will be removed in Sect. V by introducing a disturbance observer to be used for implementing the stability constraint.

IV IS-MPC: The Known Disturbance Case

Assume that the disturbance $d$ is known over $[t_{k},\infty)$. From (11) we can then derive the following computable expression of the stability constraint (compare with (7))

[TABLE]

having denoted by $\Delta_{d}^{k}$ the correction term due to the disturbance

[TABLE]

Consistently with the assumption made for $x_{z}$ in Sect. II, suppose that the disturbance is piecewise-linear

[TABLE]

with $d^{i}=d(t_{i})$. Then, a simple computation gives

[TABLE]

Replacing constraint (7) in the QP formulation with constraint (12), where $\Delta_{d}^{k}$ is given by (15), and similarly for $y$, leads to an IS-MPC scheme where the control inputs (the ZMP velocities within the control horizon) are directly modified by the profile of the disturbance, realizing a form of indirect disturbance compensation222Direct compensation is not possible for system (10) because the control input and the disturbance act at different levels.. In particular, recursive feasibility will be achieved if sufficient preview information is available, and in turn this will guarantee internal stability [17, Props. 5 and 6].

To appreciate the effect of the compensation, consider the special case in which the humanoid must balance (i.e., footsteps are fixed) in the presence of a constant disturbance $\bar{d}={\bar{F}}_{\rm ext}/m$, arising from a constant force ${\bar{F}}_{\rm ext}$ pushing on the CoM. Under the action of IS-MPC with disturbance compensation, the robot converges to a steady state where — consistently with eq. (8) — the displacement between the CoM and the ZMP is

[TABLE]

This can be interpreted as the humanoid “leaning against” the force in order to counteract it (see Fig.2). Interestingly, eq. (15) in this case readily provides

[TABLE]

showing that the correction term due to the disturbance in the stability constraint (12) is exactly equal to the steady-state CoM-ZMP displacement.

A similar compensation effect occurs when walking. Figure 3 shows a gait generated using IS-MPC with disturbance compensation in the presence of a constant force acting on the CoM, in comparison with the gait generated by IS-MPC in the absence of disturbance. The simulation is run in MATLAB with quadprog as QP solver, and uses the following parameters: $m=4.5$ kg, $\bar{z}_{c}=0.33$ m, $f_{x}=f_{y}=0.05$ m, duration of the single and double support phases 0.2 and 0.3 s, respectively, $\delta=0.01$ s, $C=100$; in the perturbed case, the external force along the $x$ axis is 1.8 N, corresponding to a CoM acceleration of 0.4 m/s2, and the same along the $y$ axis. Again, observe how the robot is leaning against the disturbance, as the CoM trajectory is pushed in the opposite direction to the force.

Overall, the behavior resulting from IS-MPC with disturbance compensation can be interpreted as a rather natural anticipative action aimed at counteracting the effect of the disturbance.

V Observer-based IS-MPC

The previous assumption of complete knowledge of the disturbance can be justified in some special cases (e.g., when walking on an inclined plane of known slope), but will not be verified in general. To this end, we design in this section a disturbance observer and discuss its use within an IS-MPC scheme with disturbance compensation. We will then showcase the performance of the resulting gait generation method via simulations on the LIP model and dynamic simulations on the NAO humanoid robot.

V-A Disturbance observer

In general, the value of the disturbance $d$ is unknown. However, we can estimate it from other measurements; in particular, in the following we assume that the coordinates of the CoM and the ZMP, respectively $x_{c}$ and $x_{z}$, are measured (this is a rather standard occurrence in humanoids). Since $d$ is piecewise-linear (see Sect. IV), we can adopt the following disturbance model (exosystem):

[TABLE]

and use it to extend the perturbed model (10), obtaining a system with state $x=(x_{c},\dot{x}_{c},x_{z},d,\dot{d})$ and characterized by the following matrices

[TABLE]

Since the system is easily found to be observable, we can build an asymptotic observer

[TABLE]

where $\hat{x}$ is the observer state and $y$ are the available measurements. The gain matrix $G$ can be computed by simple pole placement. This observer is guaranteed to reconstruct asymptotically any piecewise-linear disturbance signal.

V-B Observer-based stability constraint

To perform IS-MPC with disturbance compensation in the general case when $d$ is unknown, the estimate $\hat{d}$ provided by the asymptotic observer (17) can be used in the stability constraint (12–13). Since the observer only produces the current value $\hat{d}^{k}$ of $\hat{d}$ and does not perform any kind of prediction, the straightforward choice is to compute the correction term $\Delta_{d}^{k}$ by replacing $d(\tau)$ with $\hat{d}^{k}$ in the integral:

[TABLE]

While this is obviously an approximation, it should be considered that in the MPC algorithm $\Delta_{d}^{k}$ is recomputed at each sampling instant; as a consequence, observer-based IS-MPC can provide compensation also for slowly-varying signals. This will be shown via simulations in the remainder of this section, which also discusses some additional ideas for achieving compensation of a larger class of disturbances.

V-C Simulations on the LIP

We describe now some MATLAB simulations of the perturbed LIP under the action of observer-based IS-MPC. The parameters are exactly the same of the simulation in Sect. IV.

In the first simulation, shown in Fig. 4, the LIP is subject to the same constant disturbance of Fig. 3, i.e., $\bar{d}=0.4$ m/s2 on both $x$ and $y$. The observed disturbance $\hat{d}$ converges therefore to the actual value $\bar{d}$. As a consequence, the resulting gait is almost indistinguishable from that generated by IS-MPC when the disturbance is known (compare with Fig. 3).

In the second simulation, we add on both $x$ and $y$ the disturbance signal $d(t)=0.2+0.15\sin(0.45\pi t)$ m/s2, which is outside the piecewise-linear family due to the sinusoidal term. As shown in Fig. 5, observer-based IS-MPC is still able to produce a stable gait. This proves that the proposed method is robust to two distinct sources of discrepancy: (1) the fact that the observer cannot provide an asymptotically exact estimate of $d$ and (2) the use of the constant value $\hat{d}^{k}$ in the stability constraint.

The disturbance signal used in the third simulation is $d(t)=0.2+0.15\sin(2\pi t)$ m/s2, which includes a sinusoidal term that varies more rapidly. Pure observer-based IS-MPC fails in this case because it becomes unfeasible (results not shown). However, as shown in Fig. 6, feasibility is recovered if the ZMP constraints are restricted progressively along the control horizon. This stratagem, inspired by [7], is effective in this case because the ZMP constraint restriction can be shown to be beneficial for feasibility. Note how the gait is quite different from that produced by IS-MPC if the disturbance is known, also shown in Fig. 6; this is consistent with the reduced admissible regions for the ZMP.

Finally, we have simulated an observer-based IS-MPC scheme with automatic footstep placement in the presence of a constant disturbance $\bar{d}=0.4$ m/s2 acting along the $y$ axis. To perform AFS, the footstep positions are added to the decision variables of the MPC, while the cost function is modified by including a term for tracking a reference velocity of the CoM [6], in this case 0.1 m/s along the $x$ axis. In the resulting gait, shown in Fig. 7, one observes the expected displacement of the footsteps due to the disturbance.

V-D Dynamic simulations on NAO

As a further validation step, we have performed dynamic simulations of the proposed method for a NAO humanoid robot in DART (Dynamic Animation and Robotics Toolkit). The qpOASES library was used to solve the QP. The robot and gait parameters are the same as in the previous simulations, except for $\delta=0.05$ s and $C=20$.

In the first dynamic simulation, a constant external force of $3.8$ N along the sagittal axis is applied to the robot CoM. As shown in Fig. 8, the robot falls when nominal IS-MPC is used, whereas observer-based IS-MPC allows to counteract the disturbance successfully, producing the aforementioned effect of leaning against the force. An interesting aspect of this simulation, clearly shown in the bottom plot, is that the observer does not estimate only the constant force, because it also reacts to dynamic effects that are not modeled in the LIP (see (9).

In the second simulation, $F_{\rm ext}=2+3.8\sin(0.45\pi t)$ N includes a slow sinusoidal component. Figure 9 shows a comparison between the CoM trajectories generated by nominal vs. observer-based IS-MPC. Once again, the first fails while the second is able to maintain balance while walking.

In a third simulation, shown in Fig. 10, we considered a more realistic scenario where the disturbance is not directly applied to the robot CoM. In particular, the humanoid is carrying attached to its arm a $0.2$ kg pendulum. This could represent, e.g., an oscillating weight such as a shopping bag. Thanks to the use of observer-based IS-MPC, the robot successfully counteracts the disturbance.

Movie clips of the above dynamic simulations are shown in the video attachment.

VI Conclusions

We have presented an extension of our previously proposed IS-MPC scheme which is able to generate stable humanoid gaits in the presence of persistent disturbances. To this end, it incorporates a disturbance observer providing an estimate which is then used to correct appropriately the stability constraints. The resulting observer-based IS-MPC scheme was validated via simulations on a LIP model and a NAO humanoid, showing successful gait generation for a wide range of applied disturbances.

Future work will include:

• •

experimental validation of the proposed scheme (note that the computational load of observer-based IS-MPC is virtually the same of the standard IS-MPC, so that a real-time implementation is possible);

• •

adaptation to more general classes of disturbances;

• •

a study of the conditions for recursive feasibility of the observer-based IS-MPC algorithm.