TL;DR
This paper introduces NanoMap, a novel framework for robot navigation that efficiently queries local 3D sensor data while explicitly accounting for pose uncertainty. By avoiding global map fusion and instead searching a history of sensor views for the minimum-uncertainty perspective, NanoMap significantly improves robustness to state estimation errors, especially during high-speed maneuvers.
What Problem Does This Paper Address?
Robots navigating at high speeds in unknown environments, particularly those relying on depth sensors, face a critical challenge: state estimation uncertainty. Traditional mapping approaches, like occupancy grids, fuse sensor data into a global map. However, this process is computationally expensive and, more importantly, fragile to errors in the robot's estimated pose. Even small drifts in the robot's position can lead to significant inaccuracies in the global map, causing planning failures and potential crashes. Existing "memoryless" approaches, while robust to pose errors, discard valuable historical information, limiting maneuverability. This paper addresses the need for a mapping system that can leverage historical sensor data efficiently and robustly, even when the robot's pose is uncertain.
Methods
NanoMap introduces a "lazy search" approach over a local history of depth sensor measurements. Instead of building a global, fused map, NanoMap stores a sequence of relative pose transforms and associated depth sensor data. When a motion planner queries a point in space, NanoMap searches this history for the sensor view that provides the minimum uncertainty for that point. This is achieved by:
- Uncertainty Propagation: Modeling the uncertainty of the query point and propagating it through the relative transforms to estimate the uncertainty in each sensor's frame.
- "Lazy" Fusion and Search: Instead of fusing all data, NanoMap searches for the most recent and therefore least uncertain view of the query point. The
IsInFOV()function determines if a query point falls within a sensor's field of view, considering the uncertainty of the point's pose. - AABB for Uncertainty: To efficiently check if an uncertain query point is within a sensor's field of view, NanoMap uses the Axis-Aligned Bounding Box (AABB) of the 1-sigma uncertainty of the query point.
- Efficient Pose Updates: NanoMap can trivially incorporate updated pose information (e.g., from loop closures) by simply updating the relevant relative transforms, making it significantly faster than rebuilding global maps.
Datasets
The paper uses simulated data generated from a professional-grade urban environment created in the Unity game engine. They also present hardware validation results from flights on a quadrotor. Specific datasets for training are not discussed, as the focus is on real-time mapping and planning.
Key Insights and Ideas
- Pose uncertainty is critical for high-speed navigation: The paper strongly demonstrates that ignoring pose uncertainty leads to significant performance degradation, especially during aggressive maneuvers.
- "Lazy" search is more efficient than global fusion for planning: By avoiding the computational overhead of global map fusion, NanoMap can provide uncertainty-aware queries much faster, particularly for a limited number of queries.
- Trivial pose updates are a game-changer: The ability to quickly incorporate corrected pose information (e.g., from loop closures) without rebuilding the entire map is a significant advantage for real-time systems.
- Minimum-uncertainty view is key: Actively searching for the sensor view with the least uncertainty for a given query point provides a more robust basis for planning than relying on a single, potentially noisy, fused map.
Key Results
- Robustness to Pose Drift: NanoMap significantly increases robustness to state estimation uncertainty. For pose drifts on the order of 1% position drift (at 10 m/s) or pose corrections of ~1m, NanoMap's uncertainty-aware features substantially improve safety. For example, with a drift of 0.7 m over 5 seconds, NanoMap achieved 97-98% safety, compared to lower rates for unmodeled uncertainty.
- Computational Efficiency: For a moderate number of queries (up to 2,500), NanoMap is the fastest among the compared methods (OctoMap, Voxblox, Ewok), even in its worst-case query scenario.
- Fast Pose Updates: NanoMap is 2-4 orders of magnitude faster at incorporating updated pose information than other packages, enabling real-time adjustments after loop closures.
- Hardware Validation: The system was successfully demonstrated on a quadrotor flying at up to 10 m/s in challenging environments (forested canopy and indoor warehouses).
Limitations and Future Work
- Depth Sensor Assumption: NanoMap requires non-zero volume depth sensors (depth cameras or 3D lidars) and does not directly support 2D or 1D sensors.
- Spherical Robot Assumption: The current implementation assumes a spherical robot for collision checking, though it notes that downstream inflation can handle non-spherical approximations.
- Limited History: While NanoMap handles history, the practical limit is determined by memory and computational constraints.
- Future Work: The authors encourage future work to draw inspiration from NanoMap and supplement traditional mapping approaches. They also suggest exploring how to integrate more sensors to increase the field of view.
Summary updated