MPC-based Controller with Terrain Insight for Dynamic Legged Locomotion

the purpose of the VFA is to continuously compute adjustments for the trajectory of the feet in order to avoid collisions, and unstable or unreachable landing positions. For a more detailed description on this method we refer the reader to [villarreal19ral]. For a leg in swing phase, we initially compute a prediction of its landing position based on the current velocity of the trunk (taken from the state estimator) and the trajectory of the foot (in our case a half-ellipse) using the approximation:

where ${\hat{p}}_i\in R^3$ is the predicted foothold position of leg $i$, ${\overline{p}}_i\in R^3$ is the center of the ellipse of leg $i$, $\Delta t_i$ is the time remaining to the next stance change of leg $i$ (for $i=LF,RF,LH,RH$), $l_s\in R^3$ is the step length vector, and $\dot{r}\in R^3$ corresponds to the velocity of the base. All vector variables are given in world coordinates. In the case of the next touchdown of a swing leg, $\Delta t_i=\frac{1-D_f}{f_s}-t_{sw,i}$, where $D_f$ is the duty factor, $f_s$ is the step frequency and $t_{sw,i}$ is the elapsed swing time since the latest lift-off of leg $i$. The first two terms in (1) are related to the leg trajectory, while the third term is related to the displacement of the base. After computing the prediction of the next foothold, a 2D representation of the terrain around that foothold is acquired, namely a heightmap. We pre-train a CNN to learn the optimal footholds from heightmaps [villarreal19ral]. The CNN takes on average $0.1ms$ to evaluate a heightmap and output a safe foothold. This fast computation time allows us to continuously adapt the trajectory of the swing leg to reach the adapted foothold.

We use the computational gain obtained by the VFA to evaluate further ahead in the terrain. Knowing that the gait is periodic and defined by the step frequency $f_s$ and the duty factor $D_f$, we can estimate the timings for the non-immediate foot contacts. Using these timings, one can compute the predicted foothold locations for each of the legs at every stance change (lift-off or touchdown) replacing them for $\Delta t$ in (1). We then use our CNN-based foothold adaptation to adjust the predicted foothold location. This is done for the next two gait cycles (eight contacts in total and 16 stance changes). On the right side of the series of snapshots of Fig. 2 an example of a safe foothold sequence can be seen. Namely, $p_i\left[k\right]$ is the contact location of leg $i$ at stance change $k$, for $k=1,...,16$. In (1), $\dot{r}$ is assumed constant in between stance changes. The CNN continuously provides safe contact sequences at task frequency ($250Hz$). These contact sequences are used both as future foot positions and to inform the MPC controller to improve the COM regulation, as explained in Section III-B. This interaction is depicted in Fig. 1. One key feature of this approach is that the safe footholds are computed without including them as optimization variables in the MPC controller, which significantly decreases the complexity of the problem.

To provide the reference trajectory for the COM along the prediction horizon, we compute its location at every stance change based on the desired gait timings using $f_s$ and $D_f$. For two gait cycles, there are a total of 16 stance changes, so we compute a total of 16 COM positions. Similarly to the third term of (1), we compute the reference yaw using the desired yaw rate as

where ${\psi }_{ref}\left[k\right]\in R$ is the yaw reference at stance change $k$, $\psi \in R$ is the current yaw angle of the body, and $\Delta t\left[k\right]$ is the remaining time before the next stance change of $k$. Using the reference for the yaw angle, we compute the reference position of the COM with respect to the world

where $r_{ref}\left[k\right]\in R^3$ is the reference position for the COM at stance change $k$, $R_z\left(\Delta \psi \right)\in R^{3x3}$ is the rotation matrix around the $z$ axis about $\Delta \psi$ (with $\Delta \psi ={\psi }_{ref}\left[k\right]-\psi$) and ${\dot{r}}_{ref}$ is the reference velocity obtained from $V_f$ and ${\dot{\psi }}_{ref}$. This provides the reference for the next $x$ and $y$ positions of the COM with respect to the world. The reference for the body roll ${\varphi }_{ref}$ and pitch ${\theta }_{ref}$ relies on the contact configuration at each stance change. We estimate the orientation of the terrain and define that orientation as reference for the body. We also use the contacts to define a height $z$ reference position for the body (namely, $r_{ref,z}\left[k\right]$), setting it to remain at a constant distance from the center position of the approximated plane in the direction of the $z$ world axis. To obtain ${\dot{r}}_{ref,z}\left[k\right]$, ${\dot{\varphi }}_{ref}\left[k\right]$ and ${\dot{\theta }}_{ref}\left[k\right]$ we derive numerically between samples of $r_{ref,z}\left[k\right]$, ${\varphi }_{ref}\left[k\right]$ and ${\theta }_{ref}\left[k\right]$, respectively. Finally, we evenly sample the 16 reference points given by the stance changes, filling the gaps in between samples using a zero-order hold (ZOH). We define a desired reference vector at evenly sampled time $k$ as

