Footstep Planning for Autonomous Walking Over Rough Terrain

TL;DR

This paper introduces a new A* footstep planner for humanoid robots that efficiently plans footsteps over rough terrain using partial footholds and post-processing to improve solutions, demonstrated on Atlas and Valkyrie robots.

Contribution

The paper presents a novel A* based footstep planning algorithm that incorporates partial footholds and solution refinement for complex terrains.

Findings

Effective planning over rough terrain demonstrated on real robots.

Use of partial footholds increases available foothold options.

Post-processing improves solution quality.

Abstract

To increase the speed of operation and reduce operator burden, humanoid robots must be able to function autonomously, even in complex, cluttered environments. For this to be possible, they must be able to quickly and efficiently compute desired footsteps to reach a goal. In this work, we present a new A* footstep planner that utilizes a planar region representation of the environment enable footstep planning over rough terrain. To increase the number of available footholds, we present an approach to allow the use of partial footholds during the planning process. The footstep plan solutions are then post-processed to capture better solutions that lie between the lattice discretization of the footstep graph. We then demonstrate this planner over a variety of virtual and real world environments, including some that require partial footholds and rough terrain using the Atlas and Valkyrie humanoid robots.

Tables

Table 1. TABLE I: Footstep Planner Results. This shows the statistics of the plans presented in Figure 11 .

Plan	Number	Plan	Planning	Nodes	Percent
	of Steps	Distance (m)	Duration (s)	Expanded	Rejected
(a)	8	3.05	0.11	27	77.4
(b)	11	3.85	0.27	119	82.5
(c)	30	11.70	1.97	54	75.2
(d)	11	4.02	0.856	11	54.5
(e)	23	8.29	1.89	95	57.0

Equations

$${\mathbf{A}}_p{\mathbf{x}}_i\le {\mathbf{b}}_p(d),\forall i$$

$${\mathbf{x}}_i={\mathbf{r}}_c+{\mathbf{r}}_{p,i}+\mathbf{Jq},$$

$$\mathbf{J}=\left[\begin{array}[]{ccc}1&0&-v_{y,i}\\ 0&1&v_{x,i}\end{array}\right],\ \ \ \mathbf{q}=\left[\mathbf{v}^{T}\ \theta\right]^{T}.$$

$$\begin{array}[]{rl}\min_{\mathbf{q}}&\mathbf{q}^{T}\mathbf{Qq}\\ \text{subject to}&\mathbf{A}_{p}\mathbf{Jq}\leq\mathbf{b}_{p}(d)-\mathbf{A}_{p}\left(\mathbf{r}_{c}+\mathbf{r}_{p,i}\right),\ \forall i\\ &\mathbf{q}_{\text{min}}\leq\mathbf{q}\leq\mathbf{q}_{\text{max}},\end{array}$$

Full text

I Introduction

One of the main developmental justifications for humanoid robots is their potential for incredible mobility. Humans can run, swim, climb, and access arguably more environments than any other terrestrial creature of their size on the planet. While the mobility of humanoid robots has significantly improved, allowing them to walk quickly and robustly, they are still fairly limited as to their ability to handle rough terrain. Rough terrain for robots has a sparse, limited number of footholds, with large, discrete height changes and varied surface normals. This requires the robot to use accurate foot placement in order to reach the goal. Navigating over rough terrain is a key skill for humanoid robots to function in many of their desired operating environments, and highlights their distinct mobility capabilities. Manually placing the desired footsteps, though, is cognitively challenging and requires significant time, even for a skilled operator. To improve the capability and usability of humanoid robots, the robot should reliably determine its own foot placement, allowing it to navigate autonomously, even in complex environments.

