Optimization-Based Reference Generator for Nonlinear Model Predictive Control of Legged Robots

Abstract

Model predictive control (MPC) approaches are widely used in robotics, because they guarantee feasibility and allow the computation of updated trajectories while the robot is moving. They generally require heuristic references for the tracking terms and proper tuning of the parameters of the cost function in order to obtain good performance. For instance, when a legged robot has to react to disturbances from the environment (e.g., to recover after a push) or track a specific goal with statically unstable gaits, the effectiveness of the algorithm can degrade. In this work, we propose a novel optimization-based reference generator which exploits a linear inverted pendulum (LIP) model to compute reference trajectories for the center of mass while taking into account the possible underactuation of a gait (e.g., in a trot). The obtained trajectories are used as references for the cost function of the nonlinear MPC presented in our previous work. We also present a formulation that ensures guarantees on the response time to reach a goal without the need to tune the weights of the cost terms. In addition, footholds are corrected by using the optimized reference to drive the robot toward the goal. We demonstrate the effectiveness of our approach both in simulations and experiments in different scenarios with the Aliengo robot.

1. Introduction

1.1. Related Work

Legged robots are becoming popular nowadays, thanks to their ability to operate on irregular and complex terrains. The challenge is represented by the design of a proper control strategy that allows the robots to execute their tasks. The methods developed earliest involve the use of heuristic approaches, e.g., [1]. They demonstrated good performance and succeeded in hardware experiments, but are tailored to specific motions and scenarios. More recently, trajectory optimization (TO) techniques [2,3,4] were introduced, because constraints and cost functions can ensure dynamic feasibility and desired performance. In particular, Model Predictive Control (MPC) approaches can compensate for uncertainties and changes in the environment, by computing a new trajectory online, while the robot moves. In our previous work [5], we presented an Nonlinear Model Predictive Control (NMPC) formulation, which runs at 25 $\mathrm{Hz}$ and allows the Hydraulically actuated Quadruped (HyQ) robot [6] to perform omnidirectional motions, detect a pallet, and step on it with improved leg mobility. Minniti et al. [7] integrated control Lyapunov functions into their MPC to guarantee stability when the robot has to interact with unknown objects. Hong et al. [8] presented an NMPC implementation with a set of different gaits, and Amatucci et al. [9] also exploited a Monte Carlo tree search to optimize for the contact schedule. The last two works, however, have only been tested in simulation.

The importance of the MPC approaches is well evident in the case of disturbances, because they allow the robot to comply with external pushes (e.g., in [10]) thanks to high-frequency replanning. One of the shortcomings of a standard MPC implementation is related to the fact that the robot becomes “transparent” to external pushes, i.e., at each sample, the new trajectory starts from the actual position. Therefore an additional effort is required to track a specific reference Cartesian position or to return, after a push, to the original one. A common practice is to have user-defined values as a reference for the MPC [11], but in these approaches the user would have to manually change the reference velocity in order to compensate for the deviation caused by the push.

Another possible solution would be to add in parallel to the MPC a simple Cartesian Proportional-Derivative (PD) control action to attract the Center of Mass (CoM) to the reference position [12]. However, this control strategy has some notable limitations, in particular: (1) the added wrench does not consider the wrench produced by the MPC layer (hence, the two controllers will fight against each other with the risk of violating feasibility); (2) the PD is not aware of the hybrid dynamics of the legged robot, i.e., the intermittent contacts and the (possible) underactuation. Moreover, footholds play an important role in the robot agility, because their coherence with the CoM can improve the stability when the robot has to “react" to an external disturbance; and (3) PD approaches cannot change feet trajectories, which is crucial to deal with lateral pushes. For example, Barasuol et al. [13] presented a push recovery module that modifies the footholds to counteract the disturbance and track the position with the PD. As explained, the drawback of this approach is that the computed feet locations do no guarantee that the resulting wrench can be generated by the robot.

Another possibility to track a fixed goal is adding it in the cost function. This would solve the issue number 1 of the PD controller, because the constant position is embedded in the cost function of the MPC; hence, the “conflict” is dealt with at the cost level resulting in feasible contact forces. However, this solution suffers from the fact that it is only able to apply a limited resistive force before the legs lose their control authority (e.g., when the CoM/Zero Moment Point (ZMP) goes out of the support polygon), and therefore is of restricted applicability. Moreover, in this case, footholds are not generally designed to be consistent with the resulting CoM trajectory, because they are computed with simple heuristics. Cebe et al. [14] optimize for CoM and foothold positions, but they can replan only at the touchdown moments, due to the high computational time required.

In addition, the MPCs usually employ references for contact forces that come from crude heuristic computations (i.e., dividing the gravity weight along the legs based on static conditions). These values (i.e., purely vertical forces) are often not feasible for the motion of the base. Undoubtedly, accurately tuning the weights for the different cost terms is a tedious task [15] and having more physically meaningful references has been shown to be a preferable solution [16]. To address this problem, Bjelonic et al. [17] use the result of an offline TO as cost terms for their MPC.

In this work, we propose a novel optimization-based reference generator layer that supplies a NMPC with references for CoM and Ground Reaction Forces (GRFs) that are suitable to the task of the robot. The novelty of our approach is that the reference trajectories are computed online (as opposed to the method used in [17]), solving a simplified optimization problem that takes into account the future robot behaviour, and the intermittent contact schedule to generate the references. It is worth highlighting the difference of our approach with the reference governors [18,19,20]. In fact, a governor filters the set-points (i.e., it takes a reference signal vector and changes it to another reference signal of the same type) just to enforce state and input constraints. In the proposed approach, instead, we are interested in considering the intrinsic underactuation of a trot (chosen as a template gait) when only two feet are in contact with the ground. Moreover, we impose additional features, such as a desired interval in which the robot has to reach a fixed goal or recover its position automatically from big pushes. Another advantage of our algorithm is that it is able to affect CoM trajectories and footholds at the same time, even though the latter are not directly included among the optimization variables.