with ${\Theta }_{ref}\left[k\right]={\left[{\theta }_{ref}\right[k\left]{\varphi }_{ref}\right[k\left]{\psi }_{ref}\right[k\left]\right]}^{⊺}$ and $r_{ref}\left[k\right]={\left[r_{ref,x}\right[k\left]r_{ref,y}\right[k\left]r_{ref,z}\right[k\left]\right]}^{⊺}$. A series of COM references can be seen in Fig. 2.

our MPC-based balance controller is inspired by the work of Di Carlo et al. [iros18dicarlo]. We also model the robot as a rigid body subject to contact patches at each stance foot and we neglect the effects of precession and nutation as in [focchi2016]. However, there are two key differences in our approach: firstly, we do not define the reference roll and pitch angles to be zero. Additionally, although we do not explicitly consider the leg inertia in our model for control, we compensate for it by computing the wrench exerted by the legs using the actuated part of the joint-space inertia matrix and the desired accelerations of the joints. We explain how this is done by the end of this section. The dynamic model of the rigid body and its rotational kinematics are given by

where $r\in R^3$ is the position of the COM, $F_i\in R^3$ is the ground reaction force (GRF) at foot $i$, $m\in R$ is the robot mass, $g\in R^3$ is the gravitational acceleration, $I\in R^{3\times 3}$ is the inertia tensor of the robot, $p_i\in R^3$ is the $i$-th foot contact position, $R\in R^{3\times 3}$ is the rotation matrix from body to world coordinates according to roll $\varphi$, pitch $\theta$ and yaw $\psi$ angles and $\omega \in R^3$ is the robot’s angular velocity. The operator $[x{]}_{\times }$ is the skew-symmetric matrix such that $[x{]}_{\times }y=x\times y$. In (6) we are neglecting precession and nutation effects, namely $\omega \times I\omega \approx 0$. We rewrite equations (5), (6) and (7) in state-space representation. Initially, from (7) we can obtain the angular velocity in terms of the body’s Euler angles as

which can be rewritten as

where $\Theta =[\varphi \theta \psi {]}^{⊺}$ and $T\left(\Theta \right)$ is the matrix that maps from euler angle rates to angular velocities. The only condition on $T\left(\Theta \right)$ to be invertible is $\theta \ne \pi /2$, which in practice does not happen (it implies that the robot is pointed vertically). Thus, the angular rate can be obtained as

We then define state vector $x=[\Theta r\omega \dot{r}g{]}^{⊺}$, and rearranging (5), (6) and (10) the system can be described in state-space form as

where $u$ is the vector of GRFs. Note that no assumptions are made about the orientation of the robot¹¹1If $\theta \approxeq \varphi \approxeq 0$ then: $\dot{x}\left(t\right)=A\left(\psi \right)x\left(t\right)+B(\psi ,p_{LF},...,p_{RH})u\left(t\right)$ (except for $\theta \ne \pi$) and that we explicitly denote the dependence of $T$ and $I$ with respect to $\Theta$. In a similar fashion to [iros18dicarlo], we approximate the system dynamics in (11) to a discrete-time linear system. Namely, for each reference vector $x_{ref}\left[k\right]$ (for $k=1,...,n$, where $n$ is the prediction horizon length), we compute the approximate linear, discrete system matrices $A_d\left[k\right]$ and $B_d\left[k\right]$. We first substitute the feet locations $p_i$ obtained from the contact sequence task into matrix $B(\Theta ,p_{LF},...,p_{LH})$ for every contact configuration at time instant $k$. However, matrices $A\left(\Theta \right)$ and $B(\Theta ,p_{LF},...,p_{LH})$ are still dependent on the body orientation (in a nonlinear fashion). To obtain the linear, discrete time versions of these matrices, we follow a similar argument to [iros18dicarlo]. Assuming that the MPC-based controller will follow sufficiently close the reference trajectory given by $x_{ref}\left[k\right]$, we substitute the values of ${\Theta }_{ref}\left[k\right]$ into system matrices $A\left(\Theta \right)$ and $B(\Theta ,p_{LF},...,p_{LH})$. We also consider the values of ${\theta }_{ref}$ and ${\varphi }_{ref}$ computed by the COM reference trajectory. We then discretize the system matrices using a ZOH. The discrete-time linear system dynamics can be described as

