MPC for Humanoid Gait Generation: Stability and Feasibility

01/24/2019
by   Nicola Scianca, et al.
0

We present a novel 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 MPC 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. The stability and feasibility of the proposed method are analyzed in detail: in particular, a theoretical analysis of the feasibility of the generic MPC iteration is developed and used to obtain sufficient conditions for recursive feasibility and stability. Simulation and experimental results on the NAO and the HRP-4 humanoids are presented to illustrate the performance of the proposed method.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 13

page 14

02/08/2021

Fast Online Planning for Bipedal Locomotion via Centroidal Model Predictive Gait Synthesis

The planning of whole-body motion and step time for bipedal locomotion i...
10/01/2021

RLO-MPC: Robust Learning-Based Output Feedback MPC for Improving the Performance of Uncertain Systems in Iterative Tasks

In this work we address the problem of performing a repetitive task when...
02/21/2020

Neural Lyapunov Model Predictive Control

This paper presents Neural Lyapunov MPC, an algorithm to alternately tra...
10/09/2020

Robust walking based on MPC with viability-based feasibility guarantees

Model predictive control (MPC) has shown great success for controlling c...
01/14/2020

Parameterized and GPU-Parallelized Real-Time Model Predictive Control for High Degree of Freedom Robots

This work presents and evaluates a novel input parameterization method w...
03/26/2021

Imitation Learning from MPC for Quadrupedal Multi-Gait Control

We present a learning algorithm for training a single policy that imitat...
11/23/2020

Risk-Sensitive Motion Planning using Entropic Value-at-Risk

We consider the problem of risk-sensitive motion planning in the presenc...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Most gait generation approaches for humanoids enforce 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; in fact, this guarantees that balance is maintained during locomotion. As a consequence, 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: a sequence of footsteps, and thus a ZMP trajectory interpolating these footsteps, is assigned in advance, and 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 tracking laws, the most successful example in this context being the LQ preview controller of [5].

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 allowing to include 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. As discussed in [14]

, there are two main avenues 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 possibility is to enforce a terminal state constraint (i.e., a constraint on the state at the end of the control horizon), whose role in guaranteeing closed-loop stability under certain conditions is known in the MPC literature [16]. Terminal constraints were used for humanoid balancing in [17] and for LIP-based gait generation in [18, 19], respectively under given footsteps and automatically placed footsteps. In both cases, the unstable component of the LIP was required 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 [20]). A closely related result for cyclic gaits has been presented in [21] and then expanded in [22].

However, while in a regulation problem it is natural to encode the assigned set-point into a terminal constraint, it should be considered that gait generation for humanoid is more similar to a tracking problem. In this context, it is not even clear what is the appropriate terminal constraint to be imposed. Indeed, to the best of our knowledge, no MPC-based gait generation method exists in the literature for which a full analysis of the stability issue has been performed in connection with the choice of the terminal constraint.

Besides, 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 [23]. 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 crucial issue has seldom been explored for MPC-based gait generation, with the notable exceptions of [24, 25].

In [26] 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, was conjectured.

In this paper, 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 [26]:

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

  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. we analyze in detail the impact of the additional constraint on stability and feasibility, and show analytically how, under certain assumptions, it is possibile to guarantee recursive feasibility of the IS-MPC scheme;

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

The recursive feasibility result is particularly important because it indicates 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 — hence, the appropriate terminal constraint — depends upon the future characteristics of the commanded motion; in this sense, to guarantee recursive feasibility one should choose the anticipative tail, which makes the most use of the available preview information on such motion. Since recursive feasibility implies that the generated gait satisfies all problem constraints at any time, CoM stability is automatically ensured in this case.

Fig. 1: A block scheme of the proposed MPC-based framework for gait generation.

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 and stability. 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 (, ) and steering () 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 . Throughout the paper, it is assumed that the reference velocities , , are made available for gait generation with a preview horizon . At the generic instant , the high-level references velocities over

are then sent to the footstep generation module, which uses Quadratic Programming (QP) to generate candidate footsteps over the same interval. In particular, vectors

, 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 collects the footstep orientations, which will not be modified. The footstep generation module also generates the timing 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 , and the trajectory of the humanoid CoM over the control horizon . It is assumed that , i.e., . 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 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 tracking111We 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 , the candidate footstep generation module receives in input the high-level reference velocities over the preview horizon, i.e., from to (see Fig. 1). In output, it provides the candidate footstep sequence over the same interval with the associated timing . In particular, these quantities are defined222To keep a light notation, the symbol identifying the current sampling instant is used for the sequence vectors but not for their individual elements. as