In this work, we focus on the development of a footstep planner with a specific focus on handling rough terrain. The planning of feasible footholds lends itself nicely to graph-search approaches with a rich history[1, 2, 3, 4, 5, 6], as considering a continuum of possible step locations is not explicitly required. Provided a properly tuned cost-to-go heuristic, planners based on graph-search can quickly converge to a feasible solution, and lend themselves nicely to anytime approaches, providing partial solutions when there is not sufficient time to plan the entire contact sequence. This is critical for real-world use by autonomous robots, where plans are needed on the order of fractions of seconds to prevent planning slow-downs and to adapt to dynamic environments. Additionally, graph-search works well for nonlinear constraints and costs, where node validation is simply a binary function and cost function gradients are not used. Lastly, graph-search is well suited to problems where the number of decision variables (in this case, footsteps) is unknown, making it very well suited to path planning over rough terrain, where the number of footsteps required can be very hard to predict.

Here we present a novel Weighted A* footstep planner designed to handle 3D obstacles and terrain, using an efficient representation of the environment in the form of planar regions. There have been many quite successful and thorough implementations of footstep planners in the past, including those with impressive, fielded results[7, 6, 8]. Our focus is on the development of a planner to handle terrain with varying surface normals, large gaps, and height changes. Uniquely, our planner allows the use of partial footholds, something we believe is required to expand the valid action set for the robot to usable size, and something that has not been demonstrated before to our knowledge. We utilize a novel approach for pruning the possible action tree using a planar region representation of the environment. We also present a method for post-processing the resulting plans to address cases where a more reliable foothold exists between the lattice nodes of the graph, away from edges in the world, but is typically missed due to the discretization.

To validate our planning approach, we present several experiments using the Valkyrie humanoid by NASA Johnson Space Center and the DRC Atlas by Boston Dynamics. The experiments include walking over flat ground and avoiding dynamic obstacles, stepping stones, balance beams, and rough terrain.

II Related Work

As robots have been gaining in capability, so too has the work on footstep planners been gaining interest. Historically, many footstep planners have used graph-search techniques. One of the first uses of A* for footstep planning was presented in[3, 4], demonstrating its viability for planning specific contact sequences for obstacle avoidance over flat ground. The capabilities of different graph-search based planners were later compared, including an A*, ARA*, and R* planners, using several different guiding heuristics[2], where it was found that R* planners are often faster, but may not produce feasible results, as expected. More recently, a traversibility metric has been used to guide the expansion of ANA* planners to help accelerate convergence for multi-contact planning[9]. Additionally, it has been shown that by including an estimated dynamics edge cost, the resulting plans can better adapt to the demands of different environments on the center of mass (CoM) dynamics[10]. A planar region segmentation of the environment has also been used to extract a desired 2D body path plan for the robot, and directly used to compute the desired footholds by assuming flat ground[8]. This is in contrast to the same authors’ other work, which used an A* algorithm that uses an adaptive footstep expansion set to keep the search-space relatively small so that solutions are found quickly[11], using the same planar region representation as in[8].

Recently searching over continuous space rather than over a discrete graph has been gaining interest. One of the earlier works of actually fielded continuous space algorithms used a mixed integer quadratic program (MIQP) to compute footstep plans[7]. Like in [8], this approach requires a smart segmentation of the environment into convex regions in order to be tractable. Other planners that plan both the contact sequence and the overall motions include CHOMP [12], which uses covariant gradient descent to optimize the motions and TOWR [13], which generates contacts and trajectories using a parameterization of end-effector and CoM motions. An interesting combination of the two styles is presented in[14], which optimizes the desired end-effector locations, including knowledge of the resulting CoM dynamics using a MIQP.

Another common approach has been to compute footstep plans via pattern generators. However, these applications have typically been limited to flat ground. One of the first examples utilized a model predictive controller to generate stable walking motions, including footsteps[15]. A similar approach reformulated the problem into a nonlinear optimization to include collision avoidance[16]. One of the most recent and promising results in using pattern generation performs multi-contact planning in two stages: first reachability planning from the body path, then planning contacts using static equilibrium, both with a bi-RRT approach, which was used to generate dynamic motions with a 3D pattern generator. This approach was capable of generating results fast enough to plan at each robot step over short traverses[17].

