MPC for Humanoid Gait Generation: Stability and Feasibility

TL;DR

This paper introduces IS-MPC, a stable model predictive control framework for humanoid gait generation that guarantees stability and feasibility through explicit constraints, validated by simulations and real robot experiments.

Contribution

It proposes a novel stable MPC formulation with explicit stability constraints for humanoid gait generation, ensuring bounded CoM trajectories and recursive feasibility.

Findings

Guarantees stability of CoM/ZMP dynamics.

Ensures recursive feasibility of the MPC algorithm.

Validated on NAO and HRP-4 humanoid robots.

Abstract

We present IS-MPC, an intrinsically stable MPC framework for humanoid gait generation which incorporates an explicit stability constraint in the formulation. The proposed method uses as prediction model a dynamically extended LIP where ZMP velocities are the control inputs, producing in real time a gait (including footsteps with the associated timing) that realizes omnidirectional motion commands coming from an external source. The stability constraint links the future ZMP velocities to the current system state so as to guarantee the essential requirement that the generated CoM trajectory is bounded with respect to the ZMP trajectory. Since the control horizon of the MPC algorithm is finite, only part of the future ZMP velocities are decision variables of the QP problem; the remaining part, called tail, must be either conjectured or anticipated using preview information on the reference motion. Several possible options for the tail are discussed, and each of them is shown to correspond to a specific terminal constraint. A theoretical analysis of the feasibility of the generic MPC iteration is developed and used to obtain sufficient conditions for recursive feasibility. Finally, it is proved that IS-MPC guarantees stability of the CoM/ZMP dynamics if it is recursively feasible. Simulation and experimental results on the NAO and the HRP-4 humanoids are presented to illustrate the performance of the proposed method.

Figures

Equations

$${\hat{X}}_f^k$$

$${\hat{Y}}_f^k$$

$${\Theta }_f^k$$

$${\mathcal{T}}_s^k=\{T_s^1,\dots ,T_s^F\},$$

$$v=\bar{v}+\Delta v=\frac{{\bar{L}}_s+\Delta L_s}{{\overline{T}}_s-\Delta T_s},$$

$$T_s={\overline{T}}_s\frac{\alpha +\bar{v}}{\alpha +v}.$$

$$t_s^j=t_s^{j-1}+{\overline{T}}_s\frac{\alpha +\bar{v}}{\alpha +v(t_s^{j-1})},$$

$$\left(\begin{array}[]{c}\dot{x}\\ \dot{y}\\ \dot{\theta}\end{array}\right)=\left(\begin{array}[]{ccc}\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1\end{array}\right)\left(\begin{array}[]{c}v_{x}\\ v_{y}\\ \omega\end{array}\right).$$

$$\left(\begin{array}[]{c}\Delta x^{j}\\ \Delta y^{j}\end{array}\right)=\int_{t^{j-1}_{s}}^{t^{j}_{s}}R_{\theta}\left(\begin{array}[]{c}v_{x}(\tau)\\ v_{y}(\tau)\end{array}\right)d\tau\pm R_{j}\left(\begin{array}[]{c}0\\ \ell/2\end{array}\right),$$

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

$$\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}.$$

$${\dot{x}}_z(t)={\dot{x}}_z^i,t\in [t_i,t_{i+1}).$$

$$x_z(t)=x_z^i+(t-t_i){\dot{x}}_z^i,\text{mbox}with|{\dot{x}}_z^i|\le \gamma ,$$

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

$$R_{j-1}^{T}\left(\begin{array}[]{c}x_{f}^{j}-x_{f}^{j-1}\\[5.0pt] y_{f}^{j}-y_{f}^{j-1}\end{array}\right)\leq\pm\left(\begin{array}[]{c}0\\[5.0pt] \ell\end{array}\right)+\frac{1}{2}\left(\begin{array}[]{c}d_{a,x}\\[5.0pt] d_{a,y}\end{array}\right),$$

$$x_s$$

$$x_u$$

$${\dot{x}}_s$$

$${\dot{x}}_u$$

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

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

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

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

$$x_z(t)=x_z^k+\sum _{i=0}^{\infty }(\rho (t-t_{k+i})-\rho (t-t_{k+i+1})){\dot{x}}_z^{k+i},$$

$${\int }_{t_k}^{\infty }{e}^{-\eta (\tau -t_k)}(\rho (\tau -t_{k+i})-\rho (\tau -t_{k+i+1}))d\tau =\frac{1-e^{-\eta \delta }}{{\eta }^2}{e}^{-i\eta \delta }.$$

$$x_z(t)=x_z^k+$$

$$+$$

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

$$\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),$$

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

$${\dot{\tilde{x}}}_z^{k+i}=0\text{mbox}fori\ge C.$$

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

$$x_u^{k+C}=x_z^{k+C}.$$

Full text

I Introduction

Many gait generation approaches for humanoids guarantee that balance is maintained during locomotion by enforcing the condition that the Zero Moment Point (ZMP, the point where the horizontal component of the moment of the ground reaction forces becomes zero) remains at all times within the support polygon of the robot. Correspondingly, these approaches identify the ZMP as the fundamental variable to be controlled.

Due to the complexity of full humanoid dynamics, however, direct control of the ZMP is very difficult to achieve. In view of this, simplified models are generally used to relate the evolution of the ZMP to that of the Center of Mass (CoM) of the robot, which can be instead effectively controlled. Widely adopted linear models are the Linear Inverted Pendulum (LIP), in which the ZMP represents an input, and the Cart-Table (CT), where the ZMP appears as the output [1]. The first is appropriate for inversion-based control approaches: given a sequence of footsteps, and thus a ZMP trajectory interpolating them, the LIP is used to compute a CoM trajectory which corresponds to the ZMP trajectory; see, e.g., [2, 3, 4]. The CT model lends itself more naturally to the design of feedback laws for tracking ZMP trajectories, the most successful example in this context being the LQ preview controller of [5].

Regardless of the adopted model, there is a potential instability issue at the heart of the problem. In particular, a certain ZMP trajectory may be realized by an infinity of CoM trajectories, which, due to the nature of the CoM/ZMP dynamics, will in general be divergent with respect to the ZMP trajectory itself. In this situation, dynamic balance can be in principle achieved by properly choosing the ZMP trajectory, but internal instability indicates that such motion will not be feasible in practice for the humanoid.

The seminal paper [6] reformulates the gait generation problem in a Model Predictive Control (MPC) setting. This is convenient because it allows to generate simultaneously the ZMP and the CoM trajectories while satisfying constraints, such as the ZMP balance condition as well as kinematic constraints on the maximum step length and foot rotation [7]. Moreover, the MPC approach guarantees a certain robustness against perturbations. It is therefore not surprising that it has been adopted in many methods for gait generation; e.g., see [8, 9, 10, 11] for linear MPC and [12, 13] for nonlinear MPC.

As for all control schemes, a fundamental issue in MPC approaches is the stability of the obtained closed-loop system, especially in view of the previous remark about the instability of the CoM/ZMP dynamics. As discussed in [14], two main approaches have emerged for achieving stability when MPC is used for humanoid gait generation. The first is heuristic in nature and consists in using a sufficiently long control horizon [15], so that the optimization process can discriminate against diverging behaviors, as done for example in [7]. The second approach has been to enforce a terminal state constraint (i.e., a constraint on the state at the end of the control horizon), based on the fact that the MPC literature highlights the beneficial role of such constraints for closed-loop stability in set-point control problems [16].

In particular, terminal constraints were used for humanoid balancing in [17] and for gait generation in [18]. The latter makes use of a LIP model, requiring its unstable component to stop at the end of the control horizon, a kind of terminal constraint referred to as capturability constraint (from the concept of capture point [19]). This constraint has also been used in [20], where it is imposed only at the foot landing instant, and in [21], which addresses locomotion in a multi-contact setting.

Another approach focusing on the instability issue relies on the concept of Divergent Component of Motion (DCM), used in [22] to identify an initial condition for stable execution of regular gaits, and in [23] to realize transitions between bipedal and quadrupedal gaits. The DCM concept has also been extended to the 3D context in [24, 25]. More relevant to our review is [26], which presents an MPC scheme for gait generation that enforces a terminal constraint (actually converted to a terminal cost for the sake of feasibility) on the DCM component.

