Certifiably Correct Range-Aided SLAM

Other · Tue Feb 21 23:00:00 EST 2023 · Robotics

DOI:https://doi.org/10.48550/arXiv.2302.11614· OSTI ID:1992609

Papalia, Alan ^[1]; Fishberg, Andrew; O'Neill, Brendan; How, Jonathan; Rosen, David; Leonard, John

---

1. Massachusetts Institute of Technology

We present the first algorithm capable of efficiently computing certifiably optimal solutions to range-aided simultaneous localization and mapping (RA-SLAM) problems. Robotic navigation systems are increasingly incorporating point-to-point ranging sensors, leading state estimation which takes the form of RA-SLAM. However, the RA-SLAM problem is more difficult to solve than traditional pose-graph SLAM; ranging sensor models introduce additional non-convexity, unlike pose-pose or pose-landmark measurements, a single range measurement does not uniquely determine the relative transform between the involved sensors, and RA-SLAM inference is highly sensitive to initial estimates. Our approach relaxes the RA-SLAM problem to a semidefinite program (SDP), which we show how to solve efficiently using the Riemannian staircase methodology. The solution of this SDP provides a high-quality initialization for our original RA-SLAM problem, which is subsequently refined via local optimization, as well as a lower-bound on the RA-SLAM problem's optimal value. Our algorithm, named certifiably correct RA-SLAM (CORA), applies to problems comprised of arbitrary pose-pose, pose-landmark, and ranging measurements. Evaluation on simulated and real-world marine examples shows that our algorithm frequently produces certifiably optimal RA-SLAM solutions; moreover, even suboptimal estimates are typically within 1-2\% of the optimal value.

• Research Organization: Chris D’Urbano, Assistant Director, Georgia Tech Research Corporation, Georgia Institute of Technology, Office of Sponsored Programs, Atlanta, GA 30332-0420, Phone: [phone], E-mail: [e-mail]
• Sponsoring Organization: USDOE National Nuclear Security Administration (NNSA)
• DOE Contract Number: NA0003921
• OSTI ID: 1992609
• Journal Information: Robotics, Journal Name: Robotics
• Country of Publication: United States
• Language: English