and

where is the pose of the -th footstep in the preview horizon and is the duration of the step between the -th and the -th footstep, taken from the start of the single support phase to the next. Since the duration of steps is variable, the number of footsteps falling within the preview horizon may change at each .

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 of each step is related to the magnitude of the reference Cartesian velocity at the beginning of that step.

Assume that a triplet of cruise parameters has been chosen, where is a central value of and , are the corresponding values of the step duration and length, respectively, with . 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 should reflect on a change in both and . In formulas:

with . One easily obtains

(1)

Figure 2 shows the resulting rule for determining as a function of in comparison to other possible rules. For illustration, we have set  m/s,  s,  m and  m/s. It is confirmed that an increase of , for example, corresponds to both a decrease of and an increase in .

Fig. 2: The proposed rule for determining the step duration as a function of the magnitude of the reference Cartesian velocity. For comparison, also shown are the rules yielding constant step duration and constant step length.

Note that the reference angular velocity 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 is the same. For a purely rotational motion () where the humanoid is only required to rotate on the spot, the above rule would yield the maximum value of .

In practice, equation (1) is iterated along the preview horizon in order to obtain the footstep timestamps:

with equal to the timestamp of the last footstep before . Iterations must be stopped as soon as , discarding the last generated timestamp since it will be outside the preview horizon. The resulting step timing will be , with .

Iii-B Candidate Footstep Placement

Once the timing of the steps in the preview horizon 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 :

(2)

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.

Fig. 3: Candidate footsteps generated by the proposed method for different high-level reference velocities corresponding to a circular walk (top), L-walk (center), diagonal walk (bottom). The paths in black are obtained by integrating model (2) under the reference velocities. Footstep in magenta and cyan refer respectively to the left and right foot.

The idea is to distribute the candidate footsteps around the reference trajectory in accordance to the timing 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

Here, is the maximum allowed rotation between two consecutive footsteps. The second QP problem is

Here, is the known position of the support foot at and , are given by

where , are the rotation matrices associated respectively to (the orientation of the template robot at any given time ) and to the footstep orientation , and 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 with the associated timing is sent to the IS-MPC stage. The final footstep positions will be determined by the latter while the footstep orientations and timing 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 () 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 stability constraint guaranteeing that the CoM trajectory generated by our MPC scheme will be bounded. 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.

Fig. 4: The LIP in the direction.

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 -axis (sagittal) aligned with the support foot, and the -axis (coronal) orthogonal to the -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 axis (see Fig. 4), and let and be respectively the coordinate of the CoM and the ZMP. The LIP dynamics is

(3)

where and is the constant height of the CoM. In this model, the ZMP position represents the input, whereas the CoM position is the output.

To obtain smoother trajectories, we take the ZMP velocity as the actual control input. This leads to the following third-order prediction model (LIP dynamic extension)

(4)

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

The ZMP position will then be

(5)

where we have used the notation .

The generic iteration of IS-MPC plans over the control horizon, i.e., from to . Since , a subset of the candidate footsteps produced by the footstep generation module fall inside the control horizon; denote their number by . The MPC iteration will then generate:

  • the control variables, i.e., the input values , , for ;

  • the other decision variables, i.e., the actual footstep positions , for .

  • as a byproduct, the output history , , for 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).

Fig. 5: At time , the control variables determined by IS-MPC are the piecewise-constant ZMP velocities over the control horizon. The ZMP velocities after the control horizon are instead conjectured in order to build the tail (see Sect. V-B). Also shown are the footstep timestamps placed by the footstep generation module in the preview horizon; of them fall in the control horizon.

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 equilibrium 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 -th footstep, the admissible region for the ZMP is the interior of the footstep, which can be approximated as a rectangle of dimensions , centered at , and oriented as . Using the fact that the ZMP profile is piecewise-linear as entailed by (5), the constraint can be expressed as333For 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.:

(6)

If the above sampled-time ZMP constraint is satisfied, then the original continuous-time constraint is also satisfied thanks to the linearity of within each sampling interval. Constraint (6), complete with the corresponding left-hand side, must be imposed throughout the control horizon () and for all the associated footsteps ().

Note that constraint (6) is nonlinear in the footstep orientation , which however is not a decision variable, being simply inherited from the footstep generation module. The constraint is instead linear in , , 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 [27]. 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 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.