we can obtain a discrete time evolution of the system by successive substitution of states $x\left[k\right]$ into (12) to obtain the state evolution from $k=0$ to $k=n$. Then, we can describe the dynamics as

where $X\in R^{15n}$ is the stacked vector of states along the prediction horizon $X={\left[x^{⊺}\right[1],...,x^{⊺}[n\left]\right]}^{⊺}$, $\overline{A}\in R^{15n\times 15n}$ and $\overline{B}\in R^{15n\times in}$ are the matrices built by successive substitution along the prediction horizon, $x_0\in R^{15}$ is the actual robot state vector and $\overline{u}\in R^{12n}$ is the stacked vector of ground reaction forces along the prediction horizon $\overline{u}={\left[u^{⊺}\right[0],,...,u^{⊺}[n-1\left]\right]}^{⊺}$. We formulate the optimization problem to minimize the weighted least-squares error between the states and the reference along the prediction horizon. We enforce the gait pattern $\text{pazocal}G$ and friction consistency by setting appropriate constraints. Namely, we solve the following optimization problem

where $X_{ref}\in R^{15n}$ is the stacked vector of desired states along the prediction horizon²²2We redefine to match the state definition of $x$, vectors ${\overline{u}}_x\in R^{4n}$, ${\overline{u}}_y\in R^{4n}$ and ${\overline{u}}_z\in R^{4n}$ correspond to the components of vector $\overline{u}$ associated to coordinates $x$, $y$ and $z$, respectively, of the GRFs, $\mu \in R$ is the friction coefficient between the ground and the feet, $u_{min}\in R^{4n}$ and $u_{max}\in R^{4n}$ are the limits on the $z$ component of $\overline{u}$, matrix $G\in R^{12n\times 12n}$ is a matrix that selects the components of the GRFs that are in contact according to gait $\text{pazocal}G$, and matrices $L$ and $K$ are weighting matrices. The optimization problem defined by (14) is a QP and can be efficiently solved by several off-the-shelf solvers. After solving the problem in (14), we take the first 12 entries of the optimized control input vector ${\overline{u}}_{\ast }$, which correspond to the set of GRFs at time instant $k=1$ and compute the desired wrench coming from the MPC-based controller as

where $F_{i,\ast }$ is the optimized GRF of foot $i$.

the MPC model used for prediction neglects leg inertia. This assumption is acceptable for quasi-static motions. However, if the leg-body weight ratio is significantly large, the wrench exerted by the legs on the body plays a significant role in the dynamics. We compensate for these effects in a simple, yet effective, manner. The floating base dynamics of a robot can be described by

where $v\in R^6$ is the floating-base robot velocity, $q\in R^{n_j}$ is the joint configuration, $M_u\in R^{6\times 6}$ and $M_a\in R^{6\times n_j}$ are the direct un-actuated and actuated parts of the joint-space inertia matrix, whereas $M_{ua}\in R^{6\times n_j}$ and $M_{au}\in R^{n_j\times 6}$ correspond to the cross terms between actuated and un-actuated parts of the joint-space inertia matrix, $h_u\in R^6$ and $h_a\in R^{n_j}$ are the un-actuated and actuated vectors of Coriolis, centrifugal and gravitational terms, ${\tau }_j\in R^{n_j}$ is the vector of joint torques, $J_{c,u}\in R^{n_c\times 6}$ and $J_{c,a}\in R^{n_c\times n_j}$ are the un-actuated and actuated contact Jacobians and $F$ is the vector of GRFs. The cross-term matrix $M_{ua}$ maps the joint accelerations to the robot spatial force acting on the floating-base of the robot, namely

