NanoMap: Fast, Uncertainty-Aware Proximity Queries with Lazy Search Over Local 3D Data

Abstract

We would like robots to be able to safely navigate at high speed, efficiently use local 3D information, and robustly plan motions that consider pose uncertainty of measurements in a local map structure. This is hard to do with previously existing mapping approaches, like occupancy grids, that are focused on incrementally fusing 3D data into a common world frame. In particular, both their fragile sensitivity to state estimation errors and computational cost can be limiting. We develop an alternative framework, NanoMap, which alleviates the need for global map fusion and enables a motion planner to efficiently query pose-uncertainty-aware local 3D geometric information.The key idea of NanoMap is to store a history of noisy relative pose transforms and search over a corresponding set of depth sensor measurements for the minimum-uncertainty view of a queried point in space. This approach affords a variety of capabilities not offered by traditional mapping techniques: (a) the pose uncertainty associated with 3D data can be incorporated in motion planning, (b) poses can be updated (i.e., from loop closures) with minimal computational effort, and (c) 3D data can be fused lazily for the purpose of planning. We provide an open-source implementation of NanoMap, and analyze its capabilities and computational efficiency in simulation experiments. Finally, we demonstrate in hardware its effectiveness for fast 3D obstacle avoidance onboard a quadrotor flying up to 10 m/s.

Autonomous navigation of MAVs in unknown cluttered environments

“…In this section, we contrast our contribution with some related works. In particular, we focus this discussion in the contributions reported in Cieslewski, Kaufmann, and Scaramuzza (2017a), Collins (2019), Florence, Carter, Ware, and Tedrake (2018), Gao, Wu, Lin, and Shen (2018), Lin et al (2018), Mohta et al (2018), Oleynikova, Taylor, Siegwart, and Nieto (2018), Papachristos, Khattak, and Alexis (2017), Selin, Tiger, Duberg, Heintz, and Jensfelt (2019), Usenko, VonStumberg, Pangercic, and Cremers (2017), which present autonomous navigation for unknown environments, with on‐board mapping, planning, and trajectory generation/execution. These contributions, in our opinion, represent the closest approaches to our work.…”

“…The latter is used to help the robot escape pockets by adding edges and nodes to the graph in a way that captures potential pathways that the robot has passed but not taken. Finally, Florence et al (2018) used a 2D global map to guide local exploration based on uncertainty‐aware proximity queries for planning without any prior discretization of the data. While potentially useful, in more constrained platforms hybrid approaches introduce additional software complexity, that in our case was not necessary.…”

“…During navigation, the target for the local planner is selected on or near the guiding path and A* is used to search a minimum distance path to it. Unlike the other methods in our comparison which generate plans based on maps, Florence et al (2018) use locally a map‐less stochastic motion planner for choosing among a finite 3D motion primitive library, with global guidance provided by a 2D A* planner.…”

“…Regardless of the approach to generate the path, a critical aspect for collision‐free navigation in unknown environments is imposing constraints on the motion plan to navigate within the current FoV toward the next waypoint. Our method incorporates this idea which was also used by (Cieslewski et al, 2017a; Florence et al, 2018; Oleynikova, Taylor, Siegwart, et al, 2018). In contrast, Gao et al (2018) assumed that the sensors onboard the robot cover an entire spherical region centered at the robot, which is hard to achieve in practice.…”

“…Florence et al (2018) search a finite library for a 3D motion primitive which maximizes the Euclidean progress to the global goal, considering collision probabilities for entire paths in environments with only convex obstacles. An important aspect related to navigation in the FoV is ensuring that the camera axis, and hence the yaw orientation is aligned with the direction of motion.…”