Certifiably-Correct Mapping for Safe Navigation Despite Odometry Drift

TL;DR

This work tackles the safety problem of obstacle mapping under odometry drift by introducing certifiably-correct mapping that preserves obstacle-free regions in the robot's body frame. It develops two complementary approaches—Certified Safe Flight Corridors and Certified ESDF—each featuring a formal deflation operation that guarantees the claimed safe regions remain within true free space, even as drift grows. The methods come with theoretical proofs of correctness and are validated through Replica dataset simulations and real rover experiments, where certified maps prevent collisions that occur with baseline mappings. The framework enables safer, more reliable navigation by allowing planning to rely on certifiably-correct perception outputs while still leveraging uncertified maps for exploration and goal-directed tasks. Overall, the paper advances perception-certification for safety-critical navigation in the presence of odometry uncertainties by providing rigorous guarantees and practical, GPU-accelerated implementations.

Abstract

Accurate perception, state estimation and mapping are essential for safe robotic navigation as planners and controllers rely on these components for safety-critical decisions. However, existing mapping approaches often assume perfect pose estimates, an unrealistic assumption that can lead to incorrect obstacle maps and therefore collisions. This paper introduces a framework for certifiably-correct mapping that ensures that the obstacle map correctly classifies obstacle-free regions despite the odometry drift in vision-based localization systems (VIO}/SLAM). By deflating the safe region based on the incremental odometry error at each timestep, we ensure that the map remains accurate and reliable locally around the robot, even as the overall odometry error with respect to the inertial frame grows unbounded. Our contributions include two approaches to modify popular obstacle mapping paradigms, (I) Safe Flight Corridors, and (II) Signed Distance Fields. We formally prove the correctness of both methods, and describe how they integrate with existing planning and control modules. Simulations using the Replica dataset highlight the efficacy of our methods compared to state-of-the-art techniques. Real-world experiments with a robotic rover show that, while baseline methods result in collisions with previously mapped obstacles, the proposed framework enables the rover to safely stop before potential collisions.

Figures (9)

• Figure 1: Overview of notation and objectives. (a) depicts the operating environment, where the world $\mathcal{W}$ is the union of the free space $\mathcal{F}$ and the obstacles $\mathcal{O}$. The robot does not know $\mathcal{F}$ or $\mathcal{O}$. It starts at $B_0$, and follows the gray trajectory to $B_k$ building the map as it goes. (b) depicts the ideal mapping output, where at the $k$-th timestep, the map $\mathcal{M}_k$ is composed of the known safe region $\mathcal{S}_k$, the unknown space $\mathcal{U}_k$ and the known obstacle set $\mathcal{R}_k$. (c) depicts the map produced by current state-of-the-art methods, where due to odometry drift the map is erroneous: notice that the safe region (according to the constructed map) is not a subset of the free space, $\mathcal{S}_k \not\subset \mathcal{F}$. (d) depicts the desired behavior of the certified maps, where although the safe region is smaller, it is certifiably-correct: we can prove that $\mathcal{S}_k \subset \mathcal{F}$.
• Figure 2: Two approaches to constructing an obstacle map. (Top row) An RGBD camera provides (a) the first person RGB image, and (b) the depth image/pointcloud constructed from stereo images. (Bottom row) The SFC approach represents the free space as a union of polytopes, one of which is depicted in (c). The ESDF approach represents the world using voxels, where each voxel stores the signed distance to the nearest obstacle. From this, both the (d) ESDF at specific voxels or (e) obstacle surface locations can be extracted and used for safe navigation. To aid the reader, in (c) and (d) the raw pointcloud is also visualized, and in (d) the colorscheme is such that voxels are marked green if $d > 0$, and red otherwise. This makes the map look binary, although it contains continuous values. Furthermore, note both methods operate in 3D - the 2D slice is used for visualization.
• Figure 3: Visualization of a snapshot of the office0 environment mapped using the baseline and certified SFC methods. (a, d) shows the office0 environment, while (b, e) and (c, f) show the respective $\mathcal{S}$ sets at the 500-th timestep from an external and an internal view. The baseline map claims a larger volume to be safe compared to the certified method (red volume is larger than green volume). However, we can also see numerous regions where the red region intersects with the ground truth mesh, indicating that the claimed safe region contains obstacle points. In the certified method, we see no violations.
• Figure 4: Visualization of the maps generated using the baseline and certified ESDF methods on the office3 environment. In (a) we see the ground-truth mesh. In (b) and (c) we can see the internal view after 500 timesteps. As in \ref{['fig:sfc_summary']}, although the baseline method maps a larger volume (red mesh is larger than green mesh), it also contains many violations. In (e) and (f) we see a slice of the ESDF over time. The green region indicates the $\mathcal{S}$ set at the respective times. The small black arrows point to various violations in the baseline method, while in the certified methods we see no violations.
• Figure 5: Rover Experimental Setup. (a) Block diagram. The human is teleoperating the rover using only the FPV feed and the reconstructed obstacle map computed and streamed in real-time. The map is also used onboard the robot to stop the robot if it violates safety constraints. The safety filter can either use the baseline ESDF or the Certified ESDF. (b) Picture of the testing environment. The robot drives through the tunnel, mapping it as it passes through. After exploring the corridors, the rover tries to return through the tunnel in reverse, without remapping the tunnel. (c) shows the rover in more detail. The AION R1 UGV has been modified, with all sensing on Intel Realsense D455, and all compute on the Nvidia OrinNX 16GB.

Theorems & Definitions (12)

• Definition 1
• Lemma 1
• Theorem 1
• Remark 1
• Remark 2
• Definition 2
• Theorem 2
• Remark 3
• Remark 4
• Remark 4