Elevation Mapping for Locomotion and Navigation using GPU

In short: a robot only sees a slice of the world at a time, so it keeps a map that remembers what it has seen, and that map has to update as fast as the robot moves.

A mobile robot in an unknown place perceives it with onboard sensors: LiDARs (laser scanners) and depth cameras. Each returns a point cloud, a set of 3-D dots on whatever the beams hit. One frame covers only part of the surroundings, so robots fuse the frames over time into a map, then plan on that map.

Two kinds of users need that map. A navigation planner asks “which way can I go?” and rates the terrain by slope and roughness. A walking controller for a legged robot asks “where exactly can each foot land?” and needs fine detail around the feet. The paper builds one map that serves both, in real time.

In short: storing one ground height per square is the sweet spot for walking robots: it can describe steps and slopes, and it stays small.

The classic occupancy grid marks each square of a flat floor plan as occupied or free. That is fine for a wheeled robot indoors, but on rough terrain with slopes and steps it has no way to say “this is a step you can climb”.

An elevation map (often called 2.5-D) keeps the same flat grid but stores a ground height in each square. Earlier work extended it to several levels per square for structures like bridges. Voxel maps go fully 3-D: space is cut into small cubes, each with a probability of being occupied. They can describe anything, but for the same square size they need far more memory than a height grid.

Some systems build a voxel map and then flatten it into a height map. The paper points out the cost: fine resolution means tiny cubes and lots of memory, and the robot then has to keep both maps. This paper builds the height map directly from the points.

What this means: for legged robots the height grid gives most of the benefit of 3-D at a fraction of the memory, which is why this paper, and the controllers built on it, use one.

In short: the map update is the same small job for every point, so the paper hands each point to its own worker on the graphics chip instead of queueing them on the processor.

The starting point is the lab’s earlier open-source elevation map by Fankhauser et al. It moves each incoming point into the robot’s map frame and updates that square’s height, all on the CPU (the main processor). That worked for earlier walking and navigation experiments, but the paper found it was not efficient enough to process large point clouds in real time for faster, more agile robots.

Updating a square from a point barely depends on the other points, which is exactly the kind of work a GPU is built for. The paper writes the update as custom GPU programs (CUDA kernels, written through the Python library CuPy) that run over all points in parallel. The map lives in GPU memory the whole time; it is copied back to the CPU and published as a ROS message (the usual robot messaging system) only at a rate the user chooses, which avoids needless copying.

What this means: on the robot’s own computer, the map keeps pace with every LiDAR scan and still updates 16 times a second from 400,000-point camera frames; with a lightly thinned camera cloud it reaches almost 50 updates a second, which the paper calls fast enough for walking. (Paper Table II; 10 m × 10 m map at 4 cm.)

In short: each square keeps a height and an uncertainty, and every new point nudges the height, more if the point is trustworthy, not at all if it is clearly wrong.

The height update follows the earlier probabilistic elevation map: a one-number Kalman filter per square, the textbook way to blend a running estimate with new noisy measurements. With h the square’s height, σ_m² its variance (uncertainty), p_z a new point’s height and σ_p² that point’s variance:

The distance rule is a simplified version of a published depth-sensor noise model. A new square starts with a very large variance, so its first point is taken almost as is. A small constant variance is also added over time, so squares that stop being updated slowly become less certain.

Two guards

Outliers. As in the earlier work, a point whose height is too many standard deviations away from the square’s estimate (a large Mahalanobis distance) is rejected.

Wall edges. A square on the edge of a wall receives points from the top and from the face. Plain averaging pulls its height below the true top. The paper counts the points that fall into a square in one update; above a threshold, points lower than the current estimate are ignored. This check runs before the outlier test, which keeps edges sharp.

In short: field deployments showed three ways a height map goes wrong (drift, moved obstacles, overhangs), and the paper adds a cheap fix for each, plus one for multi-floor buildings.

Height drift compensation

No robot knows its own position perfectly; the estimate slowly drifts, and height drift is the most harmful kind because it creates fake steps. Each update, the paper measures the error ε between every new point and the map, uses only flat squares (steep ones, judged by the traversability layer, would give misleading errors) and shifts the whole map by the average error.

Visibility cleanup by ray casting

If something moves away, its old heights stay in the map until their uncertainty grows enough to pass the outlier test, which is slow. Ray casting fixes this: for each point, the paper steps along the line from the sensor to that point. Where the line passes below a square’s height (minus a margin for its uncertainty), that square must be empty, so its height is removed. To stop flickering when rays graze a surface at a shallow angle, a square is only removed if it hasn’t been updated for a while and the ray is not too parallel to the surface (|r · n| > α_n, with r the ray direction and n the surface normal).