III Weighted A* Footstep Planning

The underlying algorithm comprising our footstep planner uses a Weighted A* approach[18]. We break this search into the four main steps described below: node expansion, node snapping, edge checking, and edge scoring. We also include a very brief overview of our planar region representation framework, as this is essential to the node snapping and edge checking step steps.

III-A Node and Graph Structure

We define each footstep node in the A* graph as a unique set of $x$, $y$, and yaw positions and robot side, making it a three dimensional structure, with edges connecting two nodes of opposite sides. The grid is discretized into two sizes, one for $x$ and $y$, and one for yaw. The action graph can be reduced from the $\mathbb{R}^{6}$ footstep pose to these three values as they fully define the pose with the foot height, pitch, and roll being constrained by the environment. In this work, we use a $x$-$y$ grid size of $5\text{\,}\mathrm{cm}$ and a yaw size of [math].

III-B Algorithm Overview

All the leaves of the action tree, or ending nodes of footstep graph, are stored in a priority queue, where the node with the lowest total estimated cost is first in the queue, representing the ending nodes in the graph. The node is then removed from the queue and expanded to determine the possible actions. These children nodes are then checked, and if feasible, are scored and added to the graph, resolving any loop closures where necessary by picking the less expensive parent node. If one of the children nodes equals a goal node, the algorithm is halted, as a solution has been found. This process is described in Algorithm 1:

III-C Node Expansion

The first step of every iteration is to expand the lowest cost node to determine the feasible actions, where the cost is both the cost of the path to that point, plus the estimated cost-to-go to the goal. To determine the set of possible actions, we define a reachability box in the frame of the parent node, as shown in Figure 2. Then, we simply add a node to each vertex inside this reachability, shown on the right in Figure 2, including all possible yaws at each location, as in the left of Figure 2. We perform a brief check on total Euclidean distance, eliminating nodes that are too far from the base node. For the proposed experiments, this results in approximately 600 children nodes for each iteration. For a more in-depth overview of how the node expansion step can be modified to increase the search-speed, see[11].

III-D Planar Region Representation

One of the most important aspects for planning footsteps over complex terrain is the representation of the environment in an efficient manner for the planner. As in[8, 19, 20], we decompose the environment into a set of planar regions. Planar regions can represent a huge variety of surfaces, from estimating flat gravel as a single, large region, to a cylindrical drum as a tessellated set of regions. Each planar surface is then deconstructed into a set of convex regions, providing a highly efficient, low order, convex representation of the world, shown in Figure 3. We have found this to be useful and effective for footstep planning, allowing the exploitation of simple Euclidean geometry for most of the calculations.

III-E Node Snapping

To evaluate the edge validity, the map of each node in the 3D footstep graph to the full 6D Euclidean footstep pose must be determined. To do this, we simply “snap” the node to the planar region, as illustrated in Figure 4. However, as a foothold is not a simple point but is in fact a full polygon, we must snap the entire polygon to the world. To calculate the snap, we project the foot polygon down onto each successive planar region, then keep the transform that results in the overall highest vertex in the world, as this point must be on top.

III-F Edge Checking

To test the validity of each possible edge in the footstep graph, that is each connection between a parent and potential child node, we run through a series of checks on the child node itself and the total connection, some of which are shown in Figure 5. First is verifying that the footstep polygon can be snapped to a planar region. We then check that the normal of the planar region is below a certain incline, as in Figure 5(a). We also check that the snapped polygon contains enough intersecting area with the world, verifying that the foot has enough contact with planar region. By allowing partial footholds we can greatly increase the number of available footholds, such as in Figure 5(b), as many nodes that are near the edge result in the foot slightly hanging off, but are still feasible. We additionally check that the step is properly positioned with respect to the stance foot, and that the step isn’t too high or too low, as shown in Figure 5(c). For rough terrain, we also check that the foot isn’t too close to the base of a cliff, or a high region in the world like a step, as shown in Figure 5(d).