A possible drawback of such a cascade optimization setting is represented by the computational effort, which can result in a reduction of the NMPC frequency. Aiming to find a compromise between accuracy and computational efficiency, we employ models of different complexity by using the simplest Linear Inverted Pendulum (LIP) [21] in the reference generator and a more complex one (the Single Rigid Body Dynamics (SRBD) [22]) for the NMPC. The latter, in fact, computes the state trajectories and GRFs that are then sent to the robot, so it requires more accuracy. Thanks to the optimal references, we can avoid using a full dynamics model as in [23].

1.2. Proposed Approach and Contribution

In this work we introduce our optimization-based reference generator which endows the cost function of an NMPC [5] with physically informed reference trajectories to be tracked. We integrated this module with our previous works (see Figure 1).

To summarize, the contributions of the paper are:

• the presentation of a novel reference generator that drives the robot to accomplish a task (optionally in a user-defined time interval), taking into account the underactuation of statically unstable gaits, like the trot. Footholds are heuristically computed to be coherent with the CoM motion, and optimized GRFs are obtained in order to follow those trajectories. The formulation is lightweight enough to maintain the replanning frequency of 25 $\mathrm{Hz}$ of the NMPC;
• simulations and experiments to demonstrate the effectiveness of the proposed approach in three different scenarios: (a) straight motion, (b) fixed lateral goal, and (c) recovery after a push. We also compared in simulation our algorithm with a state-of-the-art approach (NMPC + PD action) for the scenario (c); and
• as an additional minor contribution, we demonstrate the generality of the approach, showing it was able to deal with different dynamic gaits, i.e., trot and pace.

1.3. Outline

The paper is organized as follows: Section 2 gives an overview of our planning framework, highlighting the main features of the reference generator. Section 3 describes the optimization problem with the LIP model and how it is used to compute CoM position velocity and GRFs references. Simulations and experiments with our Aliengo [25] robot are presented in Section 4. Finally, we draw the conclusions in Section 5.

2. Locomotion Framework Description

Figure 2 gives an overview of our entire locomotion framework. The user decides the leg sequence and the values of both linear ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}\in {\mathbb{R}}^2$ CoM reference velocity and heading ${\omega }_{\mathrm{z}}^{\mathrm{usr}}\in \mathbb{R}$ reference velocity, i.e., the velocities that the robot should follow. The velocities can be changed during the motion and the NMPC will immediately react accordingly. In this work, we use two modules already presented in [5]: the gait scheduler and the robocentric stepping (also used in [26]).

Given the user-defined leg sequence, the gait scheduler returns the gait status (either swing or stance) ${\mathbf{\delta }}_{i,k}\in \mathbb{R}$ of each leg $\mathrm{i}$ and for each time instant $\mathrm{k}$ in the reference horizon $N_{\mathrm{g}}$, reconciliating it with the real condition of the robot, e.g., early or late touchdown. The reference horizon $N_{\mathrm{g}}$ corresponds to the maximum response time $T_{\mathrm{f}}$ of the reference generator. Note that $N_{\mathrm{g}}$ can be different from the horizon N used in the NMPC, with $N_{\mathrm{g}}\ge N$. We use the symbol $\mathbf{\delta }\in {\mathbb{R}}^{4\times N_{\mathrm{g}}}$ to refer to the entire sequence of gait status. The robocentric stepping module, on the other hand, has the important task of computing footholds for each leg over the entire horizon. We use the symbol ${\mathbf{p}}_{\mathrm{f}}\in {\mathbb{R}}^{12\times N_{\mathrm{g}}}$ to denote these quantities for the whole horizon.

The key idea of robocentric stepping is that the touchdown points are computed with respect to the hip position and they are offset, with respect to that, depending on the CoM reference velocity. Applying the robocentric stepping with the optimized reference velocities allows us to obtain the coherence between CoM and footholds. Variables ${\mathbf{p}}_{\mathrm{f}}$ and $\mathbf{\delta }$ are also used as parameters in the SRBD model of the NMPC. If the error between the goal and average CoM position is lower than a threshold (see Section 2.1), the reference generator does not perform optimizations and user-defined velocities (heuristic references) are used as references also for the NMPC. We refer to this condition as the heuristic reference generator.

Instead, when the reference generator is in the optimize state, the sequences ${\mathbf{p}}_{\mathrm{f}}$ and $\mathbf{\delta }$ are the input parameters to a first-stage optimization that employs the LIP model (Section 3.1). Here we compute the optimal X-Y CoM trajectory ${\mathbf{p}}_{\mathrm{c}}^{\mathrm{g}},{\mathbf{v}}_{\mathrm{c}}^{\mathrm{g}}$ to reach the goal ${\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}$, where dynamic stability is satisfied (i.e., ZMP always inside the support polygon). However, because the footholds are computed the first time with the simple heuristic approach, we need to recompute them by using the optimized velocity. This will result in a new set of footholds that will be used as inputs for a second optimization. We iterate this until a stop condition is reached, e.g., the maximum number of iterations or difference between two consecutive solutions below the defined threshold. As a safety check, if, instead, the solver is not able to converge to a feasible solution, the reference generator is set back to the heuristic status and ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}$/gravity compensation are used as references.

The CoM velocity trajectory ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{g}}$ computed by the LIP model optimization is then used as velocity reference for the NMPC. As described in [5], reference CoM position, roll and pitch are not tracked, and the reference for the yaw is obtained by integrating the user-defined yaw rate ${\omega }_{\mathrm{z}}^{\mathrm{usr}}$. We use the variable ${\mathsf{\Theta }}^{\mathrm{ref}}\in {\mathbb{R}}^3$ to indicate the references for roll, pitch, and yaw. A Quadratic Program (QP)-based mapping (Section 3.2) computes the references for the GRFs ${\mathbf{u}}^{\mathrm{ref}}\in {\mathbb{R}}^{12\times N}$.

Finally, the output of the NMPC are the CoM trajectories (position, orientation, linear and angular velocities) ${\mathbf{x}}_{\mathrm{c}}^{\mathrm{des}}\in {\mathbb{R}}^{12\times (N+1)}$ (as explained in [5] we parameterized the orientation with Euler angles, hence, the state dimension is 12) and GRFs ${\mathbf{u}}^{\mathrm{des}}\in {\mathbb{R}}^{12\times N}$ that will be sent to the controller block. The controller is composed of a 250 $\mathrm{Hz}$ Whole-Body Control (WBC) [24] and a 1 $\mathrm{kHz}$ PD-joint controller. They generate the torque references ${\mathbf{\tau }}^{\mathrm{des}}$ for the low-level joint controller of the robot. The state estimator module [27] provides the actual values ${\mathbf{x}}_{\mathrm{c}}^{\mathrm{act}}$ of the robot at a frequency of 500 $\mathrm{Hz}$.