In (17), the $w_l$ can be computed directly using measurements coming from the sensors. However, using the actual joint acceleration might lead to high frequency wrench signals. Instead, We use the desired joint acceleration ${\ddot{q}}_{j,d}$ coming from the torque mapper (see Fig. 1). Then, the leg inertia compensation wrench is given by $w_l=M_{ua}{\ddot{q}}_{j,d}$. Thus, the total desired wrench is given by $w_d=w_{MPC}+w_l$.

we perform simulations commanding the robot to trot on flat terrain with a forward velocity $V_f$ of $0.5m/s$, a step frequency $f_s$ of $1.4Hz$, and a duty factor $D_f$ of 0.6. We start the simulation with our previous trunk controller [focchi2016]. We keep $V_f$ and change the controller configuration as the robot continues to trot. There are six possible configurations shown in Fig. 3 which combine the following control components: a) QP: standard QP trunk controller b) LI: stance leg impedance c) GC: gravity compensation d) IC: leg inertia compensation and e) MPC: model predictive controller. Figure 3 shows the error in velocity with respect to the commanded $V_f$ for one of the trials. The vertical dashed red lines indicate the moments when the controller configuration was changed. We check six different configurations, although we would like to stress that configuration C3 acts merely as a a transition between the standard QP and the MPC. This is because the MPC controller already compensates for the gravitational effects in the model. It can be noticed that when the inertia compensation wrench is applied, the accelerations of the body are greatly reduced. The best performing configuration corresponds to the combination C5 (fifth portion of graph in Fig. 3). Under this configuration, the robot dynamics resemble more those of the MPC model (which neglects leg inertia), since the leg inertia is being accounted for outside of the optimization.

for the second simulation the robot is also commanded to trot on flat terrain with the same gait parameters as in the first simulation. This time we perturb it three times with $700N$ of force with a duration of $0.1s$. Table I shows the root mean square (RMS) error and the maximum absolute value of the foothold prediction error for this simulation. The table helps us to compare the previous controller configuration with the MPC-based controller with leg inertia compensation. The RMS error when using MPC and leg inertia compensation is between 25% and 41% less with respect to the previous controller configuration. This represents between $3mm$ and $5mm$ of improvement. However, even if the average of the error is low in both cases, a single wrong prediction compromises the robot stability. In this scope, the maximum absolute value of the error is more representative of the reliability of the prediction under disturbances. In this case, the reduction of the error is between 7% and 36%. This represents between $0.6cm$ and $3cm$ of reduction of the foothold prediction error when using the MPC-based controller in combination with the leg inertia compensation.

to verify the improvement in performance regarding locomotion on difficult terrain, we designed the challenging scenario shown in Fig. 2. The robot is commanded to trot with a forward velocity of $0.4m/s$, a step frequency of $1.4Hz$ and a duty factor of 0.6. In order to select appropriate footholds, we use the VFA with two different control configurations a) QP + LI + GC and b) MPC + IC. To test the performance repeatability we did four trials with each control configuration. Figure 4 contains the plots corresponding to pitch angle, forward velocity, body height and an example of the foot trajectories for one of the trials with the MPC + IC configuration. Figure 2 shows 11 overlapped snapshots of the RVIZ visualization as the robot crosses the scenario, builds the map, and adjusts its footholds on the fly. Figure 2 also shows the reference trajectory of the center of mass given at the specific moment when the snapshot was taken, and the foot trajectories as the robot moves through the scenario. From Fig. 4 it can be seen that all four different trials using the MPC + IC strategy were successful and the variations in linear velocity, pitch and body height are significantly reduced with respect to the QP + IC + GC configuration. For this last configuration, the robot was not able to reach the end of the scenario for any of the trials. This task clearly shows the mutual benefits between the VFA and the MPC-based controller. The foothold prediction error is reduced when using the strategy here presented. Specifically, in the case of the MPC + IC, for all four trials and all legs (LF, RF, LH and RH), the maximum absolute value of the error in foothold prediction in all four trials was $10cm$, while in the case of the previous controller configuration the error was up to $14cm$.