We also verify that the robot is not attempting to step over an obstacle that is too high, as shown in Figure 6, left. This is done by building a planar region that spans the two feet at a specified height. We also build a bounding box around the midstance of the foot, similar to[7] with fewer collision boxes. While this one box approach is somewhat limiting, the fewer boxes greatly reduces the number of operations performed on each node in the search, which is of greater concern for graph-search type approaches than a MIQP-type approach. This convex polytope is then checked for collisions with the world regions.

III-G Edge Scoring

In an A* algorithm, the total cost of the path is sum of the cost of each edge along the path. However, to help guide the expansion to increase efficiency, a cost-to-go estimate is also included, trying to estimate the total cost of the remaining distance from the current node to the goal. The best node to expand, then, is the node with the sum of the lowest total path cost and the remaining cost estimate. Here, we describe our path cost metrics and our cost-to-go heuristic.

III-G1 Step Cost

The primary cost of the path planning problem is to minimize the total distance. To determine the distance cost, we use the distance between the nominal mid-stance poses, as shown in the left of Figure 7. We then add a cost for the height change and the yaw change of the step. We lastly evaluate the “quality” of the foothold, using metrics including percent area of the foothold and the required foot roll and pitch. To try and minimize the number of steps taken, a static cost per step is also included.

While more complex cost functions that increase the cost of yaw as the step length is increased are possible, we have not found them necessary. The influence of each of these cost terms on the resulting path taken can be varied by altering the weight on each cost term. Additionally, so that we can request an anytime solution, we keep track of the ongoing best path, by checking to see if the cost of each new node evaluated is less than the current best node, allowing the planner to operate in a “best effort” mode.

III-G2 Cost-to-Go Heuristic

The selection of an appropriate heuristic cost-to-go estimate is critical to an efficient, fast search. The role of the heuristic is to estimate the total remaining cost from each individual node to the goal, to predict the best remaining path. We model our heuristic assuming the robot will walk straight towards the goal, and then start turning near the end. We compute a reference desired orientation that changes accordingly, driving the robot towards the desired heading until it gets close to the goal, where it blends towards the final goal orientation. This is shown by the arrows in Figure 8, which point towards the goal until the end, where they start to match the goal yaw. We then estimate the cost-to-go as the total Euclidean distance and rotation plus the minimum number of steps, multiplied by an inflation weight.

III-H Goal Evaluation and Anytime Planning

If, during the expansion step, one of the child nodes is equal to either one of the goal nodes (either side), a valid path has been found. If the heuristic cost has been inflated, this path is ultimately suboptimal. Note that an ARA* planner would then continue to search by reducing the heuristic inflation and looking for an optimal solution. While this is an area of interest for future work, we are more interested in finding fast, feasible, near-optimal solutions in this work, which can be provided with a low heuristic inflation.

If the search times out before a solution is found, the “best effort” path can be returned. In this case, the best effort path is calculated as the path to the lowest cost node that is evaluated in the Edge Scoring step, and can be returned at any time. This can be particularly useful when navigating in areas where dynamic obstacles may be present, so that the footstep plan can be continuously updated in response to these obstacles. It also allows a plan to be found, even if the goal node is not feasible, such as being out of the robot’s current line of sight.

IV Edge Avoidance by Partial Foothold Wiggling

