Fast Footstep Planning on Uneven Terrain Using Deep Sequential Models

Methods for footstep planning that do not use machine learning are typically based on solving either graph search or optimization problems. These methods often generate quasi-static trajectories or are too slow to be used online. On the graph search side, [4] uses best-first search over input angles for a one-legged robot, sampling possible leg angles at each impact to find the next node. For multi-legged robots, [5, 6, 7, 8] use a variety of graph search algorithms to plan only a Center of Mass (CoM) trajectory and use terrain heuristics to find footholds along that trajectory, while [1, 9, 10] explicitly search over footsteps based on stable stance transitions.

Approaches that use optimization often make use of some relaxation or approximation of the underlying dynamics to speed up computation time. [11] express step lengths as gait phases to find center-of-mass and footstep locations using nonlinear optimization, at the cost of not enforcing some swing foot collisions. [12] formulates footstep planning as a mixed-integer quadratic program and can solve for short footstep plans in under a second, but does not consider the robot dynamics. [13] uses a convex relaxation of the dynamics in order to generate 2 to 4 step plans in roughly one second. [2, 14] consider more complete dynamic models but the whole body optimizations take minutes to solve. Most of these optimization schemes could benefit from an independent system for initializing the footstep locations which our method could provide.

Researchers have also utilized learning-based methods to plan footstep locations. One popular method is to use Reinforcement Learning (RL) and learn a policy from visual inputs to select the optimal next footstep or action [15, 16, 17, 18, 19]. These methods can leverage the full robot model, but usually only plan footsteps or motion primitives over a short horizon, and sometimes rely on hand-selected terrain features in the reward function. Variational Autoencoders have also been explored for footstep planning to deal with multimodality: the fact that there are many feasible choices for steps, depending on the desired behavior [20].

Another approach is to incorporate learned modules into the more traditional planning frameworks. Previous methods include learning cost functions to speed up optimization or search [21, 22] learning forward dynamics models [23], adaptation of next footholds based on visual information and heuristics [24, 25], and direct regression of footsteps [26].

Recurrent Neural Networks (RNNs) are an interesting architecture for this problem as they naturally handle the sequential, discrete-time nature of the problem. While RNNs have been used in policies for blind low-level control of legged robots [27] and in other planning domains [28, 29], to the authors’ knowledge this is the first application of RNNs to the footstep planning problem. Our framework has a number of advantages over other learned footstep planners: no terrain heuristics are used for training, the plan can be arbitrarily long up to the visual horizon, multimodality is handled by outputting a probability density map over all possible step locations, and it can be used as a sub-module in a traditional sampling planner.

For our work, we consider the classic planar Spring Loaded Inverted Pendulum (SLIP) hopper [30]: a one legged machine modeled as a massless, spring-loaded leg and foot attached to a point mass body. This model is used as a template [31] to plan trajectories of higher degree of freedom robots such as bipeds [32].

The SLIP hopper is a hybrid system with two modes. When the hopper is in the flight mode, the foot is not touching the ground and the only force experienced by the body is gravity. When the hopper is in the stance mode, the foot is in contact with the ground and the body experiences gravity and a spring force acting through the leg. The instantaneous transition from stance to flight is referred to as liftoff, and the transition from flight to stance is touchdown.

The passive SLIP model has one degree of freedom: the angle of the leg with respect to the ground. This input is applied at the apex of flight, where the vertical velocity is 0, and will affect the body’s trajectory during the following stance phase by controlling the tradeoff between apex height and forward velocity. The touchdown transition occurs when the foot makes contact with the ground. A prototypical trajectory for the SLIP hopper is shown in Figure 2. For the full dynamical equations, we refer the reader to [33], whose formulation we used for this work.

An important property of the SLIP model is that there is no closed form solution for the stance phase dynamics, requiring either an approximation or explicit forward integration in order to calculate the next state after applying a control input [34].

Although the passive SLIP models are relatively simplistic, there are a number of failure cases that make planning for the SLIP hopper nontrivial over broken terrain. The first is the standard kinematic case where the body collides with the terrain, or the foot collides with a vertical surface. Second, the leg angle can exit the friction cone during stance phase, causing the foot to slip. Third, if the hopper’s apex height is too high above the terrain, the spring will fully compress during touchdown and will bottom-out, also causing a crash. Finally, because the energy of the passive SLIP model is fixed, there is an inverse relationship between the apex velocity and apex height. If the hopper travels too fast, the apex height will not be sufficient for the foot to swing to its position for the next step, causing the robot to ”trip” on the ground.