Fig. 6: The ZMP moving constraint in double support.

Iv-C Kinematic Constraints

Fig. 7: The kinematic constraint on footstep placement.

The second type of constraint is introduced to ensure that all steps are compatible with the robot kinematic limits. Consider the -th step in , with the support foot centered at and oriented as . The admissible region for placing the footstep is defined as a rectangle having the same orientation and whose center is displaced from the support foot center by a distance in the coronal direction (see Fig. 7). Denoting by and the dimensions of the kinematically admissible region, the constraint can be written as

(7)

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 ().

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 stability) 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

(8)
(9)

the LIP part of system (3) is decomposed into a stable and an unstable subsystem. The unstable component is also known as divergent component of motion [21] or capture point [28] and its dynamics is expressed as

(10)

In spite of the LIP instability, for any ZMP trajectory in input there exists a special initialization of such that the resulting CoM trajectory in output is bounded with respect to the input [29]. In the MPC context, the initial condition in is , and the special initialization is expressed as

(11)

Note that this particular initialization depends on the future values of the ZMP. In the following, we refer to (11) as the stability condition.

The stability condition, which involves at the initial instant of the control horizon, can be propagated to its final instant by integrating (10) from in (11):

(12)

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

Proposition 1

For the piecewise-linear in (5), condition (11) becomes

(13)

while (12) takes the form

(14)

Proof. Rewrite eq. (5) as

(15)

where denotes the unit ramp and the unit step. Using Properties 1, 4 and 3 given in the Appendix, we get

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

To prove (14), rewrite (15) as

The contribution of the first two terms of to the integral in (12) is . 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 (14). This completes the proof.  

In (13), we should logically separate the values of within the control horizon, i.e. the control variables for , from the remaining values, i.e., from on. The infinite summation is then split in two parts and (13) can be rearranged as444Constraint (16) can be written as a function of the actual state variables of our prediction model (, and ) using the coordinate transformation (9). The same is true for all subsequent forms of the stability constraint as well as of the terminal constraint.

(16)

Observe the inversion between (13), which expresses the stable initialization at for a given , and (16), which constrains the control variables so that the associated stable initialization matches the current state at . Therefore, we will refer to (16) as the stability constraint.

The control variables do not appear in condition (14), which involves only the value of the state variable 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 , , 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 , , , we can make an informed conjecture at about these ZMP velocities, which we will denote by , and refer to collectively as the tail in the following. Correspondingly, the stability constraint (16) assumes the form

(17)

while the terminal constraint (14) becomes

(18)

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)

The stability constraint becomes

(19)

while the terminal constraint becomes

(20)

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

Interestingly, the terminal constraint (20) is equivalent to the capturability constraint, originally introduced by [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 , 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)

The stability constraint becomes

(21)

while the terminal constraint becomes

(22)

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

which can be plugged in (17) and in (18), respectively, to obtain (21) and (22).  

Note that, using (10), the terminal constraint (22) can be rewritten as

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 . This is done by first generating in that interval a ZMP trajectory contained at all times in the moving admissibile region associated to the footsteps sequence , and then sampling its time derivative every seconds.

Denote the samples obtained by this procedure by , for . The anticipative tail is then obtained by:

  • setting for ;

  • using a truncated or periodic expression for the residual part of the tail located after the preview horizon, i.e., for , .

The stability constraint (17) then becomes

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 (18) 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 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

all the MPC decision variables.

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


Note the following points.

  • While the ZMP and kinematic constraints involve simultaneously the and coordinates, the stability constraints must be enforced separately along the sagittal and coronal axes.

  • The actual expression of the stability constraint will depend on the chosen tail (truncated, periodic, anticipative).

  • As an alternative to the stability constraints, one may impose for and the corresponding terminal constraints (18).

  • The CoM coordinate only appears through 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 of candidate footsteps, with the associated timing , 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 , and at the current sampling instant . 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 goes as follows.

  1. Solve the QP problem to obtain .

  2. From the solutions, extract , , the first control samples.

  3. Set in (4) and integrate from to obtain , , for . Compute , , similarly.

  4. Define the 3D trajectory of the CoM as in and return it.

  5. Return also the actual footstep sequence with the (unmodified) timing .

We recall that the footstep sequence is used by the swing foot trajectory generation module for computing in (actually, only the first footstep is needed for this computation). This is then sent to the kinematic controller together with (see Fig. 1).

Vii IS-MPC: Stability and Feasibility