2.1. Goal Setting and Status of the Reference Generator

As already explained in Section 1, the robot cannot follow a user-defined velocity in the presence of non idealities or external disturbances, the case of a fixed goal corresponds to a scenario in which ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}},\mathrm{y}=0$) due to the MPC transparency. For this reason, we define the goal ${\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}$ as the position the robot would have reached if it had followed the user-commanded velocities ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}\in {\mathbb{R}}^2$. The goal is updated at each iteration of the NMPC. It is initialized with ${\mathbf{p}}^{\mathrm{goal}}={\mathbf{p}}_{\mathrm{c}},\mathrm{x},{\mathrm{y}}^{\mathrm{act}}+N_{\mathrm{g}}{\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}{T}_{\mathrm{s}}$ at the beginning of an experiment and at each iteration it is incremented by ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}{T}_{\mathrm{s}}$. Variable $T_{\mathrm{s}}$ is the sampling time of the reference generator and it is equal to $1/f_{\mathrm{s}}$ where $f_{\mathrm{s}}$ is the planning loop frequency. For the sake of simplicity, we assume that the sampling time $T_{\mathrm{s}}$ of the NMPC and the reference generator are the same. As an alternative, the user can decide a fixed goal, either for one coordinate or both. A SLAM algorithm [28,29] goes beyond the scope of this paper, so we assume that there are no obstacles and the goal can be reached.

In order to change the status of the reference generator from heuristic to optimize in a meaningful way (e.g., no transition with the normal sway of the robot) we consider the average ${\overline{\mathbf{p}}}_{\mathrm{c}}$ of the X-Y CoM position during the last gait cycle. To the average position, we add the offset due to the desired motion in the horizon $N_{\mathrm{g}}$ and compare it with the goal. We compute the error as: $\mathbf{e}={\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}-\left({\overline{\mathbf{p}}}_{\mathrm{c}}+N_{\mathrm{g}}{\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}{T}_{\mathrm{s}}\right)$ and when its norm $\parallel \mathbf{e}\parallel$ goes beyond a threshold when the reference generator status is set to optimize. Using Euclidean distance is a standard approach, but any other options, e.g., L1 or L${}_{\infty }$ could have been valid alternatives.

Algorithm 1 illustrates the pseudocode of the different computation phases of the reference generator.

Algorithm 1 Reference generator

1: Reference Generator status ← HEURISTIC   2: $\mathbf{e}\leftarrow {\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}-{\overline{\mathbf{p}}}_{\mathrm{c}}-\left({\int }_0^{N_{\mathrm{g}}{T}_{\mathrm{s}}}{\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}dt\right)$  3: if$\parallel \mathbf{e}\parallel >tol$then  4:     Reference Generator status ← OPTIMIZE  5: end if  6: ${\mathbf{p}}_{\mathrm{f}},\mathbf{\delta }\leftarrow \mathit{Heuristic} \mathit{References}$  7: if OPTIMIZE Reference Generator then  8:      while stop condition is not reached do  9:          ${\mathbf{v}}^{\mathrm{g}}\leftarrow \mathit{LIP} \mathit{Model} \mathit{Optimization}({\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}},{\mathbf{x}}_{\mathrm{c}}^{\mathrm{act}})$ 10:         ${\mathbf{p}}_{\mathrm{f}}\leftarrow \mathit{Robocentric} \mathit{Stepping}\left({\mathbf{v}}_{\mathrm{c}}^{\mathrm{g}}\right)$ 11:     end while 12:     if LIP Model Optimization found feasible solutions then 13:         ${\mathbf{v}}^{\mathrm{ref}}\leftarrow {\mathbf{v}}^{\mathrm{g}}$ 14:         ${\mathbf{u}}^{\mathrm{ref}}\leftarrow \mathit{QP} \mathit{Mapping}({\mathbf{p}}_{\mathrm{c}}^{\mathrm{g}},{\mathbf{w}}^{\mathrm{g}},{\mathbf{p}}_{\mathrm{f}})$ 15:     else 16:         ${\mathbf{v}}^{\mathrm{ref}}\leftarrow {\mathbf{v}}^{\mathrm{usr}}$ 17:         ${\mathbf{u}}^{\mathrm{ref}}\leftarrow \mathit{Gravity} \mathit{Compensation}$ 18:     end if 19: else 20:     ${\mathbf{v}}^{\mathrm{ref}}\leftarrow {\mathbf{v}}^{\mathrm{usr}}$ 21:     ${\mathbf{u}}^{\mathrm{ref}}\leftarrow \mathit{Gravity} \mathit{Compensation}$ 22: end if 23: ${\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}\leftarrow {\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}+{\mathbf{v}}_{\mathrm{c}}^{\mathrm{usr}}{T}_s$ 24: solve the NMPC [5]

2.2. Formal Guarantees on Response Time

In an optimization problem, if we set the tracking of the goal as either a running cost or a terminal cost, the response will depend on the tuning of the weights of the cost itself. In addition, with this approach, we cannot impose a predefined time $T_{\mathrm{f}}$ in which the robot reaches the goal (response interval). For these two reasons a hard constraint to impose that the CoM position matches the goal can be added to guarantee a response interval equal to $T_{\mathrm{f}}$, starting from the first time instant when the reference generator is set to the optimize status. Note that the size of the horizon $N_{\mathrm{g}}$ will be linked to the response time $T_{\mathrm{f}}$; therefore there will be a maximum value of $T_{\mathrm{f}}$ that can be achieved by the reference generator, related to the real-time constraint posed by the re-planning frequency $f_{\mathrm{s}}$. This will depend on the computational power of the machine where the optimization is run. We also need to impose the same hard constraint on all the samples after $\mathrm{M}=T_{\mathrm{f}}/T_{\mathrm{s}}$. Indeed, at every iteration of the NMPC, the interval without constraint should shrink because otherwise the target would never be reached (see Figure 3). For this reason, we enforce hard constraints on the goal, with the number of samples $\mathrm{M}$ that gradually reduces to zero. In this way, we ensure reaching the goal in a time that does not depend on the tuning of the weights. In practice, using hard constraints might lead the optimization to be trapped in an infeasible solution [30]. Therefore, an implementation with slack variables in the constraints, which allows us to relax them, is preferable. By penalizing their value in the cost function of problem (2), we have the guarantee that the robot reaches the target in a predefined time interval, and if this is not possible, the solver will find the best feasible solution.