The intersection of the kinematically feasible and dynamically reachable sets of footsteps is not immediately apparent just by analyzing the terrain. Therefore, when planning a trajectory, one must either precompute a set of feasible motion primitives for varying terrains, perform search by sweeping over control inputs and simulating the dynamics, or solve a nonlinear optimization problem.

We formulate our first baseline planner as a best-first search over a tree that is dynamically constructed by sampling the possible actions at each leaf, adapted from the approach in [4]. Each node $N$ is a tuple $(a,x,\theta,P,c,\{C_{1}...C_{N}\})$, where $a$ is the apex state at which the leg angle $\theta$ is applied to reach $x$, the footprint of the CoM at the start of the subsequent stance phase. The other terms contain information about the graph structure. $P$ is the parent of $N$, $c$ is the cost of $N$, and $\{C_{1}...C_{N}\}$ is the list of the children of $N$.

Our search algorithm begins with a root node, which is the inevitable touchdown point from the initial apex. From that node, we sample all the available actions and simulate the dynamics equations. If an action does not result in one of the failure cases outlined in Section IV-B, a node corresponding to the current apex state and subsequent touchdown location is added to the tree as a leaf. Afterwards, the node with the lowest cost is fetched from a priority queue and expanded. This process repeats until a desired number of nodes exceed the goal distance. We refer to this planner as the ”Angle-space” planner. Pseudocode for this algorithm is shown in Algorithm 1.

To evaluate the cost of each transition, we use a function that takes the child node as input. This cost function is formulated as

Where $x_{i}$ is the node $N_{i}$’s location and $x^{\prime}$ is the goal location. This cost function penalizes the distance of the node to the goal, high node apex velocity, and the node’s ”isolation”. $D(N_{i-1},N_{i},N_{i+1})$ is a function calculating the spread between the nearest neighbors of $N_{i}$. If the $N_{i}$ obtained by applying $\theta_{i}$ is far away from from its closest fellow child, that means a small deviation from $\theta_{i}$ is more likely to cause a crash in the presence of obstacles. $(w_{1},w_{2},w_{3})$ are the relative weights of these terms.

While the planner shown in Algorithm 1 also produces the input angles necessary to hit the footstep sequence it outputs, it is inadvisable to use these in practice, since the open-loop angles will be incorrect as soon as there is any deviation from the reference sequence. Our approach is similar in spirit to [35]; we train a simple Multi-Layer Perceptron (MLP) that is called once per apex. The training data for the MLP is generated by gridding over the space of current velocity $\dot{x}$, height $z$, and leg angle $\theta$, and running a simulation to obtain the resulting step length $x_{L}$. Then, $\dot{x},\,z,\,\text{and}\,x_{L}$ are fed as inputs to the MLP and the output is the required leg angle $\theta$.

Naturally, the learned controller does not perfectly achieve the desired step lengths. It can produce errors of up to 0.5m on our simulated robot, since at high velocities small angle errors can cause large position errors.

One fault of the planner shown in Algorithm 1 is that it has no knowledge of the underlying controller and will output sequences that are physically achievable by the robot, but may cause significant errors if provided as inputs to the actual system consisting of an imperfect controller composed with the dynamics.

To incorporate the controller model, we alter the sampling procedure used in line 6 of Algorithm 1. Instead of directly generating leg angles, we instead generate a grid of possible step lengths ${x_{L}}$, pass those with the current apex state into the controller to obtain the desired $\theta$, and use it in the planning algorithm. The branching factor for this variant corresponds to the size and spacing of the grid ${x_{L}}$. The final sequence output by this modified planner uses the sampled step lengths $x_{L}$, as those feedforward inputs are known to result in feasible actions when input to the controller. This approach has the advantage that steps that encounter terrain obstacles can be avoided a priori, but requires a model for the low-level controller which is not always known in practice. We refer to this modification as the ”Step-space planner”.

We can also modify the Step-space planner to sample next footsteps from the output probability distribution of the LSTM instead of from a fixed grid. To generate these samples, we fetch the path from the root node to the current node, and pass the path into the LSTM as the past steps, along with the initial apex and terrain.