In this paper, we move from the fundamental observation that the control problem addressed in MPC-based gait generation is neither a set-point nor a tracking problem. In fact, since the ZMP control objective is encoded via time-varying state constraints, there is no error to be regulated to (or close to) zero. The only significant stability issue in this context is internal stability, i.e., the boundedness of the CoM trajectory with respect to the ZMP trajectory. Therefore, one cannot simply claim that the use of a terminal constraint will automatically entail internal stability. In fact, to the best of our knowledge, no MPC-based gait generation method exists in the literature for which a rigorous analysis of the stability issue has been performed in connection with the use and the choice of a terminal constraint.

Another tightly related aspect to be considered is that terminal constraints may have a detrimental effect on feasibility, i.e., the existence of solutions for the optimization problem which is at the core of any MPC scheme [27]. A particularly desirable property is recursive feasibility, which entails that if the optimization problem is feasible at a certain iteration it will remain such in future iterations. It appears that this also crucial issue has seldom been explored for MPC-based gait generation, with the notable exceptions of [28, 29].

In [30] we have introduced a novel MPC approach for humanoid gait generation which relies on the inclusion of an explicit stability constraint in the formulation of the problem. In particular, the idea was to enforce a condition on the future ZMP velocities (representing the control inputs) so as to guarantee that the generated CoM trajectory remains bounded with respect to the ZMP trajectory. Since the control horizon of the MPC algorithm is finite, only part of the future ZMP velocities are decision variables and can therefore be subject to a constraint; the remaining part, called tail, must be conjectured.

Here, we fully develop our approach into a complete, Intrinsically Stable MPC (IS-MPC) framework for gait generation. In particular, the paper adds the following contributions with respect to [30]:

we describe a footstep generation module that can be used in conjunction with our MPC scheme in order to modify step timing and length in real time in response to omnidirectional motion commands coming from a higher-level module; 2. 2.

depending on the available preview information on the commanded motion, we discuss several versions of the tail (truncated, periodic, anticipative) to be used in the stability constraint, and show that each of them corresponds to a specific terminal constraint; 3. 3.

we analyze in detail the impact of the new constraint on feasibility, and show analytically how, under certain assumptions, it is possibile to guarantee recursive feasibility of the IS-MPC scheme; 4. 4.

we prove that recursive feasibility of IS-MPC implies the desired internal stability of the CoM/ZMP dynamics; 5. 5.

we validate our findings by providing dynamic simulations and actual experiments on two different humanoid robots: an HRP-4 and a NAO.

The results on tails, recursive feasibility and internal stability are the main contributions of this paper. We consider them particularly important because they indicate that, contrarily to what is often claimed in the literature, simply adding a terminal constraint (e.g., the capturability constraint) does not per se guarantee stability of MPC-based gait generation schemes. Indeed, the appropriate tail to be used in the stability constraint — equivalently, the appropriate terminal constraint — depends upon the future characteristics of the commanded motion. In this sense, to guarantee recursive feasibility one should always choose the anticipative tail, which makes the most use of the available preview information on such motion. Once recursive feasibility is achieved, CoM/ZMP stability is automatically ensured in IS-MPC.

Another potential benefit of the theoretical analysis of feasibility is that it paves the road for a formal study of the robustness of IS-MPC. Although this is out of the scope of this paper, by relying on this analysis it should be possible to devise modifications of the basic scheme which will preserve recursive feasibility in the presence of quantified bounded uncertainties and/or disturbances.

The paper is organized as follows. In the next section, we formulate the considered gait generation problem and discuss the structure of the proposed approach. Section III describes the algorithm which generates timing and locations of the candidate footsteps. In Sect. IV we introduce the prediction model and the constraints used in the IS-MPC scheme, with the exception of the stability constraint which is given a thorough discussion in the dedicated Sect. V. The IS-MPC algorithm is described in detail in Sect. VI. Section VII addresses the central issues of stability and feasibility of the proposed method; in particular, a theoretical analysis of the feasibility of the generic IS-MPC iteration is presented and used to obtain sufficient conditions for recursive feasibility, whose role in guaranteeing stability is rigorously established. Simulations on the HRP-4 humanoid are presented in Sect. VIII, while experimental results on both the NAO and the HRP-4 humanoids are shown in Sect. IX. Section X offers a few concluding remarks.

II Problem and Approach

Consider the problem of generating a walking gait for a humanoid in response to high-level reference velocities, which are given as the driving ($v_{x}$, $v_{y}$) and steering ($\omega$) velocities of an omnidirectional single-body mobile robot chosen as a template model for motion generation. These velocities, which may encode a persistent trajectory or converge to a stationary point, are produced by an external source; this could be a human operator in a shared control context, or another module of the control architecture working in open-loop (planning) or in closed-loop (feedback control).

The proposed MPC-based framework, whose block scheme is shown in Fig. 1, works in a digital fashion over sampling intervals of duration $\delta$. Throughout the paper, it is assumed that the reference velocities $v_{x}$, $v_{y}$, $\omega$ are made available for gait generation with a preview horizon $T_{p}=P\cdot\delta$, with $P$ the number of intervals within the preview horizon. At the generic instant $t_{k}=k\cdot\delta$, the high-level references velocities over $[t_{k},t_{k}+T_{p}]$ are then sent to the footstep generation module, which uses Quadratic Programming (QP) to generate candidate footsteps over the same interval. In particular, vectors $\hat{X}_{f}^{k}$, $\hat{Y}_{f}^{k}$ collect the Cartesian positions of the footsteps, with the ‘hat’ indicating that these are candidates which can be modified by the MPC module; whereas vector $\Theta_{f}^{k}$ collects the footstep orientations, which will not be modified. The footstep generation module also generates the timing ${\cal T}_{s}^{k}$ of the sequence.

The output of the footstep generation module is sent to the Intrinsically Stable MPC (IS-MPC) module, which solves another QP problem to produce in real time the actual footstep positions $X_{f}^{k}$, $Y_{f}^{k}$ and the trajectory {\mbox{\boldmathp}}^{\ast}_{c} of the humanoid CoM over the control horizon $T_{c}=C\cdot\delta$, with $C$ the number of intervals within the control horizon. It is assumed that $T_{c}\leq T_{p}$, i.e., $C\leq P$. The inclusion of a stability constraint in the formulation guarantees that the CoM trajectory will be bounded, in a sense to be made precise later.

The pose (position and orientation) of the footsteps with the associated timing is used to generate — still in real time — the swing foot trajectory {\mbox{\boldmathp}}^{\ast}_{\it swg} over the control horizon. Together with the CoM trajectory, this is sent to the kinematic control block, which generates velocity inputs at the joint level in order to achieve output tracking (we are assuming that the humanoid robot is velocity- or position-controlled).

In the next sections we will discuss in detail the proposed control scheme. We will first describe the footstep generation scheme, and then turn our attention to the IS-MPC algorithm, which is our core contribution. The kinematic control block can use any standard pseudoinverse-based feedback law and therefore will not be discussed further.

III Candidate Footstep Generation

The proposed footstep generation module runs synchronously with the IS-MPC scheme and chooses both the timing and the candidate location of the next footsteps in response to the high-level reference velocities. Timing is determined first by a simple rule expressing the fact that a change in the reference velocity should affect both the step duration and length. The candidate footstep locations are then chosen through quadratic optimization.

Note that generating the timing and the orientation of the candidate footsteps outside the IS-MPC is essential to retain the linear structure of the latter. The IS-MPC scheme will still be able to adapt the position of the footsteps to guarantee reactivity to disturbances.

At each sampling instant $t_{k}$, the candidate footstep generation module receives in input the high-level reference velocities over the preview horizon, i.e., from $t_{k}$ to $t_{k}+T_{p}=t_{k+P}$ (see Fig. 1). In output, it provides the candidate footstep sequence $(\hat{X}^{k}_{f},\hat{Y}^{k}_{f},\Theta^{k}_{f})$ over the same interval with the associated timing ${\cal T}_{s}^{k}$. In particular, these quantities are defined111To keep a light notation, the $k$ symbol identifying the current sampling instant is used for the sequence vectors but not for their individual elements. as

[TABLE]

and

[TABLE]

where $(x_{f}^{j},y_{f}^{j},\theta_{f}^{j})$ is the pose of the $j$-th footstep in the preview horizon and $T^{j}_{s}$ is the duration of the step between the $(j-1)$-th and the $j$-th footstep, taken from the start of the single support phase to the next. Since the duration of steps is variable, the number $F$ of footsteps falling within the preview horizon $T_{p}$ may change at each $t_{k}$.

Below, we discuss first how timing is determined and then describe the procedure for generating the candidate footsteps.

