Real-time optimal navigation planning using learned motion costs

In short: a legged robot should pick routes by what its own legs can do — how much effort, how long, how likely to fall — not by distance alone.

Legged robots have become very capable on rough ground. But being able to cross something is not the same as it being a good idea. The paper points out that the robot can end up with different outcomes and risks on different ground, and that these depend strongly on which walking controller it runs. A good route is smooth and sensible, with little travel expense and little chance of failure.

Classic navigation either marks each patch of ground as traversable or not, and then treats the planning cost as distance. For a quadruped facing stairs, steps and clutter, that throws away exactly the information that matters. The planner in this paper instead plans on an elevation map — a grid of ground heights — with a cost that was learned from the robot’s own controller, so it is aware of the robot’s locomotion capabilities.

Scene 1 is a made-up world to show the idea: the terrain, the stumble and the arrival times are illustrative.

In short: earlier work already learned good motion costs, but planning with them took minutes, because the network was asked about one candidate move at a time.

The paper builds on two lines of earlier work. One writes the cost by hand: formulas that score each foothold from terrain features (Wermelinger et al., 2016). That works, but needs strong expertise for each robot, and gets harder as terrain grows more complex or new cost factors like energy and time are added. The other learns the cost: Chavez-Garcia et al. (2018) trained a network in simulation to judge traversability, and Guzzi et al. (2020) predicted several motion costs of short moves and planned with RRT* and a related planner.

The trouble is speed. Sampling planners like RRT* add one random move at a time, and each new move needs a fresh query to the neural network. Getting a smooth, feasible path takes many moves, so planning takes long — too long to re-plan while the robot walks. Other approaches that learn a whole planning policy (from human demonstrations, imitation or reinforcement learning) need many demonstrations, rely on another planner, or have been shown mainly on flat ground.

This paper’s answer is to change how the planner asks. It uses a fixed set of moves on a grid, so all of them can be prepared as one block of numbers and scored by the graphics card (GPU) in one go, and then refines the result with a fast optimizer.

In short: the robot’s real controller tries hundreds of thousands of short moves in simulation; a network learns to predict each move’s energy, time and risk of failing from the local ground shape.

What is a “move”?

The robot’s state is its position and heading on the map, (x, y, ψ). A short move goes from one state to another and is described to the network as (Δx, Δy, Δψ, ψ) — how far it goes sideways and forward, how much it turns, and which way it faced at the start. The ground around it is a height scan of 2 m × 2 m at 4 cm resolution, centred on the robot.

Collecting the data

ANYmal C was simulated in the Raisim physics engine with a learned walking controller (Lee et al., 2020) that can cross challenging terrain. For each sample, a random move of up to 0.5 m and any turn was commanded (with at least 10° of turning when the move is very short). Each move was repeated 12 times, with tiny changes to the terrain and a random foot friction between 0.75 and 0.80. The average energy and time of the successful tries, and the share of tries that failed (the risk, 0 to 1), became the labels.

In total 370k samples were collected on randomly generated terrain: stairs, slopes and steps of different sizes, irregular ground made from layered Perlin noise, and narrow paths 0.5–2.0 m wide. The narrow paths make up 36 % of the data, and a third of those moves simply walk along the path, so the network sees many useful examples of squeezing through. A few centimetres of noise on every terrain stand in for real-world roughness and mapping errors.

The network

The predictor has two parts, trained together with a mean-squared-error loss on 80 % of the data (20 % held out). A feature extractor made of convolution layers turns the height map into terrain features; a small cost predictor of fully connected layers takes the features at the robot’s location plus the move command and outputs the three costs c_E, c_T, c_R.

The key design choice for speed: because the feature extractor is convolutional, it can run once over the whole map. After that, scoring any move is just a lookup of local features plus a tiny network — and thousands of those can be batched on the GPU.

In short: instead of random samples, the planner scores a fixed grid of moves in one GPU batch, adds slightly jiggled copies so narrow gaps are not missed, and finds a route with A*.

The planner is a modified probabilistic roadmap (PRM): a graph of positions joined by moves. Normally the positions are random. Here they are a fixed grid of 50 × 50 points, 0.2 m apart. From each point the planner considers moves to its 20 neighbours, from 16 different headings. To keep it fast, the robot’s heading for each move is simply the direction of the move. Because the locations and lengths of all moves are known in advance, they can be packed into one batch and sent to the GPU after each map update.

Each move gets a connection and a price:

A classic search, A*, then finds the cheapest route through this roadmap. Its guess of the remaining cost is one tenth of the straight-line distance to the goal, which matches the cost of walking on flat ground with no risk.

Vague sampling

A fixed grid has a weakness the paper names directly: it is probabilistically incomplete. If a narrow passage sits between grid lines, every grid move through it may clip an edge and look risky, and the planner finds no route. The fix is vague sampling: each move is also scored as 10 copies shifted by up to ±0.1 m and turned by up to ±0.4 rad. If the safest copy is below the risk threshold, the move counts as connected.