Sampling from the output of the LSTM allows us to reduce the required branching factor of the Step-space planner by sampling from an informed distribution, while also incorporating a model of the controller into the LSTM planner. We use temperature scaling [38] to widen the output distribution and make it more suitable for sampling.

We generate training data for our LSTM by running the Angle-space planner with the parameters described in Table I. A single run of the Angle-space planner can give multiple training sequences as any path from the root node to a leaf is a feasible sequence of footsteps. In order to still provide reasonably good training sequences, we only add sequences that make a certain amount of progress towards the goal to the training set. We use the Angle-space planner instead of the Step-space planner to prevent the LSTM from overfitting to the performance of any particular low-level controller.

Each terrain is either a ditches or steps scenario. We model the terrain 8 meters in front of the robot and 3 meters behind, discretized in segments of 10cm. We also use a lower coefficient of friction during training in order to encourage further robustness by the LSTM. This process generates roughly 50,000 sequences of varying lengths (although a few of the sequences on the same terrain are usually highly similar to each other) and takes 1.5 hours with multithreading.

Our testing data consists of 15 random ditch-world and step-world terrains coupled with 5 different initial states swept over varying initial velocities (150 total test cases). Given that the train and test sets use completely random terrains and initial states, it is highly unlikely that any of the test terrains appear in the training set. The coefficient of friction used for all the test experiments is 0.8. Similar to training, the goal for the robot is to cross a certain $x$ threshold, set to 10 meters.

We test 5 different planners: the Angle-space planner, Step-space planner, LSTM planner, LSTM-Guided sampling planner, and a terrain-heuristic planner. The terrain-heuristic planner is meant to mimic the approaches of [24, 25], where steps are selected based on a nominal gait dependent on the initial apex velocity, and then the planned steps are moved if they land in ditches. All planners are tested in a receding-horizon fashion, where the planned sequence is executed by the low-level controller for 3 steps before replanning occurs.

The success rate of all 5 planners on the ditch and step world scenarios are shown in Tables III and III. In these results, the Angle-space planner is used with an initial branching factor of 20 with a fallback branching factor of 30 if the first try fails. The Step-space planner is used with a horizon of 5m and spacing of 0.5 meters (branching factor of 10), and the LSTM-Guided planner is used with a branching factor of 3 and fallback factor of 5.

We can see that the Step-space and LSTM-Guided planners have the best overall performance, succeeding on over 80% of the terrains from both suites. This makes intuitive sense as they incorporate the controller’s behavior into the planner. The pure LSTM planner performs slightly worse than the baseline Angle-space planner in both the ditches and steps scenarios; at low velocities, it tends to suggest steps that either cause the controller to violate the friction cone or that are very close to the edges of ditches and steps. However, the high performance of the LSTM-Guided sampler shows that the LSTM still suggests a high-quality probability distribution over possible locations. The heuristic planner sometimes performs well but is highly sensitive to variations in its parameters.

Tables III and III show that the LSTM-Guided planner achieves high success rates while making fewer than 1/6 the number of ODE calls than the Step-space planner, and fewer than 1/4 of the Angle-space planner. Since it guides sampling to a few areas, the LSTM-Guided planner has to explore fewer nodes to find a feasible plan, and so has a planning time of 0.04s on average, compared to 0.3s for the Step-space planner.

Figure 4 shows 4-step plans and the probability distributions for the second step generated by the LSTM over 3 different initial apex states. As we increase both the apex height and initial velocity, the LSTM alters its plan accordingly to traverse the terrain. On the ditch terrain, the peaks of the distribution for the second step move further away as the initial energy increases. On the step terrain, when starting with too high an initial velocity it shortens its first two steps due to the tradeoff mentioned in Section IV-B. In the 3rd subfigure in Figure 4b, with too high an initial velocity, it will not be able to generate the necessary height to clear the obstacles, so it takes a short step to slow down.

The LSTM planner does have a number of shortcomings. Although it takes noticeably shorter steps when the hopper has lower energy, it still tends to propose more aggressive plans than the Angle-space planner. In addition, many of the pathological behaviors present in the training data also appear in the learned sequences, including stepping very close to edges, as seen in Figure 4b. In Figure 4a, there are spikes in the probability distribution around the edges of the islands, corresponding to training sequences where the Angle-space planner selects the node that minimizes the distance to the goal. This occurs in part because we chose not to manually bias the planner using terrain heuristics.