III-A Candidate Footstep Timing

In our method, the duration $T_{s}$ of each step is related to the magnitude $v=(v_{x}^{2}+v_{y}^{2})^{1/2}$ of the reference Cartesian velocity at the beginning of that step.

Assume that a triplet of cruise parameters $(\bar{v},\overline{T}_{s},\bar{L}_{s})$ has been chosen, where $\bar{v}$ is a central value of $v$ and $\overline{T}_{s}$, $\bar{L}_{s}$ are the corresponding values of the step duration and length, respectively, with $\bar{v}=\bar{L}_{s}/\overline{T}_{s}$. The choice of these parameters will depend on the specific kinematic and dynamic capabilities of the humanoid robot under consideration.

The idea is that a deviation from $\bar{v}$ should reflect on a change in both $T_{s}$ and $L_{s}$. In formulas:

[TABLE]

with $\Delta L_{s}=\alpha\Delta T_{s}$. One easily obtains

[TABLE]

Figure 2 shows the resulting rule for determining $T_{s}$ as a function of $v$ in comparison to other possible rules. For illustration, we have set $\bar{v}=0.15$ m/s, $\overline{T}_{s}=0.8$ s, $\bar{L}_{s}=0.12$ m and $\alpha=0.1$ m/s. It is confirmed that an increase of $v$, for example, corresponds to both a decrease of $T_{s}$ and an increase in $L_{s}$.

Note that the reference angular velocity $\omega$ does not enter into rule (1). The rationale is that the step duration and length along curved and rectilinear paths do not differ significantly if the Cartesian velocity $v$ is the same. For a purely rotational motion ($v=0$) where the humanoid is only required to rotate on the spot, the above rule would yield the maximum value of $T_{s}$.

In practice, equation (1) is iterated along the preview horizon $[t_{k},t_{k}+T_{p}]$ in order to obtain the footstep timestamps:

[TABLE]

with $t^{0}_{s}$ equal to the timestamp of the last footstep before $t_{k}$. Iterations must be stopped as soon as $t^{j}_{s}>t_{k+P}$, discarding the last generated timestamp since it will be outside the preview horizon. The resulting step timing will be ${\cal T}^{k}_{s}=\{T^{1}_{s},\ldots,T^{F}_{s}\}$, with $T^{j}_{s}=t_{s}^{j+1}-t^{j}_{s}$.

III-B Candidate Footstep Placement

Once the timing of the steps in the preview horizon $[t_{k},t_{k}+T_{p}]$ has been chosen, the poses of candidate footsteps are generated. To this end, we use a reference trajectory obtained by integrating the following template model under the action of the high-level reference velocities over $T_{p}$:

[TABLE]

This is an omnidirectional motion model which allows the template robot to move along any Cartesian path with any orientation, so as to perform, e.g., lateral walks, diagonal walks, and so on.

The idea is to distribute the candidate footsteps around the reference trajectory in accordance to the timing ${\cal T}_{s}^{k}$ while taking into account the kinematic constraints of the robot. These constraints will also be used in the IS-MPC stage, and therefore we will provide their description directly in Sect. IV-C (see also Fig. 7).

A sequence of two QP problems is solved. The first is