One of the disadvantages of using a graph-search based planning approach is the risk that, due to the graph discretization, many possible solutions are ignored. This becomes a particular problem when trying to avoid stepping near the edge of a planar region. For many reasons, it is desirable to not step near the edge of regions, including tracking error, slip, and support shifting. Additionally, when possible it is preferable to maximize the foothold area, rather than using a partial foothold. For example, in Figure 9, we see that a foothold that aligns with the grid may be over the edge of a region. However, if this foothold is shifted between nodes on the lattice, it isn’t as far inside as the obviously valid nodes, but is far enough inside to still be a very good choice. If relying purely on the footstep graph, though, this would never be a considered solution. As a more rare edge case, if we simply invalidate nodes that are too close to the edge, we may end up without any valid footstep nodes. For example, see Figure 10. In this example, the area of good footholds is shown by the inner blue bound. As shown, there are no valid nodes that are within this blue region, with the four nodes in the planar region being the red circles.

Instead of invalidating nodes that are too close to the edge, we can choose to only invalidate nodes that don’t have enough area, and shift them to be a desired distance inside the region in a post-processing step. This will allow nodes near the edge that could result in full footholds, like in Figure 9, and allow using area that would have no viable footholds otherwise, like in Figure 10. To do this post processing, we can set up an optimization problem for each node in the path where we constrain the foot to be a certain distance inside the region polygon while minimizing the total translation and rotation. This has the effect of shifting the foot inside the polygon, as shown in Figure 9.

To set up the optimization problem, we first define a constraint where all the points $\mathbf{x}_{i}\in\mathbb{R}^{2}$ that make up the footstep polygon are inside the polygon that defines the planar region, $\mathbf{P}_{p}$, by distance $d$. This can be done by requiring the inequality constraint

[TABLE]

be satisfied, where $\mathbf{A}_{p}\in\mathbb{R}^{n\times 2}$ and $n$ is the number of edges in polygon $\mathbf{P}_{p}$. The objective, then, is to find the a translation, $\mathbf{v}$, and rotation, $\theta$, of the step such that Equation 1 holds.

If we define the centroid of the foot polygon as $\mathbf{r}_{c}$, the vector from the centroid to the $i^{th}$ foot polygon vertex is $\mathbf{r}_{p,i}$. Assuming that $\theta$ remains small, the location of the vertex after translation and rotation is then

[TABLE]

where

[TABLE]

This allows us to then write our optimization problem as a quadratic program with 3 decision variables,

[TABLE]

where $\mathbf{Q}$ is a diagonal positive definite weighting matrix.

To keep this consistent with the node structure of the graph search, we must ensure that the desired distance inside $d$ is less than the discretization size. Otherwise, a different node should be chosen, such as selecting a node where the foothold would already be fully within the region in Figure 9. That is, if achieving the desired distance inside the planar region requires moving a node past another valid node, that second node is the better option.

We execute this post-processing step on each of the nodes in the footstep planner solution. This does mean that nodes can end up being inconsistent with the reachability limits imposed in subsection III-F, as nodes on the bounds of the reachability can be shifted slightly outside of it. However, as long as the maximum allowed shift distance is small (in our case, $2\text{\,}\mathrm{cm}$), this is not of much concern.

V Results

To aid in the development and testing of the footstep planner, we generated many 2D and 3D environments using planar regions, some of which are shown in Figure 11. For many of the 3D environments, such as Figure 11(c),(d), and (e), we ran a virtual LIDAR sensor over the terrain, generating planar regions using the approach outlined in subsection III-D, to better represent actual robot data, which is imperfect, particularly in edge detection. We then specified a start and goal pose of the robot, shown by the green and red spheres in Figure 11, respectively. For the results presented in Figure 11 we use the same parameters that we use on the Atlas hardware in Figure 1. As can be seen, this results in a variety of quality plans, with the resulting data being shown in Table I. The times presented are generated using a desktop with a 4th gen 4-core i7 processor. As expected, as the number of required steps to reach the goal increases, the planning duration increases correspondingly. It is worth noting that a large percentage of the nodes expanded on each iteration are found to be invalid, including 15% rejected when planning on flat ground. This suggests that if a smarter node expansion, such as that presented in [11], were used, the planning time could be correspondingly reduced. One of the advantages, however, to having rough terrain where many nodes are found to be invalid is that the search-space of the problem is greatly reduced, leading to much faster results.