3. Optimized Reference Generator

As already mentioned, the task of the reference generator is to compute along a horizon $N_g$ the linear (i.e., longitudinal and lateral) CoM velocity reference trajectories ${\mathbf{v}}_{\mathrm{c}}^{\mathrm{g}}$ to reach a desired goal/recover from an external disturbance in a predefined time $T_{\mathrm{f}}$ and the trajectory of GRFs ${\mathbf{u}}^{\mathrm{ref}}$ to follow it.

3.1. LIP Model Optimization

One of the features of the proposed reference generator is to take into account the intrinsic underactuated nature of a robot when only two feet are on the ground [31]. Therefore, we employ the simplest model that is able to capture this underactuation: the LIP model [21]. Indeed, this allows us to compute the optimal trajectory for the CoM, while imposing a desired behavior for the ZMP. The ZMP is defined as “a point on the ground at which the tangential component of the moment generated by the ground reaction force/moment becomes zero” [32], and its position determines the direction and magnitude of the CoM acceleration. Guaranteeing that the GRFs are such that the resulting ZMP is inside the support polygon ensures that they also satisfy the unilateral constraints [33] (the legs can only push and not pull the ground); therefore, they can be effectively realized by a real robot. Because the support polygon boils down to a line connecting the stance feet during a two-leg-stance phase, the ZMP will be able to move only on that segment. Even though the ZMP-based models have been efficiently used in MPC approaches, e.g., [34], we are aware that the LIP model presents some assumptions (vertical and angular dynamics are neglected). As already mentioned, our NMPC uses the SRBD model that will consider these dynamics in the lower optimization stage.

We define the state of the reference generator ${\mathbf{x}}_{\mathrm{c}}^{\mathrm{g}}=\{{\mathbf{x}}_{\mathrm{c},0}^{\mathrm{g}},\dots {\mathbf{x}}_{\mathrm{c},\mathrm{N}\mathrm{g}}^{\mathrm{g}}\}\in {\mathbb{R}}^{4\times (N_{\mathrm{g}}+1)}$, with ${\mathbf{x}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}={\left[\begin{array}{c}{\mathbf{p}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}},{\mathbf{v}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}\end{array}\right]}^T\in {\mathbb{R}}^4$, the stack of X-Y CoM position, and velocity at time $\mathrm{k}$. The control inputs are ${\mathbf{w}}^{\mathrm{g}}\in {\mathbb{R}}^{2\times N_{\mathrm{g}}}=\{{\mathbf{w}}_0^{\mathrm{g}},\dots {\mathbf{w}}_{N_{\mathrm{g}}-1}^{\mathrm{g}}\}$, with ${\mathbf{w}}_k^{\mathrm{g}}\in {\mathbb{R}}^2$ X-Y position of the ZMP at time $\mathrm{k}$. Footholds ${\mathbf{p}}_{\mathrm{f}},\mathrm{k}$ and gait status ${\delta }_{\mathrm{k}}$ for all the feet at each sample $\mathrm{k}$ are the parameters used to compute the support polygon at each node of the reference horizon $N_{\mathrm{g}}$. Additional parameters are initial Z CoM position ${\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}$ and $\mathrm{g}\in \mathbb{R}=9.81\mathrm{m}/{\mathrm{s}}^2$. To obtain ${\mathbf{x}}_{\mathrm{c}}^{\mathrm{g}}$ and ${\mathbf{w}}^{\mathrm{g}}$, we cast the following optimization problem:

$$\begin{array}{cc}\hfill {\displaystyle \underset{{\mathbf{x}}^{\mathrm{g}},{\mathbf{w}}^{\mathrm{g}}}{min}}& \sum _{k=0}^{N_{\mathrm{g}}}\Vert {\mathbf{p}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}-{\mathbf{p}}_{\mathrm{c}}^{\mathrm{goal}}{\Vert }_{{\mathit{Q}}_{\mathrm{p}}}^2+{\Vert {\mathbf{v}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}\Vert }_{{\mathit{Q}}_{\mathrm{v}}}^2+\hfill \end{array}$$

(1a)

$$\begin{array}{cc}\hfill & \sum _{k=0}^{N_{\mathrm{g}}-1}{\Vert {\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}-{\mathbf{w}}_{\mathrm{k}}^{\mathrm{ref}}\Vert }_{{\mathit{Q}}_{\mathrm{w}}}^2\hfill \end{array}$$

(1b)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& {\mathbf{x}}_{\mathrm{c}},0^{\mathrm{g}}={\mathbf{x}}_{\mathrm{c},\mathrm{x},\mathrm{y}}^{\mathrm{act}}\hfill \end{array}$$

(1c)

$$\begin{array}{cc}\hfill & {\mathbf{x}}_{\mathrm{c},\mathrm{k}+1}^{\mathrm{g}}={\mathbf{x}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}+\left[\begin{array}{c}{\mathbf{v}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}{T}_{\mathrm{s}}+\frac{T_{\mathrm{s}}^2\mathrm{g}}{2{\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}}\left({\mathbf{p}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}-{\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}\right)\\ \\ \frac{\mathrm{g}}{{\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}}\left({\mathbf{p}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}-{\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}\right)T_{\mathrm{s}}\end{array}\right]\hfill \end{array}$$

(1d)

$$\begin{array}{cc}\hfill & {\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}\in \mathcal{S}({\mathbf{p}}_{\mathrm{f},\mathrm{k}},{\delta }_{\mathrm{k}})k\in {\mathbb{I}}_0^{N_{\mathrm{g}}-1}.\hfill \end{array}$$