\left\{\begin{minipage}{433.62pt} $$\min_{\Theta^{k}_{f}}\>\sum_{j=1}^{F}(\theta^{j}_{f}-\theta^{j-1}_{f}-\int_{t^{j-1}_{s}}^{t^{j}_{s}}\omega(\tau)d\tau)^{2}$$ $$\mbox{subject to}\quad|\theta^{j}_{f}-\theta^{j-1}_{f}|\leq\theta_{\rm max}$$ \end{minipage}\right.

Here, $\theta_{\rm max}$ is the maximum allowed rotation between two consecutive footsteps. The second QP problem is

\left\{\begin{minipage}{433.62pt} $$\min_{\hat{X}_{f}^{k},\hat{Y}_{f}^{k}}\>\sum_{j=1}^{F}(\hat{x}_{f}^{j}-\hat{x}_{f}^{j-1}-\Delta x^{j})^{2}+(\hat{y}_{f}^{j}-\hat{y}_{f}^{j-1}-\Delta y^{j})^{2}$$ \centerline{\hbox{subject to kinematic constraints~{}(\ref{eq:footposcon})}} \end{minipage}\right.

Here, $(\hat{x}_{f}^{0},\hat{y}_{f}^{0})$ is the known position of the support foot at $t_{k}$ and $\Delta x^{j}$, $\Delta y^{j}$ are given by

[TABLE]

where $R_{\theta}$, $R_{j}$ are the rotation matrices associated respectively to $\theta(\tau)$ (the orientation of the template robot at any given time $\tau$) and to the footstep orientation $\theta_{j}$, and $\ell$ is the reference coronal distance between consecutive footsteps. The sign of the second term alternates for left/right footsteps.

At the end of this procedure, the candidate footstep sequence $(\hat{X}^{k}_{f},\hat{Y}^{k}_{f},\Theta^{k}_{f})$ with the associated timing ${\cal T}_{s}^{k}$ is sent to the IS-MPC stage. The final footstep positions $(X^{k}_{f},Y^{k}_{f})$ will be determined by the latter while the footstep orientations $\Theta^{k}_{f}$ and timing ${\cal T}_{s}^{k}$ will not be modified.

Some examples of candidate footsteps generation are shown in Fig. 3. Note that the orientation of the humanoid robot is tangent to the path for the circular walk, but is kept constant ($\omega=0$) for the other two walks, which represent then proper examples of omnidirectional motion.

IV IS-MPC: Prediction Model and Constraints

The IS-MPC module uses the Linear Inverted Pendulum (LIP) as a prediction model. The constraints are of three kinds. The first concerns the position of the ZMP, which must be at all times within the support polygon defined by the footstep sequence and the associated timing. The second type of constraint ensures that the generated steps are compatible with the kinematic capabilities of the robot. The third is the new stability constraint guaranteeing that the CoM trajectory generated by our MPC scheme will be bounded with respect to the ZMP trajectory. The first two constraints must be verified throughout the control horizon, whereas the third is a single scalar condition on each coordinate.

In this section, we discuss in detail the prediction model and the constraints on ZMP and kinematic feasibility. The next section will be devoted to the stability constraint, which deserves a thorough discussion.

IV-A Prediction Model

The LIP is a popular choice for describing the motion of the CoM of a biped walking on flat horizontal floor when its height is kept constant and no rotational effects are present. From now on, we express motions in the robot frame, which has its origin at the center of the current support foot, the $x$-axis (sagittal) aligned with the support foot, and the $y$-axis (coronal) orthogonal to the $x$-axis. In the LIP model, which applies to both point feet and finite-sized feet, the dynamics along the sagittal and coronal axes are governed by decoupled, identical linear differential equations.

Consider for illustration the motion along the $x$ axis (see Fig. 4), and let $x_{c}$ and $x_{z}$ be respectively the coordinate of the CoM and the ZMP. The LIP dynamics is

[TABLE]

where $\eta=\sqrt{g/h_{c}}$, with $g$ the gravity acceleration and $h_{c}$ the constant height of the CoM. In this model, the ZMP position $x_{z}$ represents the input, whereas the CoM position $x_{c}$ is the output.

To obtain smoother trajectories, we take the ZMP velocity $\dot{x}_{z}$ as the actual control input. This leads to the following third-order prediction model (LIP $+$ dynamic extension)

[TABLE]

Our MPC scheme uses piecewise-constant control over the sampling intervals (see Fig. 5):

[TABLE]

In particular, a bound of the form $|\dot{x}_{z}^{i}|\leq\gamma$, with $\gamma$ a positive constant, will be satisfied for all $i$. In fact, the reference velocities $v_{x}$, $v_{y}$, $\omega$ will be bounded in any realistic gait generation problem. As shown by Fig. 2, the footstep generation module will then produce a sequence of footsteps along which the step duration is bounded below. This timing will be reflected in the associated ZMP constraints (see Sect. IV-B), which will in turn entail as solution a piecewise-continuous trajectory $x_{z}(t)$ with bounded derivative. Therefore, for $t\in[t_{i},t_{i+1}]$ it will be

[TABLE]

where we have used the notation $x_{z}^{i}=x_{z}(t_{i})$.

The generic iteration of IS-MPC plans over the control horizon, i.e., from $t_{k}$ to $t_{k}+T_{c}=t_{k+C}$. Since $T_{c}\leq T_{p}$, a subset of the $F$ candidate footsteps produced by the footstep generation module fall inside the control horizon; denote their number by $F^{\prime}<F$. The MPC iteration will then generate:

• $\bullet$

the control variables, i.e., the input values $\dot{x}_{z}^{k+i}$, $\dot{y}_{z}^{k+i}$, for $i=0,\dots,C-1$;

• $\bullet$

the other decision variables, i.e., the actual footstep positions $(x_{f}^{j},x_{f}^{j})$, for $j\!=\!1,\ldots,F^{\prime}$.

• $\bullet$

as a byproduct, the output history $x_{c}(t)$, $y_{c}(t)$, for $t\in[t_{k},t_{k+C}]$ which will be ultimately used to drive the actual humanoid.

As already mentioned, the orientations of the footsteps are instead inherited from the generated sequence (more on this in Sect. IV-B).

Note that the footsteps do not appear in the prediction model, but will show up in the constraints, as discussed in the rest of this section.

IV-B ZMP Constraints

The first constraint guarantees dynamic balance by imposing that the ZMP lies inside the current support polygon at all time instants within the control horizon.

When the robot is in single support on the $j$-th footstep, the admissible region for the ZMP is the interior of the footstep, which can be approximated as a rectangle of dimensions $d_{z,x},d_{z,y}$, centered at $(x_{f}^{j},y_{f}^{j})$, and oriented as $\theta^{j}$. Using the fact that the ZMP profile is piecewise-linear as entailed by (5), the constraint can be expressed as222For compactness, we shall only write the right-hand side of bilateral inequality constraints. For example, constraint (6) should be completed by a left-hand side obtained by adding (rather than subtracting) the two terms that appear in the right-hand side.:

[TABLE]

If the above sampled-time ZMP constraint is satisfied, then the original continuous-time constraint is also satisfied thanks to the linearity of $x_{z}(t)$ within each sampling interval. Constraint (6), complete with the corresponding left-hand side, must be imposed throughout the control horizon ($i=0,\dots,C-1$) and for all the associated footsteps ($j=0,\dots,F^{\prime}$).

Note that constraint (6) is nonlinear in the footstep orientation $\theta^{j}$, which however is not a decision variable, being simply inherited from the footstep generation module. The constraint is instead linear in $x_{f}^{j}$, $y_{f}^{j}$, as well as in the ZMP velocity inputs.

During double support, the support polygon would be the convex hull of the two footsteps, whose boundary is a nonlinear function of their relative position. To preserve linearity, we adopt an approach based on moving constraints [31]. In particular, the admissible region for the ZMP in double support has exactly the same shape and dimensions it has in single support, and it roto-translates (i.e., simultaneously rotates and translates) from one footstep to the other in such a way to always remain in the support polygon (see Fig. 6). This results in a slightly conservative constraint which is however linear in the decision variables.

IV-C Kinematic Constraints

The second type of constraint is introduced to ensure that all steps are compatible with the robot kinematic limits. Consider the $j$-th step in $T_{c}$, with the support foot centered at $(x_{f}^{j-1},y_{f}^{j-1})$ and oriented as $\theta^{j-1}$. The admissible region for placing the footstep is defined as a rectangle having the same orientation $\theta^{j-1}$ and whose center is displaced from the support foot center by a distance $\ell$ in the coronal direction (see Fig. 7). Denoting by $d_{a,x}$ and $d_{a,y}$ the dimensions of the kinematically admissible region, the constraint can be written as

[TABLE]

with the sign alternating for the two feet. The above constraint, complete with the corresponding left-hand side, must be imposed for all footsteps in the control horizon ($j=1,\dots,F^{\prime}$).

V IS-MPC: Enforcing Stability

The LIP dynamics (3) is inherently unstable. As a consequence, even when the ZMP lies at all times within the support polygon (gait balance) it may still happen that the CoM diverges exponentially with respect to the ZMP; in this case, the gait would obviously become unfeasible in practice, due to the kinematic limitations of the robot. The role of the stability constraint is then to guarantee that the CoM trajectory remains bounded with respect to the ZMP (internal stability).

In this section, we first describe the structure of the stability constraint and then discuss the possible tails for its implementation.

V-A Stability Constraint

Since we want to enforce boundedness of the CoM w.r.t. the ZMP, we can ignore the dynamic extension and focus directly on the LIP system.

By using the following change of coordinates

[TABLE]

the LIP part of system (3) is decomposed into a stable and an unstable subsystem:

[TABLE]

The unstable component $x_{u}$ is also known as divergent component of motion (DCM) [22] or capture point [32].

In spite of the LIP instability, for any input ZMP trajectory $x_{z}(t)$ of the form (5) there exists a special initialization of $x_{u}$ such that the resulting output CoM trajectory is bounded with respect to the input [33]. In particular, this is the (only) initial condition on $x_{u}$ for which the free evolution of (11) exactly cancels the component of the forced evolution that would diverge with respect to $x_{z}(t)$. In the MPC context, where the initial condition at $t_{k}$ is denoted by $x_{u}(t_{k})=x_{u}^{k}$, the special initialization is expressed as

[TABLE]

Note that this particular initialization depends on the future values of the LIP input, i.e., the ZMP coordinate $x_{z}$. In the following, we refer to (12) as the stability condition.

The stability condition, which involves $x_{u}$ at the initial instant $t_{k}$ of the control horizon, can be propagated to its final instant $t_{k+C}$ by integrating (11) from $x_{u}^{k}$ in (12):

[TABLE]

Condition (12) — or equivalently, (13) — can be used to set up the corresponding constraint for the MPC problem. To this end, we use the piecewise-linear profile (5) of $x_{z}$ to obtain explicit forms.

Proposition 1

For the piecewise-linear $x_{z}$ in (5), condition (12) becomes

[TABLE]

while (13) takes the form

[TABLE]

Proof. Rewrite eq. (5) as

[TABLE]

where $\rho(t)=t\,\delta_{-1}(t)$ denotes the unit ramp and $\delta_{-1}(t)$ the unit step. Using Properties 1, 4 and 3 given in the Appendix, we get

[TABLE]

Plugging this expression in condition (12) and using Property 2 of the Appendix one obtains (14).

To prove (15), rewrite (16) as

[TABLE]

The contribution of the first two terms of $x_{z}$ to the integral in (13) is $x_{z}^{k+C}$. Using Properties 1, 3 and 4 one verifies that the contribution of the third term is exactly the second term in the right hand side of (15). This completes the proof.

In (14), one should logically separate the values of $\dot{x}_{z}^{i}$ within the control horizon, i.e. the control variables $\dot{x}_{z}^{i}$ for $i=k,\dots,k+C-1$, from the remaining values, i.e., from $k+C$ on. The infinite summation is then split in two parts and (14) can be rearranged as333Constraint (17) can be written as a function of the actual state variables of our prediction model ($x_{c}$, $\dot{x}_{c}$ and $x_{z}$) using the coordinate transformation (9). The same is true for all subsequent forms of the stability constraint as well as of the terminal constraint.

[TABLE]

Observe the inversion between (14), which expresses the stable initialization at $t_{k}$ for a given $x_{z}(t)$, and (17), which constrains the control variables so that the associated stable initialization matches the current state at $t_{k}$. In the following, we will refer to (17) as the stability constraint.

The control variables do not appear in condition (15), which involves only the value of the state variable $x_{u}^{k+C}$ at the end of the control horizon. In other terms, this condition represents what is called a terminal constraint in the MPC literature.

Both the stability and the terminal constraint contain an infinite summation which depends on $\dot{x}_{z}^{k+C}$, $\dot{x}_{z}^{k+C+1},\dots$, i.e., the ZMP velocities after the control horizon. These are obviously unknown, because they will be determined by future iterations of the MPC algorithm; as a consequence, including either of the constraints in the MPC formulation would lead to a non-causal (unrealizable) controller. However, by exploiting the preview information on $v_{x}$, $v_{y}$, $\omega$, we can make an informed conjecture at $t_{k}$ about these ZMP velocities, which we will denote by $\dot{\tilde{x}}_{z}^{k+C}$, $\dot{\tilde{x}}_{z}^{k+C+1},\dots$ and refer to collectively as the tail in the following. Correspondingly, the stability constraint (17) assumes the form

[TABLE]

while the terminal constraint (15) becomes

[TABLE]

Using either of these in the MPC formulation will lead to a causal (realizable) controller.

V-B Tails

We now discuss three possible options for the structure of the tail depending on the assumed behavior of the ZMP velocities after the control horizon. Basically, they correspond to (i) neglecting them (ii) assuming they are periodic (iii) anticipating a more general profile based on preview information. For each option, we shall explicitly compute the corresponding form of both the stability and the terminal constraint.

V-B1 Truncated Tail

The simplest option is to truncate the tail, by assuming that the corresponding ZMP velocities are all zero. This is a sensible choice if the preview information indicates that the robot is expected to stop at the end of the control horizon.

Proposition 2

Let (truncated tail)

[TABLE]

The stability constraint becomes

[TABLE]

while the terminal constraint becomes

[TABLE]

Proof. The above expressions are readily derived from the general constraints (18) and (19), respectively.

Interestingly, the terminal constraint (21) is equivalent to the capturability constraint, originally introduced in [18].

V-B2 Periodic Tail

The second option is to use a periodic tail obtained by infinite replication of the ZMP velocities within the control horizon. This assumption is justified when the reference velocities are themselves periodic (in particular, constant) in $T_{c}$, which is typically chosen as the gait period (total duration of two consecutive steps) or a multiple of it. Formulas for a replication period different from the control horizon may be easily derived.

Proposition 3

Let (periodic tail)

[TABLE]

The stability constraint becomes

[TABLE]

while the terminal constraint becomes

[TABLE]

Proof. If the tail is periodic, the infinite summation in (18) can be rewritten as follows:

[TABLE]

which can be plugged in (18) and in (19), respectively, to obtain (22) and (23).

Note that, using (11), the terminal constraint (23) can be rewritten as

[TABLE]

V-B3 Anticipative Tail

In the general case, one can use the candidate footsteps produced by the footstep generation module beyond the control horizon to conjecture a tail in $[T_{c},T_{p}]$. This is done in two phases: in the first, we generate in $[T_{c},T_{p}]$ a ZMP trajectory which belongs at all times to the admissible ZMP region defined by the footsteps $\{(\hat{x}^{F^{\prime}}_{f},\hat{y}^{F^{\prime}}_{f},\theta^{F^{\prime}}_{f}),\dots,(\hat{x}^{F}_{f},\hat{y}^{F}_{f},\theta^{F}_{f})\}$. In the second phase, we sample the time derivative of this ZMP trajectory every $\delta$ seconds.

Denote the samples obtained by the above procedure by $\dot{x}_{z,\rm ant}^{k+i}$, for $i=C,\dots,P-1$. The anticipative tail is then obtained by:

• •

setting $\dot{\tilde{x}}_{z}^{k+i}=\dot{x}_{z,\rm ant}^{k+i}$ for $i=C,\dots,P-1$;

• •

using a truncated or periodic expression for the residual part of the tail located after the preview horizon, i.e., for $\dot{\tilde{x}}_{z}^{k+i}$, $i=P,P+1,\dots\,$.

The stability constraint (18) then becomes

[TABLE]

Once a form is chosen for the residual part of the tail, this formula leads to a closed-form expression of the stability constraint which consists of a finite number of terms, and is therefore still amenable to real-time implementation. Similarly, one can use (19) to derive the corresponding expression of the terminal constraint.

In the following, and specifically in the feasibility analysis of Sect. VII-B2, we will use a particular form of anticipative tail such that (i) the ZMP trajectory in $[T_{c},T_{p}]$ is always at the center of the ZMP admissible region, and (ii) the residual part of the tail is truncated.

VI IS-MPC: Algorithm

Each iteration of our IS-MPC algorithm solves a QP problem based on the prediction model and constraints described in Sect. IV, with the addition of the stability constraint discussed in the previous section.

VI-A Formulation of the QP Problem

Collect in vectors

[TABLE]

all the MPC decision variables.

At this point, the QP problem can be formulated as:

$\left{\begin{minipage}{433.62pt}

\scriptstyle X_{f}^{k},Y_{f}^{k}\end{array}}\|\dot{X}_{z}^{k}\|^{2}+\|\dot{Y}_{z}^{k}\|^{2}+\beta\left(\|X_{f}-\hat{X}_{f}\|^{2}+\|Y_{f}-\hat{Y}_{f}\|^{2}\right)$$ \centerline{\hbox{subject to:}} \par\IEEEitemize\par\par\itemize@item@ZMP constraints~{}(\ref{eq:ZMPcon}) \par\par\itemize@item@kinematic constraints~{}(\ref{eq:footposcon}) \par\par\itemize@item@stability constraints~{}(\ref{eq:StabConstrSplitCaus}) for $x$ and $y$ \par\par\endIEEEitemize \end{minipage}\right.$ Note the following points. - $\bullet$ While the ZMP and kinematic constraints involve simultaneously the $x$ and $y$ coordinates, the stability constraints must be enforced separately along the sagittal and coronal axes. - $\bullet$ The actual expression of the stability constraint will depend on the chosen tail (truncated, periodic, anticipative). - $\bullet$ The same expression of the stability constraint is obtained by imposing for $x$ and $y$ the corresponding terminal constraint. - $\bullet$ The CoM coordinate $x_{c}$ only appears through $x_{u}$ in the stability (or terminal) constraints. ### VI-B *Generic Iteration* We now provide a sketch of the generic iteration of the IS-MPC algorithm. The input data are the sequence $(\hat{X}^{k}_{f},\hat{Y}^{k}_{f},\Theta^{k}_{f})$ of candidate footsteps, with the associated timing ${\cal T}^{k}_{s}$, as well as the high-level reference velocities used for footstep generation (these are used explicitly in the MPC if the anticipative tail is used). As initialization, one needs $x_{c}$, $\dot{x}_{c}$ and $x_{z}$ at the current sampling instant $t_{k}$. Depending on the available sensors, one may either use measured data (typically true for the CoM variables) or the current model prediction (often for the ZMP position). The IS-MPC iteration at $t_{k}$ goes as follows. 1. 1. Solve the QP problem to obtain $\dot{X}_{z}^{k},\dot{Y}_{z}^{k},X_{f}^{k},Y_{f}^{k}$. 2. 2. From the solutions, extract $\dot{x}_{z}^{k}$, $\dot{y}_{z}^{k}$, the first control samples. 3. 3. Set $\dot{x}_{z}=\dot{x}_{z}^{k}$ in ([4](#S4.E4)) and integrate from $(x_{c}^{k},\dot{x}_{c}^{k},x_{z}^{k})$ to obtain $x_{c}(t)$, $\dot{x}_{c}(t)$, $x_{z}(t)$ for $t\in[t_{k},t_{k+1}]$. Compute $y_{c}(t)$, $\dot{y}_{c}(t)$, $y_{z}(t)$ similarly. 4. 4. Define the 3D trajectory of the CoM as ${\mbox{\boldmath$p$}}^{\ast}_{c}\!=\!(x_{c},y_{c},h_{c})$ in $[t_{k},t_{k+1}]$ and return it. 5. 5. Return also the actual footstep sequence $(X^{k}_{f},Y^{k}_{f},\Theta^{k}_{f})$ with the (unmodified) timing ${\cal T}^{k}_{s}$. We recall that the footstep sequence is used by the swing foot trajectory generation module for computing ${\mbox{\boldmath$p$}}^{*}_{\it swg}$ in $[t_{k},t_{k+1}]$ (actually, only the first footstep is needed for this computation). This is then sent to the kinematic controller together with ${\mbox{\boldmath$p$}}^{\ast}_{c}$ (see Fig. [1](#S1.F1)). ## VII IS-MPC: Feasibility and Stability In this section we address the crucial issues of feasibility and stability of the proposed IS-MPC controller in itself, i.e., independently from the footstep generation module. We start by reporting some simulations that show how the introduction of the stability constraint is beneficial in guaranteeing that the CoM trajectory is always bounded with respect to the ZMP trajectory. A theoretical analysis of the feasibility of the generic IS-MPC iteration is then presented and used to obtain explicit conditions for recursive feasibility; simulations are used again to confirm that the choice of an appropriate tail is essential for achieving such property. Finally, we formally prove that internal stability of the CoM/ZMP dynamics is ensured provided that IS-MPC is recursively feasible. ### VII-A *Effect of the Stability Constraint* We present here some MATLAB simulation results of IS-MPC for the dynamically extended LIP model, in which we have set $h_{c}=0.78$ m (an appropriate value for the HRP-4 humanoid robot, see Sect. [VIII](#S8)). A sequence of evenly spaced footsteps is given with a constant step duration $T_{s}=0.5$ s, split in $T_{ss}=0.4$ s (single support) and $T_{ds}=0.1$ s (double support). The dimensions of the ZMP admissible regions are $d_{z,x}=d_{z,y}=0.04$ m and the sampling time is $\delta=0.01$ s. For simplicity, the footstep sequence given to the MPC is not modifiable (this corresponds to $\beta$ going to infinity in the QP cost function of Sect. [VI-A](#S6.SS1)); correspondingly, the kinematic constraints ([7](#S4.E7)) are not enforced. The QP problem is solved with the quadprog function, which uses an interior-point algorithm. We compare the performance of the proposed IS-MPC scheme with a standard MPC. In IS-MPC, we have used ([22](#S5.E22)) as stability constraint, which corresponds to choosing a periodic tail. In the standard MPC, the stability constraint is removed and the ZMP velocity norms in the cost function are replaced with the CoM jerk norms in order to bring the CoM into play. This corresponds to entrusting the boundedness of the CoM trajectory entirely to the cost function, in the hope that minimization of the CoM jerk will penalize diverging behaviors, as done in early MPC approaches for gait generation. Figure [9](#S7.F9) shows the performance of IS-MPC and standard MPC for $T_{c}=1.5$ s, i.e., 1.5 times the gait period. Both gaits are stable, with the IS-MPC gait more aggressively using the ZMP constraints in view of its cost function that penalizes ZMP variations. Figure [9](#S7.F9) compares the two schemes when the control horizon is reduced to $T_{c}=1$ s. The standard MPC loses stability: the resulting ZMP trajectory is always feasible but the associated CoM trajectory diverges444In particular, the divergence occurs in this case on the coronal coordinate $y_{c}$. However, it is also possible to find situations where divergence occurs on the sagittal coordinate $x_{c}$, or even on both coordinates. with respect to it, because the control horizon is too short to allow sorting out the stable behavior via jerk minimization. With IS-MPC, instead, boundedness of the CoM trajectory with respect to the ZMP trajectory is preserved in spite of the shorter control horizon thanks to the embedded stability constraint. The accompanying video shows an animation of the evolutions in Figs. [9](#S7.F9)–[9](#S7.F9). Another interesting situation is that of Fig. [10](#S7.F10), in which the CoM height is increased to $h_{c}=1.6$ m while keeping the ‘long’ control horizon $T_{c}=1.5$ s of Simulation 1. Once again, standard MPC is unstable while IS-MPC guarantees boundedness of the CoM with respect to the ZMP. Since it is $\eta^{2}=g/h_{c}$, a similar situation can be met when $g$ is decreased, as in gait generation for low-gravity environments (e.g., the moon). We emhasize that the onset of instability in standard MPC cannot be avoided by adding to the cost function a term for keeping the ZMP close to the foot center. The result of this common expedient is shown in Fig. [11](#S7.F11), in which the divergence occurs even earlier than in Fig. [9](#S7.F9), because the additional cost term has actually the effect of depenalizing the norm of the CoM jerk. Instead, IS-MPC remains stable also with this modified cost function, with the ZMP pushed well inside the constraint region. ### VII-B *Feasibility Analysis* The introduction of the stability constraint (or the corresponding terminal constraint), although beneficial in guaranteeing boundedness of the CoM trajectory, has the effect of reducing the *feasibility region*, i.e., the subset of the state space for which the QP problem of Sect. [VI-A](#S6.SS1) admits a solution. In some situations this might even lead to a loss of feasibility; i.e., the system may find itself in a state where it is impossible to find a solution satisfying all the constraints. In the following we show how to determine the feasibility region at a given time. Then we address *recursive feasibility*: this property holds if, starting from a feasible state, the MPC scheme always brings the system to a state which is still feasible. In particular, we will prove that one can achieve recursive feasibility by using the preview information conveyed by the sequence of candidate footsteps. #### VII-B1 Feasibility Regions To focus on the feasibility issue, consider the case of given footsteps ($\beta\to\infty$ in the QP cost function) with fixed orientation. Thanks to the latter assumption, and to the use of a moving ZMP constraint in double support (Fig. [6](#S4.F6)), the QP problem separates in two decoupled problems, one for the $x$ and one for the $y$ ZMP coordinate. Let us focus on the $x$ coordinate henceforth, with the understanding that every development is also valid for the $y$ coordinate. The general coupled case can be treated by using an appropriate coordinates change. Consider the $k$-th step of the IS-MPC algorithm. The QP problem is feasible at $t_{k}$ if there exists a ZMP trajectory $x_{z}(t)$ that satisfies both the ZMP constraint for $t\in[t_{k},t_{k+C}]$ [TABLE] and the stability constraint [TABLE] where: - • $x_{z}^{m}(t)$ and $x_{z}^{M}(t)$ are respectively the lower and upper bound of the ZMP admissible region at time $t$, as derived from ([6](#S4.E6)); - • $\tilde{x}_{z}$ is the ZMP position555In the rest of this section, we will for simplicity use the term ‘tail’ for both the ZMP velocity and the corresponding position. corresponding (through integration) to the chosen velocity tail; - • both the ZMP and the stability constraint have been expressed in continuous time for later convenience (in particular, eq. ([25](#S7.E25)) is obtained from ([12](#S5.E12)) by splitting the integral in two and plugging the tail in the second integral); - • the kinematic constraints ([7](#S4.E7)) are not enforced since footsteps are given. ###### **Proposition 4** *At time $t_{k}$, IS-MPC is feasible if and only if* [TABLE] *where* [TABLE] *Proof*. To show the necessity of ([26](#S7.E26)), multiply each side of the ZMP constraint ([24](#S7.E24)) by $e^{-\eta(t-t_{k})}$ and integrate over time from $t_{k}$ to $t_{k+C}$. Adding to all sides the integral term in the right-hand side of ([25](#S7.E25)), the middle side becomes exactly $x_{u}^{k}$, while the left- and right-hand sides become $x_{u}^{k,m}$ and $x_{u}^{k,M}$ as defined in the thesis. The sufficiency can be proven by showing that if ([26](#S7.E26)) holds then the ZMP trajectory [TABLE] satisfies both the ZMP constraint ([24](#S7.E24)) and the stability constraint ([25](#S7.E25)). The interpretation of ([26](#S7.E26)) is the following: it is the admissible range for $x_{u}$ at time $t_{k}$ to guarantee solvability of the QP problem associated to the current iteration of IS-MPC. Since $x_{u}$ is related to the state variables of the prediction model through ([9](#S5.E9)), eq. ([26](#S7.E26)) actually identifies the feasibility region in state space. Note that [TABLE] where we have used the fact that $x_{z}^{M}(t)-x_{z}^{m}(t)=d_{z,x}$ for all $t$, as implied by ([6](#S4.E6)). This shows that the extension $x_{u}^{k,M}-x_{u}^{k,m}$ of the admissible range for $x_{u}$ depends on the dimension $d_{z,x}$ of the ZMP admissible region, and tends to become exactly $d_{z,x}$ as the control horizon $T_{c}$ is increased. On the other hand, the midpoint of this range depends on the tail chosen for the stability constraint ([25](#S7.E25)), because $\eta\int_{t_{k+C}}^{\infty}e^{-\eta(\tau-t_{k})}\tilde{x}_{z}d\tau$ acts as an offset in both the left- and right-hand sides of ([26](#S7.E26)). Figure [12](#S7.F12) illustrates how the admissible range for $x_{u}$ moves over time, for the case of a single step and of a sequence of steps. These results were obtained with $h_{c}=0.78$ m, $d_{z,x}=0.04$ m and $T_{c}=0.5$ s. In both cases, an anticipative tail was used, with the residual part truncated; the preview horizon is $T_{p}=1$ s. Note that, as expected, the extension of the range is constant and smaller than $d_{z,x}$, and that the range itself gradually shifts toward the next ZMP admissible region as a step is approached. #### VII-B2 Recursive Feasibility We prove next that the use of an anticipative tail provides recursive feasibility under a (sufficient) condition on the preview horizon $T_{p}$. ###### **Proposition 5** *Assume that the anticipative tail is used in the stability constraint ([25](#S7.E25)). Then, IS-MPC is recursively feasible if the preview horizon $T_{p}$ is sufficiently large.* *Proof*. To establish recursive feasibility, we must show that if the IS-MPC QP problem is feasible at $t_{k}$, it will be still feasible at time $t_{k+1}$. Let us assume that ([26](#S7.E26)) holds. This implies that the ZMP constraint ([24](#S7.E24)) holds for $t\in[t_{k},t_{k+C}]$, and that the stability constraint ([25](#S7.E25)) is satisfied, i.e., [TABLE] with $\tilde{x}_{z}$ chosen as the anticipative tail at $t_{k}$. Using ([11](#S5.E11)), the value of $x_{u}$ at $t_{k+1}$ is written as [TABLE] Plugging the above expression for $x_{u}^{k}$ in this equation, simplifying, and considering that $x_{z}(t)\leq x_{z}^{M}(t)$ for $t\in[t_{k},t_{k+C}]$ we obtain [TABLE] According to Prop. [4](#Sx2.EGx9), feasibility at $t_{k+1}$ requires666From now on, we focus only on the right-hand side of the feasibility condition for compactness. In fact, imposing the left-hand side leads to the same condition ([28](#S7.E28)). [TABLE] with $\tilde{x}^{\prime}_{z}(\tau)$ in the second integral denoting the anticipative tail at $t_{k+1}$. Recursive feasibility is then guaranteed if the right-hand side of the last equation is not larger than that of the penultimate. This condition can be rewritten as [TABLE] where we have used the fact that the anticipative tails at $t_{k}$ and $t_{k+1}$ coincide over $[t_{k+C+1},t_{k+P}]$. From this we derive the equivalent inequality [TABLE] At this point, exploiting the fact (see the end of Sect. [V-B3](#S5.SS2.SSS3)) that *(i)* $x_{z}^{M}(t)-\tilde{x}_{z}(t)=d_{z,x}/2$ in the preview horizon, and *(ii)* the residual part of the anticipative tail is truncated, a lenghty but simple calculation leads to the condition [TABLE] where $(\dot{\tilde{x}}^{\prime}_{z})^{k+P}$ is the last velocity sample in the preview horizon of the anticipative tail at $t_{k+1}$. Finally, if we denote by $v^{\rm max}_{z,x}$ the upper bound on the absolute value of $(\dot{\tilde{x}}^{\prime}_{z})^{k+P}$, we can claim that a sufficient condition for recursive feasibility is [TABLE] thus concluding the proof. Note the following points. - • An upper bound $v^{\rm max}_{z,x}$ to be used in ([28](#S7.E28)) can be derived (and enforced in the tail) based on the dynamic capabilities of the specific robot or, even more directly, using the information embedded in the footstep sequence and timing. This is the same kind of reasoning that led us to postulate the existence of an upper bound $\gamma$ on $\dot{x}^{i}_{z}$ in ([5](#S4.E5)). - • Equation ([28](#S7.E28)) shows that a longer preview horizon $T_{p}$ is needed to guarantee recursive feasibility for taller and/or faster robots (larger $\eta$ and/or $v^{\rm max}_{z,x}$, respectively), or for robots with more compact feet (smaller $d_{z,x}$). - • Proposition [5](#Thmproposition5) provides only a sufficient condition, and therefore does not exclude that recursive feasibility of IS-MPC can be achieved with a smaller preview horizon, or even with a different tail. For example, in the next subsection we will describe a case (Simulation 3) in which the periodic tail represents a sufficiently accurate conjecture and therefore recursive feasibility is achieved. #### VII-B3 Recursive Feasibility — Simulations We now report some comparative MATLAB simulations aimed at showing how different choices for the tail lead to different results in terms of recursive feasibility. We use the same LIP model and parameters of Sect. [VII-A](#S7.SS1). The MPC still operates under the assumption that the footstep sequence is given and not modifiable. The control horizon $T_{c}$ is 0.8 s while the preview horizon $T_{p}$ is 1.6 s. Figure [13](#S7.F13) shows a comparison between IS-MPC using the truncated and periodic tail for a regular footstep sequence. When using the truncated tail, gait generation fails because the system reaches an unfeasible state, due to the significant mismatch between the truncated tail and the persistent ZMP velocities required by the gait. Recursive feasibility is instead achieved by using the periodic tail, which coincides with an anticipative tail for this case. Figure [14](#S7.F14) refers to a situation in which the assigned footstep sequence is irregular: two forward steps are followed by two backward steps on the same footsteps. Use of the periodic tail leads now to a loss of feasibility, as IS-MPC is wrongly conjecturing that the ZMP trajectory will keep on moving forward. The anticipative tail, which is the recommended choice for this scenario, correctly anticipates the irregularity therefore achieving recursive feasibility. The accompanying video shows an animation of the evolutions in Figs. [13](#S7.F13)–[14](#S7.F14). ### VII-C *Recursive feasibility implies stability* In Sect. [VII-B2](#S7.SS2.SSS2) it has been shown that recursive feasibility can be guaranteed by using the anticipative tail, provided that the preview horizon $T_{p}$ is sufficiently large (Proposition [5](#Thmproposition5)). Now we prove that recursive feasibility in turn implies internal stability (i.e., boundedness of the CoM trajectory with respect to the ZMP). We recall a definition first. A function $f(t)$ is said to be *of exponential order $\alpha_{0}$* if [[34](#bib.bib34)] [TABLE] According to this definition, any bounded or polynomial function is of exponential order [math], whereas $e^{at}$ is of exponential order $a$. In particular, $x_{z}$ is of exponential order 0 in IS-MPC, because it is piecewise-linear with bounded derivative, see ([5](#S4.E5)). ###### **Proposition 6** *If IS-MPC is recursively feasible, then internal stability is guaranteed.* *Proof*. We establish the result by contradiction; that is, we assume that internal stability is violated, and show that this is inconsistent with IS-MPC being recursively feasible. We focus on the dynamics along the sagittal axis $x$; an identical reasoning can be done along the coronal axis $y$. Assume that internal stability is violated, i.e., $x_{c}-x_{z}$ diverges. This implies that $x_{u}-x_{z}$ diverges, because *(i)* $x_{c}=(x_{s}+x_{u})/2$ in view of ([8](#S5.E8)–[9](#S5.E9)), and *(ii)* $x_{s}-x_{z}$ is bounded (in fact, its dynamics is BIBO stable and forced by $\dot{x}_{z}$, which is bounded). Since the dynamics of $x_{u}-x_{z}$ has a single eigenvalue $\eta$ and is also forced by $\dot{x}_{z}$, then $x_{u}-x_{z}$ will diverge with exponential order $\eta$. Finally, this implies that the feasibility condition ([26](#S7.E26)) will be violated at a future instant of time, as the upper and lower bound in the inequality are functions of the same exponential order as $x_{z}$. This contradicts the assumption that IS-MPC is recursively feasible. ### VII-D *Wrapping up* As discussed at the end of Sect. [V-A](#S5.SS1), a causal MPC can only contain an approximate version of the stability constraint, because the tail in ([17](#S5.E17)) is unknown and therefore must be conjectured. Nevertheless, Proposition [6](#Thmproposition6) states that the repeated enforcement of this constraint at each iteration of IS-MPC is effective, in the sense that *internal stability is achieved as long as the controller is recursively feasible*. In turn, the latter property is guaranteed if the anticipative tail is used with a $T_{p}$ that extends beyond $T_{c}$ enough to make the approximation sufficiently accurate (Proposition [5](#Thmproposition5), and in particular eq. ([28](#S7.E28))). At this point, the reader may wonder whether there is a requirement on the minimum control horizon $T_{c}$ in order for IS-MPC to work. The answer is that $T_{c}$ may indeed be arbitrarily small, with one caveat: as shown by eq. ([27](#S7.E27)), the feasibility region shrinks as $T_{c}$ decreases. However, once the system is initialized in this reduced region, the recursive feasibility of IS-MPC will depend only on $T_{p}$ through the sufficient condition ([28](#S7.E28)). The possibility of decreasing $T_{c}$ without affecting stability is a distinct advantage of IS-MPC with respect to schemes which need sufficiently long $T_{c}$ to work. In fact, a shorter $T_{c}$ means less computation, which may be important for real-time on-board implementation on low-cost platforms, such as the NAO of our experiments. Moreover, since the MPC needs to know the (candidate) footstep locations in the control horizon, decreasing $T_{c}$ means that footsteps are required over a smaller interval, making it possible to use short-term reactive planners. ## VIII Simulations We now report some complete gait generation results (footstep generation + IS-MPC) obtained in the V-REP simulation environment. The humanoid platform is HRP-4, a 34-dof, 1.5 m tall humanoid robot. We enabled dynamic simulation using the Newton Dynamics engine. The whole gait generation framework runs at 100 Hz ($\delta=0.01$ s). Footstep timing is determined using rule ([1](#S3.E1)) with $\bar{L}_{s}=0.12$ m, $\bar{T}_{s}=0.8$ s, $\bar{v}=0.15$ m/s as cruise parameters, and $\alpha=0.1$ m/s (as in Fig. [2](#S3.F2)). Each generated $T_{s}$ is split into $T_{ss}$ (single support) and $T_{ds}$ (double support) using a 60%-40% distribution. Candidate footsteps are generated as explained in Sect. [III-B](#S3.SS2), with $\theta_{\rm max}=\pi/8$ rad and $\ell=0.18$ m. In the IS-MPC module, which uses a control horizon $T_{c}$ of 1.6 s, we have set $h_{c}=0.78$ m. The dimensions of the ZMP admissible region are $d_{z,x}=d_{z,y}=0.04$ m, while those of the kinematically admissible region are $d_{a,x}=0.3$ m, $d_{a,y}=0.07$ m. The weight in the QP cost function is $\beta=10^{4}$. The qpOASES library was used to solve the QP, here as well as in the experiments to be presented in the next section. Figure [16](#S8.F16) shows a stroboscopic view of the first simulation (see the accompanying video for a clip). The robot is commanded a sagittal reference velocity $v_{x}$ of $0.1$ m/s which is then abruptly increased to $0.3$ m/s. The preview horizon is $T_{p}=3.2$ s and the anticipative tail is used. The generated CoM and ZMP trajectories together with the sagittal CoM velocity are shown in Fig. [16](#S8.F16). As expected, the higher commanded velocity is realized by increasing both step length and frequency. In the second simulation, shown in Fig. [18](#S8.F18) and the accompanying video, the reference velocities are aimed at producing a cusp trajectory. In particular, initially we have $v_{x}=0.2$ m/s and $\omega=0.2$ rad/s; after a quarter turn we change $v_{x}$ to $-0.2$ m/s; after another quarter turn, $\omega$ is zeroed. As before, $T_{p}$ is 3.2 s and the anticipative tail is used for the stability constraint. Figure [18](#S8.F18) shows plots of the generated ZMP and CoM trajectories, together with the sagittal CoM velocity. Video clips of the complete simulations are shown in the accompanying video. ## IX Experiments Experimental validation of the proposed method for gait generation was performed on two platforms, i.e., the NAO and HRP-4 humanoid robots. NAO is a 23-dof, 58 cm tall humanoid equipped with a single-core Intel Atom running at 1.6 GHz. Our method, implemented as a custom module in the B-Human RoboCup SPL team framework [[35](#bib.bib35)], runs in real-time on the on-board CPU at a control frequency of 100 Hz ($\delta=0.01$ s). Footstep timing is determined using rule ([1](#S3.E1)) with $\bar{L}_{s}=0.075$ m, $\bar{T}_{s}=0.5$ s, $\bar{v}=0.15$ m/s as cruise parameters, and $\alpha=0.1$ m/s (as in Fig. [2](#S3.F2)). Candidate footsteps are generated as explained in Sect. [III-B](#S3.SS2), with $\theta_{\rm max}=\pi/8$ rad and $\ell=0.1$ m. In the IS-MPC module we have set $T_{c}=1.0$ s and $h_{c}=0.23$ m. The dimensions of the ZMP admissible region are $d_{z,x}=d_{z,y}=0.03$ m, while those of the kinematically admissible region are $d_{a,x}=0.1$ m, $d_{a,y}=0.05$ m. The weight in the QP cost function is $\beta=10^{4}$. The anticipative tail is used with a preview horizon $T_{p}=2.0$ s. The software architecture of HRP-4 requires control commands to be generated at a frequency of 200 Hz ($\delta=0.005$ s). Gait generation runs on an external laptop PC and joint motion commands are sent to the robot via Ethernet using TCP/IP. The parameters are the same of the V-REP simulations in the previous section, including $T_{c}=1.6$ s, with the exception of $d_{z,x}$, $d_{z,y}$ that are reduced to 0.01 m for increased safety. The anticipative tail is used in the stability constraint. Before presenting complete locomotion experiments, we report in Fig. [19](#S9.F19) some data from a typical forward gait of HRP-4. In particular, the plot shows the nominal ZMP trajectory, as generated by IS-MPC, together with the ZMP measurements reconstructed from the force-torque sensors at the robot ankles [[36](#bib.bib36)]. Note how the restriction of the ZMP admissible region is effective, in the sense that while the measured ZMP violates the constraints, it stays well within the original ZMP admissible region used in the simulation. The accompanying video shows two successful experiments for each robot. In the first, the robots are required to perform a forward-backward motion as shown in Fig. [20](#S9.F20). The reference velocities are $v_{x}=\pm 0.15$ m/s for the NAO and $v_{x}=\pm 0.2$ m/s for the HRP-4. In the second experiment, which is shown in Fig. [21](#S9.F21), the robots are given reference velocities aimed at performing an L-shaped motion. In particular, we have $v_{x}=0.15$ m/s followed by $v_{y}=0.05$ m/s for the NAO, and $v_{x}=0.2$ m/s followed by $v_{y}=0.2$ for the HRP-4. ## X Conclusions We have presented a complete MPC framework (IS-MPC) for generating intrinsically stable humanoid gaits that realize high-level cartesian velocity commands. We have discussed various versions of the newly introduced stability constraint, which may be used depending on the available quantity of preview information on the reference motion. It has also been shown how the different stability constraints can be interpreted as terminal constraints, some of which are new in the literature. A detailed study of the feasibility of the generic MPC iteration has been developed and used to derive conditions under which recursive feasibility can be guaranteed. Comparative simulations have been presented to illustrate the effect of the different tails on the resulting gait, and have confirmed that incorporating preview information in the tail is essential to preserve feasibility. Finally, it has been shown that recursive feasibility of IS-MPC implies internal stability of the CoM/ZMP dynamics. Experimental results obtained with an on-board NAO implementation have proved that the proposed algorithm is viable even in the presence of limited computing capabilities. Additional successful experiments were carried out on the full-sized humanoid HRP-4. The advantages of IS-MPC can be summarized as follows: - • It includes an explicit stability constraint which, through the choice of the tail, can be declined on the basis of the preview information so as to accommodate different gaits to be executed. - • It is guaranteed to be recursively feasible if the anticipative tail is used and the preview horizon is sufficiently long (Proposition [5](#Thmproposition5)). This clarifies the role and the amount of the required preview information, in contrast with most literature where such an analysis is missing. - • It is the first MPC-based gait generation with an explicit proof of internal stability, which is shown to be a direct consequence of recursive feasibility (Proposition [6](#Thmproposition6)). - • It is general enough to be applicable to different humanoids (such as NAO and HRP-4) without significant adaptation. We are currently working on several extensions of the proposed approach, such as: - • developing a robust version of the proposed IS-MPC scheme that can withstand unmodeled dynamics and disturbances [[37](#bib.bib37)]; - • extending our approach to the 2.5D case (piecewise-horizontal ground, such as stairs or flat step stones), for which we have presented a preliminary version of IS-MPC in [[38](#bib.bib38)] and a footstep planner in [[39](#bib.bib39)]; - • investigating the use of learning techniques in conjunction with MPC in order to improve performance. ## Acknowledgments The authors would like to thank Dr. Abderrahmane Kheddar of CNRS for hosting Daniele De Simone at LIRMM in Montpellier and allowing him to perform experiments on the HRP-4 humanoid robot. ## Appendix We collect here some useful properties used in the proofs of the various propositions. For compactness, we use the following notation [TABLE] ###### **Property 1** *Linearity in $x_{z}(t)$:* [TABLE] ###### **Property 2** *If $x_{z}(t)=\delta_{-1}(t-t_{k})$, we get* [TABLE] ###### **Property 3** *If $x_{z}(t)=\rho(t-t_{k})$, we get* [TABLE] Properties 1-3 are easily derived by explicit computation of the integral in ([A.1](#Sx2.E1)). ###### **Property 4** *If $x_{z}(t)=0$ for $t<t_{k}$, we get* [TABLE] *Proof*. [TABLE] Property 4 (*time shifting)* shows how the stability condition for the time-shifted function $x_{z}(t-T)$ can be written in terms of the stability condition for the original function $x_{z}(t)$.