Abstract
Real-time terrain perception is critical for the navigation and locomotion of mobile robots in unstructured environments. While 2.5D elevation maps provide the necessary geometric detail for agile movement, traditional CPU-based implementations struggle to process high-frequency, dense sensor data from modern LiDAR and depth cameras. This paper presents a GPU-accelerated elevation mapping pipeline, elevation_mapping_cupy, which leverages the CuPy library to parallelize terrain processing. By implementing features such as GPU-based Kalman filtering, height drift compensation, and visibility cleanup, the framework achieves update rates of up to 50Hz on embedded hardware. The system's efficacy was demonstrated through its deployment on the ANYmal quadruped during the DARPA Subterranean Challenge.
Introduction
For autonomous robots—particularly legged platforms like quadrupeds—accurate knowledge of ground geometry is essential for survival. While 2D occupancy grids are sufficient for flat indoor environments, rough and unstructured terrain requires 2.5D elevation maps, where each grid cell stores a height value and associated uncertainty.
Historically, the gold standard for this task was a CPU-based probabilistic mapping framework. However, as robots move faster and sensors (such as LiDAR and depth cameras) produce millions of points per second, CPU-based methods have become a significant bottleneck. These methods often require heavy down-sampling, which destroys the fine geometric detail needed for secure foot placement. Furthermore, standard elevation maps are prone to "ghost" artifacts caused by sensor drift, dynamic obstacles, and overhanging structures. This paper addresses these challenges by shifting the computational load to the GPU.
Methodology
The authors developed a framework integrated with the Robot Operating System (ROS) that leverages NVIDIA GPUs via the CuPy library. The pipeline introduces several technical innovations to improve both speed and map quality:
- GPU-Parallelized Kalman Filtering: Every point in a cloud is processed in parallel to update the height and variance of map cells. This formulation allows the system to handle dense data without the exponential increase in processing time seen in CPU baselines.
- Height Drift Compensation: To combat SLAM and odometry drift, the system calculates the error between new measurements and the existing map in identified "flat" areas. It then shifts the map to match, preventing the appearance of artificial "steps" in the terrain data.
- Visibility Cleanup and Ray Casting: The pipeline utilizes ray casting to clear out "ghost" obstacles, such as pedestrians walking past the robot. It also maintains an "upper bound" layer to represent the maximum possible height in unobserved areas, providing a safety margin for navigation planners.
- Exclusion Areas: A ramp-based parameterization allows the system to ignore ceilings or high overhangs that would otherwise be registered as impassable walls in a 2.5D representation.
- On-GPU Inference: Because the map resides in GPU memory, the authors can run Convolutional Neural Networks (CNNs) for traversability estimation directly on the map data. This avoids costly CPU-GPU data transfers and allows post-processing to run at the full map update rate.
Experimental Results
The system was evaluated using the ANYmal quadruped robot equipped with Robosense Bpearl LiDAR (360° field-of-view) and Intel RealSense depth cameras. Testing environments included indoor stairs, outdoor rough terrain, and the extreme conditions of the DARPA Subterranean (SubT) Challenge, including caves and urban ruins.
Performance and Scalability
On a desktop PC (RTX 2080Ti), the GPU pipeline's processing time remained nearly constant regardless of the number of points, whereas the CPU baseline's processing time grew exponentially. On edge hardware, specifically the NVIDIA Jetson Xavier, the system achieved update rates of approximately 50Hz for filtered depth data and 20Hz for raw LiDAR data. A full update cycle—including transformation, drift compensation, ray casting, and traversability analysis—takes only ~6.8ms for 43,000 points.
Field Validation
The software served as a core component of Team CERBERUS’s winning entry in the DARPA SubT Challenge. Its ability to maintain high-resolution maps in degraded, high-stakes environments proved its reliability for real-world deployment.
Discussion and Limitations
While the framework provides a significant leap in performance, it is fundamentally constrained by its 2.5D nature. It cannot simultaneously represent a floor and a ceiling in the same vertical column, which remains a challenge in complex multi-level structures. Additionally, the traversability filter remains the most computationally expensive module, accounting for roughly 60% of the total processing time.
Future work will focus on optimizing the traversability module and exploring ways to mitigate the 2.5D constraint. Despite these limitations, the "data locality" advantage—keeping the map on the GPU for immediate post-processing—sets a new standard for real-time robotic perception.
Conclusion
By leveraging GPU acceleration, the proposed elevation mapping pipeline enables high-resolution, real-time terrain perception for agile mobile robots. The inclusion of pragmatic robustness features like drift compensation and visibility cleanup ensures that the map remains accurate even in the presence of sensor noise and localization errors, providing a robust foundation for autonomous navigation in the wild.
Summary updated