(1e)

The terms (1a) and (1b) of the cost function aim to minimize the distance between CoM position and the goal, the norm of the velocities and the distance of the ZMP from the center of the support polygon ${\mathbf{w}}_{\mathrm{k}}^{\mathrm{ref}}$ (for robustness purposes). In addition, the minimization of velocity term (1a) prevents having large velocities that could determine footholds outside of the workspace of the leg due to the robocentric stepping. Matrices ${\mathit{Q}}_{\mathrm{v}}\in {\mathbb{R}}^{2\times 2}$ and ${\mathit{Q}}_{\mathrm{w}}\in {\mathbb{R}}^{2\times 2}$ are positive definite weighting matrices. The initial condition (1c) is expressed by setting ${\mathbf{x}}_{\mathrm{c},0}^{\mathrm{g}}$ equal to the corresponding values ${\mathbf{x}}_{\mathrm{c}}^{\mathrm{act}}$ received from the state estimator. Equation (1d) corresponds to the discrete CoM dynamics for the LIP model. Equation (1e) imposes that the ZMP always lies inside the support polygon $\mathcal{S}({\mathbf{p}}_{\mathrm{f}},\mathrm{k},{\delta }_{\mathrm{k}})$. The symbol ${\mathbb{I}}_0^{N_{\mathrm{g}}-1}$ indicates the set of integer numbers in the closed interval [0, $N_{\mathrm{g}}-1$].

The optimization problem (1) does not have any guarantee on the response time. As already discussed in Section 2.2, a formulation with a slack variable can be used to impose a predefined instant in which the robot has to reach the target:

$$\begin{array}{cc}\hfill {\displaystyle \underset{{\mathbf{x}}^{\mathrm{g}},{\mathbf{w}}^{\mathrm{g}},\mathbf{s}}{min}}& \sum _{k=0}^{N_{\mathrm{g}}}\Vert {\mathbf{v}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}{\Vert }_{{\mathit{Q}}_{\mathrm{v}}}^2+\sum _{k=0}^{N_{\mathrm{g}}-1}{\Vert {\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}-{\mathbf{w}}_{\mathrm{k}}^{\mathrm{ref}}\Vert }_{{\mathit{Q}}_{\mathrm{w}}}^2+\hfill \end{array}$$

(2a)

$$\begin{array}{cc}\hfill & \sum _{k=0}^{N_{\mathrm{g}}}\left(\Vert {\mathbf{s}}_{\mathrm{k}}{\Vert }_{{\mathit{Q}}_{\mathrm{s},\mathrm{q}}}^2+{\mathit{Q}}_{\mathrm{s},\mathrm{l}}{\mathbf{s}}_{\mathrm{k}}\right)\hfill \end{array}$$

(2b)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& {\mathbf{x}}_{\mathrm{c},0}^{\mathrm{g}}={\mathbf{x}}_{\mathrm{c},\mathrm{x},\mathrm{y}}^{\mathrm{act}}\hfill \end{array}$$

(2c)

$$\begin{array}{cc}\hfill & {\mathbf{x}}_{\mathrm{c},\mathrm{k}+1}^{\mathrm{g}}={\mathbf{x}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}+\left[\begin{array}{c}{\mathbf{v}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}{T}_{\mathrm{s}}+\frac{T_{\mathrm{s}}^2\mathrm{g}}{2{\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}}\left({\mathbf{p}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}-{\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}\right)\\ \\ \frac{\mathrm{g}}{{\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}}\left({\mathbf{p}}_{\mathrm{c},\mathrm{k}}^{\mathrm{g}}-{\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}\right)T_{\mathrm{s}}\end{array}\right]\hfill \end{array}$$

(2d)

$$\begin{array}{cc}\hfill & {\mathbf{w}}_{\mathrm{k}}^{\mathrm{g}}\in \mathcal{S}({\mathbf{p}}_{\mathrm{f},\mathrm{k}},{\delta }_{\mathrm{k}})\mathrm{k}\in {\mathbb{I}}_0^{N_{\mathrm{g}}-1}\hfill \end{array}$$

(2e)

$$\begin{array}{cc}\hfill & \left[\begin{array}{c}{\mathbf{s}}_{\mathrm{k},\mathrm{x}}\\ {\mathbf{s}}^{\mathrm{k},\mathrm{y}}\end{array}\right]\ge \left[\begin{array}{c}|{\mathbf{p}}_{\mathrm{c},\mathrm{x},\mathrm{k}}^{\mathrm{g}}-{\mathbf{p}}_{\mathrm{c},\mathrm{x}}^{\mathrm{goal}}|\\ |{\mathbf{p}}_{\mathrm{c},\mathrm{y},\mathrm{k}}^{\mathrm{g}}-{\mathbf{p}}_{\mathrm{c},\mathrm{y}}^{\mathrm{goal}}|\end{array}\right]\mathrm{k}\in {\mathbb{I}}_{\mathrm{M}}^{N_{\mathrm{g}}}\hfill \end{array}$$

(2f)

$$\begin{array}{cc}\hfill & {\mathbf{s}}_{\mathrm{k},\mathrm{x}},{\mathbf{s}}_{\mathrm{k},\mathrm{y}}\ge 0\mathrm{k}\in {\mathbb{I}}_0^{N_{\mathrm{g}}}.\hfill \end{array}$$

(2g)

The slack variable ${\mathbf{s}}_{\mathrm{k}}\in {\mathbb{R}}^{2\times 1}$ is added in the cost term (2b) and thanks to Equations (2f), and (2g) it allows us to impose that the robot CoM coincides with the goal after $\mathrm{M}$ samples. In contrast to the optimization problem (1), the cost function does not explicitly include a position term, because it is enforced with slacks into Equation (2b). Matrices ${\mathit{Q}}_{\mathrm{s},\mathrm{q}}\in {\mathbb{R}}^{2\times 2}$ and ${\mathit{Q}}_{\mathrm{s},\mathrm{l}}\in {\mathbb{R}}^{1\times 2}$ are the additional weights for the quadratic and linear term for the slack variables. Equations (2c)–(2e) are the same as Equation (1c)–(1e) respectively.

