Open Source Robot Localization for Non-Planar Environments

TL;DR

This paper tackles localization for mobile robots operating on non-planar terrain by extending Adaptive Monte Carlo Localization to non-planar maps using a dual-map representation: a GridMap-based elevation layer and an Octomap-based occupancy map. The method, integrated into Nav2 under the name MH-AMCL, updates particle poses with odometry, corrects with a 3D/2D sensor observation model leveraging raytracing in the octomap, and adapts the particle cloud via a reseeding strategy; it also aligns with a TF-based frame system for consistent localization across frames. Across simulated and real-world experiments on Tiago and Summit XL, MH-AMCL achieves sub-meter translation and sub-0.1 rad orientation indoors, and under 1 meter outdoors, outperforming 2D AMCL and providing competitive robustness against 3D SLAM in complex terrain. The work emphasizes open-source Nav2 integration, facilitating replication and broader deployment in GPS-denied, non-planar navigation tasks on indoor and outdoor environments.

Abstract

The operational environments in which a mobile robot executes its missions often exhibit non-flat terrain characteristics, encompassing outdoor and indoor settings featuring ramps and slopes. In such scenarios, the conventional methodologies employed for localization encounter novel challenges and limitations. This study delineates a localization framework incorporating ground elevation and incline considerations, deviating from traditional 2D localization paradigms that may falter in such contexts. In our proposed approach, the map encompasses elevation and spatial occupancy information, employing Gridmaps and Octomaps. At the same time, the perception model is designed to accommodate the robot's inclined orientation and the potential presence of ground as an obstacle, besides usual structural and dynamic obstacles. We provide an implementation of our approach fully working with Nav2, ready to replace the baseline AMCL approach when the robot is in non-planar environments. Our methodology was rigorously tested in both simulated environments and through practical application on actual robots, including the Tiago and Summit XL models, across various settings ranging from indoor and outdoor to flat and uneven terrains. Demonstrating exceptional precision, our approach yielded error margins below 10 centimeters and 0.05 radians in indoor settings and less than 1.0 meters in extensive outdoor routes. While our results exhibit a slight improvement over AMCL in indoor environments, the enhancement in performance is significantly more pronounced when compared to 3D SLAM algorithms. This underscores the considerable robustness and efficiency of our approach, positioning it as an effective strategy for mobile robots tasked with navigating expansive and intricate indoor/outdoor environments.

Figures (16)

• Figure 1: Robot in a non-planar environment equipped with LIDAR 2D and 3D using the popular visualization tool RViz. Octomap map (upper image) codifies 3D occupancy (colors indicate elevation in Z axe). Gridmap maps (bottom image) codify elevation and occupancy (colors indicate free space with fuchsia and obstacles with red. The green points are the obstacles detected by the 3D LIDAR, and the red points are those detected by the 2D LIDAR.
• Figure 2: 2D Occupancy gridmap (upper image, with free space in gray and obstacles in black) and the corresponding Gridmap map (bottom image, where colors indicate elevation in Z axe).
• Figure 3: Diagrams of the distinct processes that update $\mathcal{P}_t$. After the initialization, three synchronous processes update the particles at different rates.
• Figure 4: Some frames of the robot's TF tree. The robot's inclination is provided by the transform generated by the robot's driver.
• Figure 5: Ray that links the origin of a 3D laser sensor reference axes with the position of one of its readings. The red sphere indicates the collision of the beam with the octomap.