Distributed Certifiably Correct Range-Aided SLAM

TL;DR

This work presents the first distributed algorithm for RA-SLAM that can efficiently recover certifiably globally optimal solutions via the Riemannian Staircase method, and demonstrates DCORA's efficacy on real-world multi-agent datasets by achieving absolute trajectory errors comparable to those of a state-of-the-art centralized certifiably correct RA-SLAM algorithm.

Abstract

Reliable simultaneous localization and mapping (SLAM) algorithms are necessary for safety-critical autonomous navigation. In the communication-constrained multi-agent setting, navigation systems increasingly use point-to-point range sensors as they afford measurements with low bandwidth requirements and known data association. The state estimation problem for these systems takes the form of range-aided (RA) SLAM. However, distributed algorithms for solving the RA-SLAM problem lack formal guarantees on the quality of the returned estimate. To this end, we present the first distributed algorithm for RA-SLAM that can efficiently recover certifiably globally optimal solutions. Our algorithm, distributed certifiably correct RA-SLAM (DCORA), achieves this via the Riemannian Staircase method, where computational procedures developed for distributed certifiably correct pose graph optimization are generalized to the RA-SLAM problem. We demonstrate DCORA's efficacy on real-world multi-agent datasets by achieving absolute trajectory errors comparable to those of a state-of-the-art centralized certifiably correct RA-SLAM algorithm. Additionally, we perform a parametric study on the structure of the RA-SLAM problem using synthetic data, revealing how common parameters affect DCORA's performance.

Figures (3)

• Figure 1: (a) Graphical representation of the RA-SLAM problem in the distributed setting. In DCORA, synchronized agents (e.g., $\mathcal{A}_\alpha, \mathcal{A}_\beta, \mathcal{A}_\gamma$) jointly estimate their state in a common frame by performing local computation steps and exchanging information over the distributed communication graph shown in (b). States, including poses (circles) and landmarks (stars), are interconnected via relative pose (black lines) and range (blue lines) measurements. To facilitate efficient communication and computation, states are labeled private (black) or public (red), while measurements exist as inter-agent loop closures (dotted lines), intra-agent loop closures (dash-dotted lines), or odometry (solid lines) as discussed in \ref{['sec:dcoras_variable_ownership_structure']}.
• Figure 2: Modified (a) smallGrid3Dtian2021distributed and (b) sphere2500kaess2012isam2 pose graph optimization (PGO) datasets used to study the range-aided SLAM problem structure in the distributed setting. Original PGO datasets (with their trajectories illustrated in red) have been modified to generate 24 new datasets, which include multiple agents (not shown), landmarks (blue and green circles), and range measurements (not shown).
• Figure 3: Evolution of the normalized relative suboptimality upper bound and the Riemannian gradient norm at each RBCD iterate for rank three in the Riemannian staircase. Modified datasets (a) smallGrid3Dtian2021distributed and (b) sphere2500kaess2012isam2 have the following parameters: (i) landmarks: 8, range measurement probability: 0.5; (ii) landmarks: 16, range measurement probability: 0.5; (iii) landmarks: 8, range measurement probability: 1.0; (iv) landmarks: 16, range measurement probability: 1.0; for two-agent (blue), four-agent (orange), and eight-agent (green) systems. All plots are log-linear.

Theorems & Definitions (1)

• Definition 1