Exclusion area for overhangs

A beam or ceiling just in front of the robot returns points high above the floor, which a height map would record as a wall, blocking the path underneath. The paper ignores points above a ramp-shaped boundary in front of the sensor: flat and just above sensor height close to the robot, then rising at an angle θ_a to a maximum. Close overhangs are ignored; slopes rising further away are still mapped.

Overlap clearance

In multi-floor buildings, a 2.5-D map can keep heights from the floor above or below. Near the robot, the paper clears heights that differ from the robot’s own height by more than a threshold, which the authors report works well when the robot climbs or descends stairs.

Tested on the same recorded data

What this means: the paper shows these results as map pictures rather than scores; in all three cases the fix removes the artifact that the feature-off setting and the earlier software show.

In short: even where the ground is hidden, the rays that flew over it prove it can be no higher than they were, and the slope of that limit hints at whether the hole is harmless or a drop.

Holes in the map have to be interpreted by whatever uses it. A navigation planner typically either fills them optimistically (for example by image inpainting) or treats them as untraversable. Neither is right everywhere.

While stepping along each ray for the cleanup, the paper also records, for every square without a valid height, the lowest ray height that passed over it. If the ground there were higher, the ray would have hit it. This is stored as an upper-bound layer, similar to the idea of “virtual surfaces” in earlier navigation work. Ground hidden behind an obstacle usually gives an upper bound with a small incline; a real drop gives a steeper one, because the rays that graze its edge go down more steeply.

In short: because the map already lives on the graphics chip, it can cheaply carry extra layers that navigation and walking controllers need, from a learned traversability score to flat regions for footholds.

For navigation

Traversability. A small convolutional neural network (from the lab’s ArtPlanner navigation work) scores each square from the local geometry. It is written in PyTorch and shares GPU memory with the map, so no data has to be copied, and it is light enough to run at the full map update rate. Surface normals are computed per square as well.

For walking

Optimization-based walking controllers need a complete, well-behaved terrain. The package offers optional post-processing:

• Minimum filter (inpainting): empty squares are filled with the minimum height found along the border of the hole, as in the TAMOLS planner.
• Smoothing: Gaussian, box-blur and median filters from OpenCV; the median filter is reported to be better at removing artifacts.
• Plane segmentation: the map is split into continuous flat regions, returned as polygons (outer boundary and holes), which a whole-body model-predictive controller uses as foothold constraints.

The map fed three kinds of controllers in the lab’s later work: a reinforcement-learning walking controller that samples heights around each foot (also used at the SubT Challenge), a model-based planner that scores footholds from normals and roughness, and a whole-body MPC that uses the plane segmentation.

In short: on the robot’s own computer the whole update, with every feature on, takes under 7 ms, and the map ran on four robots in a major underground competition.

The timing below was measured on the Jetson Xavier onboard ANYmal, averaged over 1000 updates with Bpearl LiDAR data (10 m × 10 m map, 4 cm squares).

What this means: the LiDAR delivers a scan every 50 ms, and the full update needs about a seventh of that. The traversability network is 60 % of the time, so the authors name it as the part to speed up next. (Paper Table I.)

In the field

At the DARPA Subterranean Challenge, team CERBERUS ran this map on four ANYmal robots exploring underground. A local planner used the traversability and upper-bound layers to pick safe paths and avoid places like cliffs, toward targets set by an exploration planner.

The reinforcement-learning walking controller took its terrain heights from this map. With leg odometry (position estimated from the legs’ motion), the robot’s height tended to drift, especially when feet slipped, which made fake steps in the map. Drift compensation made walking smoother; in long missions, a pose estimate fusing the inertial sensor with LiDAR SLAM plus drift compensation reduced the issue further.

In short: this is an engineering paper about a fast, practical tool; its fixes are simple rules with thresholds, and it keeps the one-height-per-square view of the world.

• One height per square. Bridges, tables and multiple floors cannot be represented at once. The exclusion area and overlap clearance are practical workarounds, not a full 3-D model.
• Rules with thresholds. Drift compensation, the wall-edge rule, the exclusion ramp and overlap clearance each depend on user-set parameters.
• Qualitative map-quality results. Map quality is compared with pictures (Fig. 6), not an error score against ground truth; timing is measured precisely.
• Where the time goes. The traversability network takes most of each update (4.102 of 6.857 ms), so the paper points to it for future speed-ups.

The package was released as open source (elevation_mapping_cupy) and, per the paper, formed the terrain perception for learning-based and model-based walking controllers, navigation planners and a learned method for filling occluded regions.