3.2. QP Mapping

The reference generator computes CoM trajectories which the NMPC must follow, but the latter also requires reference GRFs ${\mathbf{u}}^{\mathrm{ref}}\in {\mathbb{R}}^{12\times N}$. Because the output of the optimization problem (1) or (2) is a trajectory for the ZMP ${\mathbf{w}}^{\mathrm{g}}$, we need to map this into a set of consistent GRFs, thus moving from a bidimensional to the higher dimensional space of contact forces. For this reason, we define the set of indices of the legs in contact with the ground by $\mathbb{C}$. A parametric QP is solved to find the vector of GRFs ${\mathbf{u}}^{\mathrm{qp}}\in {\mathbb{R}}^{3\left|\mathbb{C}\right|}$, which corresponds to the ZMP location ${\mathbf{w}}^{\mathrm{g}}$. If a foot is in the swing phase, its GRFs are set to 0 by default,

$${\mathbf{u}}_{\mathrm{i}}^{\mathrm{ref}}=\left\{\begin{array}{cc}{\mathbf{u}}_{\mathrm{i}}^{\mathrm{qp}}\hfill & \mathrm{i}\in \mathbb{C}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill & \mathrm{i}\notin \mathbb{C},\hfill \end{array}\right.$$

(3)

where $\mathrm{i}$ is the leg index. Note that we need to solve a QP for each sample $\mathrm{k}$ in the horizon N, therefore to avoid overloading the notation, we do not specify the subscript k in the following quantities. The parameters of the model are the footholds ${\mathbf{p}}_{\mathrm{f}}$, the X-Y components of the CoM position ${\mathbf{p}}_{\mathrm{c}}^{\mathrm{g}}$, the ZMP trajectory ${\mathbf{w}}^{\mathrm{g}}$, the initial Z CoM position ${\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}$, and the mass of the robot $\mathrm{m}\in \mathbb{R}$. Thus, for every $\mathrm{k}$ we solve

$$\begin{array}{cc}\hfill {\displaystyle \underset{{\mathbf{u}}^{\mathrm{qp}}}{min}}& \Vert {\mathbf{u}}_{\mathrm{x},\mathrm{y}}^{\mathrm{qp}}{\Vert }_{{\mathit{Q}}_{\mathrm{u}}}^2+{∥\sum _{i\in \mathbb{C}}{\left[{\mathbf{p}}_{\mathrm{f},\mathrm{i}}-{[{\mathbf{p}}_{\mathrm{c}}^{\mathrm{g}},{\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}]}^T\right]}_{\mathrm{x}}{\mathbf{u}}_{\mathrm{i}}^{\mathrm{qp}}∥}_{{\mathit{Q}}_{\mathrm{k}}}^2\hfill \end{array}$$

(4a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& {\mathbf{w}}_{\mathrm{x},\mathrm{y}}^{\mathrm{g}}=\frac{{\sum }_{i\in \mathbb{C}}{\mathbf{p}\mathrm{f}}_{\mathrm{i},\mathrm{x},\mathrm{y}}{\mathbf{u}}_{\mathrm{i},\mathrm{z}}^{\mathrm{qp}}}{{\sum }_{i\in \mathbb{C}}{\mathbf{u}}_{\mathrm{i},\mathrm{z}}^{\mathrm{qp}}}\hfill \end{array}$$

(4b)

$$\begin{array}{cc}\hfill & \sum _{i\in \mathbb{C}}{\mathbf{u}}_{\mathrm{i},\mathrm{z}}^{\mathrm{qp}}=\mathrm{m}\mathrm{g}\hfill \end{array}$$

(4c)

$$\begin{array}{cc}\hfill & \frac{\mathrm{g}}{{\mathbf{p}}_{\mathrm{c},\mathrm{z}}^{\mathrm{act}}}({\mathbf{p}}_{\mathrm{c}}^{\mathrm{g}}-{\mathbf{w}}^{\mathrm{g}})=\frac{{\sum }_{i\in \mathbb{C}}{\mathbf{u}}_{\mathrm{i},\mathrm{x},\mathrm{y}}^{\mathrm{qp}}}{\mathrm{m}}.\hfill \end{array}$$

(4d)

The first term of Equation (4a) is the regularization term on the X-Y components of the GRFs, with ${\mathit{Q}}_{\mathrm{u}}\in {\mathbb{R}}^{2\times 2}$ as weighting matrix, whereas the second one minimizes the angular momentum rate, and it is weighted by ${\mathit{Q}}_{\mathrm{k}}\in {\mathbb{R}}^{3\times 3}$. Variable ${\left[·\right]}_{\mathrm{x}}$ represents the skew-symmetric matrix associated with the cross-product. Equation (4b) is the definition of ZMP, Equation (4c) represents the gravity compensation. Equation (4d) guarantees that the X-Y CoM acceleration computed with the LIP model in the reference generator (left term) coincides with the one of the SRBD used in NMPC. It is worth highlighting that when the robot is on two legs, the ZMP lies on a line, and so it can only move in a 1D manifold. During that phase, imposing the Equation (4b) for the X component is enough to guarantee that the Y coordinate of the ZMP also respects the same constraint. Along the horizon $N_{\mathrm{g}}$, each problem is independent from the others, so they can be solved in parallel. In this way the computation effort remains low (a couple of ms to solve a QP problem with linear constraints) and therefore the new reference generator can be integrated into our high-frequency NMPC scheme.

4. Simulation and Experimental Results

The optimization-based reference generator endows the NMPC planner with the capability of recovering from disturbances and avoiding drifting in the face of nonidealities. To show its effectiveness, we tailored three template scenarios: (a) motion along a straight line, (b) reaching a fixed goal, and (c) recover from external pushes. We performed the simulations and experiments on the 22 $\mathrm{kg}$ quadruped robot AlienGo of Unitree.

We consider the trot as a template gait for our experiments because of its inherently unstable nature. Indeed, any asymmetry in the real robot can make it drift when setting a pure forward velocity. The quadruped is thus not able to follow a straight line.

The variable $T_{\mathrm{s}}$ is set to 40 $\mathrm{ms}$ and the horizons of both reference generator ($N_{\mathrm{g}}$) and NMPC (N) are 50 nodes, corresponding to an interval of 2 $\mathrm{s}$. We keep, thus, the same planning loop frequency of $25\mathrm{Hz}$ as our previous work. The trot parameters are cycle time $T_{\mathrm{c}}$ = 1 $\mathrm{s}$, duty factor D = duration of stance phase/$T_{\mathrm{c}}$ = 0.65. The values of the weighting matrices are reported in

Table 1. Weights used in the governor.

Cost	Weight	Value
Velocities LIP	${\mathit{Q}}_{\mathrm{v}}$	$\mathrm{diag}(200,300$)
ZMP	${\mathit{Q}}_{\mathrm{w}}$	$\mathrm{diag}(100,350)$
Slack	${\mathit{Q}}_{\mathrm{s},\mathrm{q}}$	$\mathrm{diag}(0,1000)$
	${\mathit{Q}}_{\mathrm{s},\mathrm{l}}$	$[0,1000]$
Forces QP mapping	${\mathit{Q}}_{\mathrm{u}}$	$\mathrm{diag}(100,100)$
Angular Momentum Rate	${\mathit{Q}}_{\mathrm{k}}$	$\mathrm{diag}(1,1,1)$

. Their value has been tuned with a trial and error procedure. We refer to Table 2 of [5] for the values of the weights of the NMPC and the WBC + PD Joint Controller. Without any lack of generalization, slack variables are considered only for the Y component.

We used the HPIPM [35] solver integrated into acados [36] library to find the solutions of the problem (2). The problem (4), instead, is solved by using eiquadprog [37], because it offers a more straightforward interface for the QPs. Both reference generator and NMPC run on an Intel Core i7-10750H CPU @ 2.60 $\mathrm{GHz}$. The LIP model optimization takes 2–4 $\mathrm{ms}$ with the prediction horizon of $2\mathrm{s}$ and thus N equal to 50, whereas the time required by each QP (Equation (4a–4d)) is negligible, approximately 0.01 ms.

In this section, we will call the output of the reference generator as reference, the output of the NMPC as desired, and the real values measured by the state estimator as actual.

4.1. Simulations

The four-legged-stance phase in a walking trot is the only moment of the gait during which the robot has full control authority and is able to track the reference velocities. Neglecting this fact would lead to failure or unpredictably longer response times. In our algorithm, the reference generator already takes into account the underactuation, enabling us to deal with this issue successfully. The simulations of the scenario (a) are only reported in the accompanying video [38].

Figure 4 refers to the simulation of the scenario (b) in which the robot has to go to a lateral target ($-0.2$ $\mathrm{m}$ on the Y coordinate) in a predefined time ($T_{\mathrm{f}}=4.8$ $\mathrm{s}$) and then keep it while walking. Because X-Y directions are decoupled in the LIP model, the X component of the goal ${\mathbf{p}}_{\mathrm{c},\mathrm{x}}^{\mathrm{goal}}$ is updated at each iteration of the NMPC according to Algorithm 1 and continuously tracked. The top plot demonstrates that the reference generator is able to compute reference CoM trajectories (yellow line) that allow the actual CoM values (red line) to accomplish the task. Because the reference quantities satisfy the underactuation of the gait, reference velocities are nicely tracked by the NMPC (respectively, yellow and blue line, bottom part of Figure 4) and consequently by the actual ones.

Figure 5, instead, shows the reference ${\mathbf{w}}^{\mathrm{g}}$ and actual X-Y components of the ZMP locations for the same simulation (computed by Equation (4b)). As can be seen, the robot is able to track the reference values for the entire cycle, for both X-Y coordinates, i.e., they have the same trend. In this simulation, we decided to keep the reference generator always in the optimize status to show how the algorithm is able to track (once the target has been reached) a zero user-defined lateral velocity ${\mathbf{v}}_{\mathrm{c},\mathrm{y}}^{\mathrm{usr}}$.

Figure 6 reports scenario (b) with the same goal (−0.2 $\mathrm{m}$) but different response interval ($T_{\mathrm{f}}=3$ $\mathrm{s}$). Due to the smaller interval, the response is more aggressive and presents an overshoot which is then recovered within the 3 $\mathrm{s}$ interval. The task is achieved by simply modifying the response interval $T_{\mathrm{f}}$; no tuning of the weights of the cost function is required. Shaded areas highlight the response interval. Figure 7 shows the tracking error between the actual and desired Y CoM position. It confirms that the error is negligible and the desired values are tracked by the robot.

Next, we want to validate our approach by comparing it with a simple Cartesian PD approach in the scenario (c). This is typically implemented computing a feed-forward force $f_{\mathrm{ff}},\mathrm{y}\in \mathbb{R}$ which depends on the error between the actual state and the goal, i.e., $f_{\mathrm{ff},\mathrm{y}}=K_{\mathrm{p}}\left({\mathbf{p}}_{\mathrm{c},\mathrm{y}}^{\mathrm{goal}}-{\mathbf{p}}_{\mathrm{c},\mathrm{y}}^{\mathrm{act}}\right)+K_{\mathrm{d}}\left({\mathbf{v}}_{\mathrm{c},\mathrm{y}}^{\mathrm{usr}}-{\mathbf{v}}_{\mathrm{c},\mathrm{y}}^{\mathrm{act}}\right)$.

In this scenario, for the first 5 seconds of the motion the robot is pushed with a force of 15 $\mathrm{N}$ in the Y direction. The user velocity ${\mathbf{v}}_{\mathrm{c},\mathrm{y}}^{\mathrm{usr}}$ is zero, so the task is to come back to the initial Y position. In Figure 8 we report the actual Y CoM position ${\mathbf{p}}_{\mathrm{c},\mathrm{y}}^{\mathrm{act}}$ in three different cases.

The green line represents the case in which the system is modeled as a second order with critically damped response.

From the control theory, we know that the settling time is equal to $T_{\mathrm{f}}=\frac{4}{\zeta {\omega }_{\mathrm{n}}}$, with $\zeta$ equal to damping ratio and ${\omega }_{\mathrm{n}}$ natural frequency. To have a critically damped response we aim at $\zeta$ = 1.

Rewriting the second order equation of the mass/spring/damper as a function of $\zeta$ and ${\omega }_{\mathrm{n}}$ we have $K_{\mathrm{p}}=m{\omega }_{\mathrm{n}}^2$ and $K_{\mathrm{d}}=2\zeta \sqrt{\mathrm{m}{K}_{\mathrm{p}}}$.

Merging the two expressions results $T_{\mathrm{f}}=4\sqrt{\frac{\mathrm{m}}{K_{\mathrm{p}}}}$ with $\zeta =1$. Considering $T_{\mathrm{f}}=4.8$ $\mathrm{s}$ and a total robot mass $\mathrm{m}=22$ $\mathrm{kg}$, we obtained $K_{\mathrm{p}}=15$ $\mathrm{N}/\mathrm{m}$ and consequently $K_{\mathrm{d}}=36\mathrm{Ns}/\mathrm{m}$.

It is evident that the CoM is far from reaching the goal in the predefined time.

The purple line, instead, represents the case in which the values of proportional and derivative gain are manually tuned such that the robot reaches the goal faster. The result is $K_{\mathrm{p}}=170$ N/m and $K_{\mathrm{d}}=122$ $\mathrm{Ns}/\mathrm{m}$. As can be seen in the plot, once the disturbance is removed, the robot moves toward the goal, but it is not able to follow it once it has been reached. In fact, the robot keeps drifting, moving away from the initial position. We can conclude thus that the approach with a feed-forward force results in a slow response, with a steady-state error with respect to the goal. The common practice of setting the PD parameters considering the system behaving as a second order system (i.e., spring/mass /damper) is not valid due to the underactuation, showing the limitation of the approach. In addition, even hand-tuning of the parameters does not allow the user to obtain the desired behaviour, e.g., the values of $K_{\mathrm{p}}$ and $K_{\mathrm{d}}$ which have good performance in recovering from the push (purple line) are not suitable for the straightforward motion. The result of our approach, instead, is reported with a light blue line. Once the disturbance has been removed, the reference generator computes a velocity trajectory that brings the robot toward the goal in the desired time $T_{\mathrm{f}}$ (shaded area) and without steady state error.

4.2. Experiments

In this section, we present preliminary experiments for the scenarios (a), (b), and (c) carried out with the real robot platform. In these experiments, we employed the problem (1) without ensuring guarantees on the response time. As a first experiment, we performed scenario (a) on the robot, as illustrated in Figure 9.

The figure demonstrates that the NMPC with only a heuristic reference generator (dashed lines) does not succeed in moving on a straight line for a statically unstable gait as trot.

Indeed, the robot suffers lateral and backward (less visible) drifts for two reasons: (1) the trot being an unstable gait, the CoM always diverges between two four-legged-stance events in opposite directions. Any little asymmetry in the robot results in a cumulative drift in one direction; and (2) because Aliengo has c-shaped legs, they create nonzero moments about the pitch axis during a swing.

Enabling the optimal reference generator, instead, the robot is able to reach the goal and prevent the trajectory from drifting (see the continuous line in Figure 9).

Figure 10 shows the change in the reference lateral velocity (yellow line) done by the reference generator. When the average Y position (${\overline{\mathbf{p}}}_{\mathrm{y}}$, red line) exceeds the bounds (${\overline{\mathbf{p}}}_{\mathrm{y}}^{\mathrm{bound}}$, dashed lines), the status is changed to optimize and the reference generator brings back the CoM close to the goal. A threshold of 1 $\mathrm{cm}$ around the constant goal has been chosen for the activation. Once the goal has been reached, the reference generator automatically resets to heuristic, and the reference velocity becomes equal to the user one (zero). The continuous changing of the status of the reference generator demonstrates the need to have an external module which corrects the reference trajectories during a trot.

Figure 11 shows the Y position of the robot in the scenario (b) with a fixed goal of −0.15 $\mathrm{m}$. As in simulation, the robot is able to track the reference value, due to the fact that the velocity takes into account the underactuation of the trot gait. In this case, we decided to keep the reference generator always set to optimize to demonstrate that it is able to work properly also when the target has been reached, compensating drifts as in scenario (a).

In the last experiment, we show how we can use our reference generator to react to external disturbances (see Figure 12). As we have already mentioned in the Introduction, the task is not to reject the disturbance, but to cope with it and later recover from its effect. An analysis of techniques to reject disturbances goes beyond the scope of this work. Figure 13 shows the Y position of the CoM in a real hardware experiment in scenario (c) when the robot receives two manually applied pushes. The threshold on the error is set to 1 $\mathrm{cm}$ (dashed purple line). During the push, the robot tries to resist to the disturbance and, thanks to the high-frequency re-planning of the NMPC, it is able to keep the stability and avoid falling. Once the pushing force is removed, the reference generator drives the robot back toward the initial position. As in the previous cases, the reference generator is set to optimize when the robot is diverging from the goal.

The readers are encouraged to check the experiments corresponding to the mentioned results in the accompanying video.

5. Conclusions

In this work, we presented a novel optimization-based reference generator for quadruped locomotion, which deals with the underactuation of statically unstable gaits and with external disturbances. It exploits the LIP model to compute feasible reference trajectories that allow the robot to accomplish a tracking task. A QP mapping is used to determine the GRFs which correspond to the ZMP location computed by the LIP model. Velocities and GRFs are used as informative tracking references by a 25 $\mathrm{Hz}$ lower-stage NMPC planner introduced in [5]. This results in the absence of conflicting tasks in the cost function, which simplifies the tuning of the weights inside the costs of the NMPC. We validated our approach by performing simulations and experiments with the 22 $\mathrm{kg}$ quadruped robot Aliengo in three different scenarios: (a) straight motion, (b) fixed lateral goal, and (c) recovery after a push. For the last scenario, we demonstrated that the simple solution of adding a Cartesian PD in parallel to the NMPC is not enough to return in a predefined time to the required position. In addition, we presented and validated in simulations a formulation with slack variables that are guaranteed to reach the goal in a specified time, without the need to further tune any parameters.

Future work involves extending the optimize reference generator also to the heading (i.e., yaw) orientation, and to perform additional experiments with the real platform with the formulation that respects a specified response time.