In this section we address the crucial issues of stability and feasibility 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. Finally, we use simulations again to confirm that the choice of an appropriate tail is essential for achieving such property.

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  m (an appropriate value for the HRP-4 humanoid robot, see Sect. VIII). A sequence of evenly spaced footsteps is given with a constant step duration  s, split in  s (single support) and  s (double support). The dimensions of the ZMP admissible regions are  m and the sampling time is  s. For simplicity, the footstep sequence given to the MPC is not modifiable (this corresponds to going to infinity in the QP cost function of Sect. VI-A); correspondingly, the kinematic constraints (7) 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 (21) 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.

Fig. 8: Simulation 1: Gaits generated by IS-MPC (top) and standard MPC (bottom) for  s. The given footstep sequence is shown in magenta. Note the larger region corresponding to the initial double support.
Fig. 9: Simulation 2: Gaits generated by IS-MPC (top) and standard MPC (bottom) for  s. Note the instability in the standard MPC solution.
Fig. 8: Simulation 1: Gaits generated by IS-MPC (top) and standard MPC (bottom) for  s. The given footstep sequence is shown in magenta. Note the larger region corresponding to the initial double support.

Figure 9 shows the performance of IS-MPC and standard MPC for  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 compares the two schemes when the control horizon is reduced to  s. The standard MPC loses stability: the resulting ZMP trajectory is always feasible but the associated CoM trajectory diverges, 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. 99.

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 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 ( 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), the QP problem separates in two decoupled problems, one for the and one for the ZMP coordinate. Let us focus on the coordinate henceforth555The general coupled case can be treated by using an appropriate coordinates change., with the understanding that every development is also valid for the coordinate.

Consider the -th step of the IS-MPC algorithm. The QP problem is feasible at if there exists a ZMP trajectory that for satisfies both the ZMP constraint

(23)

and the stability constraint

(24)

where:

  • and are respectively the lower and upper bound of the ZMP admissible region at time , as derived from (6);

  • is the ZMP position666In 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. (24) is obtained from (11) by splitting the integral in two and plugging the tail in the second integral);

  • the kinematic constraints (7) are not enforced since footsteps are given.

Proposition 4

At time , IS-MPC is feasible if and only if

(25)

where

Proof. To show the necessity of (25), multiply each side of the ZMP constraint (23) by and integrate over time from to . Adding to all sides the integral term in the right-hand side of (24), the middle side becomes exactly , while the left- and right-hand sides become and as defined in the thesis.

The sufficiency can be proven by showing that if (25) holds then the ZMP trajectory

satisfies both the ZMP constraint (23) and the stability constraint (24).  

The interpretation of (25) is the following: it is the admissible range for at time to guarantee solvability of the QP problem associated to the current iteration of IS-MPC. Since is related to the state variables of the prediction model through (9), eq. (25) actually identifies the feasibility region in state space.

Note that

where we have used the fact that for all , as implied by (6). This shows that the extension of the admissible range for only depends on the corresponding dimension of the ZMP admissible region, to which it tends as the control horizon is increased. On the other hand, the midpoint of this range depends on the tail chosen for the stability constraint (24), because acts as an offset in both the left- and right-hand sides of (25).

Figure 10 illustrates how the admissible range for moves over time, for the case of a single step and of a sequence of steps. These results were obtained with  m,  m and  s. In both cases, an anticipative tail was used, with the residual part truncated; the preview horizon is  s. Note that, as expected, the extension of the range is constant and smaller than , and that the range itself gradually shifts toward the next ZMP admissible region as a step is approached.

Fig. 10: Feasibility regions. Top: The robot is taking a single step. Bottom: The robot is taking a sequence of steps. The anticipative tail is used in both cases.

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 .

Proposition 5

Assume that the anticipative tail is used in the stability constraint (24). Then, IS-MPC is recursively feasible if the preview horizon is sufficiently large.

Proof. To establish recursive feasibility, we must show that if the IS-MPC QP problem is feasible at , it will be still feasible at time .

Let us assume that (25) holds. This implies that the ZMP constraint (23) holds for , and that the stability constraint (24) is satisfied, i.e.,

with chosen as the anticipative tail at .

Using (10), the value of at is written as

Plugging the above expression for in this equation, simplifying, and considering that for we obtain

According to Prop. 4, feasibility at requires777From now on, we focus only on the right-hand side of the feasibility condition for compactness.

with in the second integral denoting the anticipative tail at . 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