Two details keep this cheap and cautious. The copies add no new nodes to the graph — they only decide whether an existing road is open — so the search stays as fast as before. And the road keeps the price of the original move, not the lower risk of its best copy, so A* still prefers safer roads and only uses a “rescued” one when needed. In the paper’s setting this adds 500k copies, 11× the number of scored moves.

In short: the grid route is safe but jagged; an optimizer moves every waypoint a little at a time toward lower cost, giving a smooth, cheaper path in about 50 rounds.

The optimizer takes the raw path from A* and treats the position and heading of every waypoint between start and goal as adjustable. It scores the whole path with one number:

The sum adds up the energy and time of every move. The first term is worth a second look: the path’s risk is the risk of its single riskiest move, multiplied by the number of moves — one bad step is enough to fall. A penalty p_d = ω_p·d² keeps moves from getting longer than the network was trained for; moves too small to predict are ignored. The weights are the planner’s (5, 5, 100) and ω_p = 10.

How does it know which way is “downhill”? The network could in principle give exact gradients by back-propagation, but the authors found finite differences faster in practice: shift a waypoint by ±0.08 m (or turn it by ±0.05 rad), re-score the two moves that touch it, and divide the change in cost by the shift. The update itself uses Adam, which adapts the step size for each waypoint separately — handy because raw paths have different numbers of nodes. The learning rate starts at 0.16 and shrinks by a factor 0.96 per round. Usually 50 rounds give a feasible path.

In short: with the same learned costs, the new planner finds paths about half as costly as RRT*, in about half a second instead of over two minutes.

The test maps are 12 m × 12 m at 4 cm resolution; the planning area is 10 m × 10 m (8 cm after the convolution layers). All planners use the same cost network on a desktop computer (AMD Ryzen 9 3950X, Nvidia GeForce GTX 970) and receive a clean map without sensor noise. RRT* — set up as in Guzzi et al. — is stopped after 150 s, which gives a tree of about 10k edges, and does not optimize rotation. On three maps, start–goal pairs 4.0 m apart were sampled, and 60 successful plans were recorded per map.

What this means: even the unpolished grid path beats RRT* on cost, and polishing roughly halves RRT*’s cost while adding only about 0.1 s — the whole plan takes about half a second instead of about two and a half minutes.

The paper sums up the speed gain as three orders of magnitude over RRT*. Read directly from Table I, the full planner is about 260–300 times faster than RRT*’s time; note that RRT*’s time is set by its 150 s budget, not by it finishing.

On the mountain and garden maps, both planners find the same overall routes — between peaks and valleys, around rough patches and steps, up and down stairs, along a narrow path. One telling case: in the garden, the planners walk around steps and a slightly rough patch even though the controller could usually handle them, because flat ground is cheaper. The new planner’s paths are visibly smoother than RRT*’s, which matters for a real robot following them.

In short: on the robot’s own computer the whole plan takes about 1.5 s, so it re-plans continuously while exploring — through a crowded corridor, and around (or over) a low block.

On ANYmal C, the planner runs on an onboard Nvidia Jetson AGX Xavier, which at the same time builds and updates the elevation map from two RoboSense RS-BPearl lidars (using a GPU version of elevation mapping). One planning loop breaks down as follows:

What this means: the robot gets a fresh, capability-aware plan about every second and a half, fast enough to react as its map fills in.

In the experiments the final target was far away, outside the area the robot could plan in. So each loop set a temporary goal: a reachable point near the edge of the current planning area, in the direction of the target. A simple path follower then walked the optimized path while the next plan was computed.

Crowded corridor. With a goal 10 m ahead in a corridor full of obstacles, the robot reached it without collision or human help, in multiple repeats.

Low block. A wooden block with a 12 cm step was placed in front of the robot. The walking controller does not use its cameras or lidars, so it can climb the block but may briefly trip. When there was room, the planner took a detour; when the detour was blocked, it led the robot straight over the block.

In short: the speed comes from simplifications — a fixed grid, headings tied to motion direction, costs learned in simulation — each with a price.

• Grid gaps. The fixed grid is probabilistically incomplete and can fail in narrow places due to aliasing. Vague sampling raises the chance of finding a route but does not guarantee it.
• Heading in the planner. For speed, the grid planner does not sample different headings: each move faces its own direction. The optimizer adjusts headings afterwards.
• Clean maps in the comparison. The simulation comparison uses static maps without perception noise, and RRT* ran under a fixed 150 s budget without rotation optimization.
• Simulation-learned costs. The costs are only as good as the simulated controller and terrain. Training adds a few centimetres of noise to mimic real maps; the real-robot tests show the approach working in a lab corridor and around a block.

The paper’s outlook: because the costs are learned from simulation, and such learned costs have been used for other robots before, the same planning framework could be moved to other robotic platforms with little manual effort.