To illustrate the advantages of allowing partial footholds, we set up an environment consisting of two platforms spanned by a 4” beam, which was more narrow that the width of the foot, shown in Figure 11(a). The reachability of the robot was also limited so that it could not put one foot directly in front of the other, requiring each step to be offset slight from the center of the beam. If partial footholds were not allowed, the planner would not have been able to find solutions for Figure 11(b) either, which also required foothold cropping.

We also explored the planner’s capability at finding more advantages plans that require the robot to adjust its body using the bounding box collision model in Figure 6 in Figure 12. In this environment, the robot had to turn sideways to squeeze between two vertical obstacles. As expected, this requires a significantly longer planning time, expanding many nodes near the turn before the robot attempts to turn sideways, particularly if the heuristic inflation is high.

We extensively tested this planner on hardware using Atlas and Valkyrie. To model the environment, we used the approach briefly outlined in subsection III-D. Several of the environments from Figure 11 were directly reproduced for the robot, as shown in Figure 13. For the results shown in Figure 1, we were able to achieve a relatively high success rate of approximately 90%, with most failures due to balance errors, rather than planning errors. One of the challenges that was noticed was the robot would often try and execute plans that would result in it stepping on its own feet, requiring the introduction and subsequent increase of the “no-go” area shown in Figure 5(c). Additionally, to prevent the robot from taking steps that were both high or low and far forward or wide, we introduced a maximum length and width for steps that were above or below a certain distance. For Figure 13, our results were a little less reliable, achieving only about a 50% success rate, although this was again primarily due to balance errors. Additionally, for these examples, we had to reduce the required area for acceptance of the foothold to 70%.

Lastly, to test the anytime capabilities of the planner, we set up an environment where obstacles were put in between Valkyrie and the goal, requiring both their detection and rapid replanning to avoid collisions, as shown in Figure 14. In this experiment, the robot was told to plan to the package. As it approached, an obstacle was placed in front of it, causing the robot to replan to its left. Then another obstacle was placed in front of the robot, causing it to back up and plan to the right, eventually reaching the goal.

VI Discussion and Future Work

Several state-of-the-art approaches are moving towards a hierarchical/multi-stage planning approach, where the search-space of the low-level contact sequence planning is reduced through the use of a guiding heuristic plan or body path plan that can be found very quickly. We believe that this type of hierarchy is not only an excellent method to increase planning speed, but is also more in line with nature, where the full contact sequence is not planned far in advance[21]. We believe that by combining this type of short-horizon planning with our Weighted A* approach, we can get plans at a realtime rate, without having to resort to methods such as the adaptive action set described in[11], despite its excellent merits. Additionally, environments such as that shown in Figure 12 can be challenging for A* search planners, as too high of a heuristic weight can cause the planner to get “stuck”, only selecting nodes to expand that are close to the goal. Indeed, the proverbial cul-de-sac problem can cause the planner to exhaustively check all the possible nodes in the cul-de-sac before continuing its search around. We believe that the inclusion of a body path as a guiding heuristic will allow this type of environment to be solved very quickly, which address these type of problems well[8].

One of the areas our approach is most lacking is a consideration of the CoM dynamics, as in[10]. While it is unclear if the learned-type dynamic cost prediction presented in that work is necessarily the best approach, it is certainly promising for producing plans that are dynamically variable to the environment structure. It is likely that, as our algorithm is expanded to consider multi-contact motions, some kind of dynamic modeling will be required to weight between the contributions of hand vs. foot contacts.

We are also exploring modifying our existing Weighted A* approach to a full Anytime Repairing A* algorithm to continually improve the resulting plans once one is found[22].

VI-A Source Code and Media

Our implementation of our planar region segmentation algorithm, our footstep planner, and our walking controller can be found on our GitHub page, https://github.com/ihmcrobotics. The accompanying video can be found at https://youtu.be/PqLZP8TANlg.