We present the first algorithm to efficiently compute certifiably optimal solutions to range-aided simultaneous localization and mapping (RA-SLAM) problems. Robotic navigation systems increasingly incorporate point-to-point ranging sensors, leading to state estimation problems in the form of RA-SLAM. However, the RA-SLAM problem is significantly more difficult to solve than traditional pose-graph SLAM: Ranging sensor models introduce nonconvexity and single range measurements do not uniquely determine the transform between the involved sensors. As a result, RA-SLAM inference is sensitive to initial estimates yet lacks reliable initialization techniques. Our approach, certifiably correct RA-SLAM (CORA), leverages a novel quadratically constrained quadratic programming formulation of RA-SLAM to relax the RA-SLAM problem to a semidefinite program (SDP). CORA solves the SDP efficiently using the Riemannian Staircase methodology; the SDP solution provides both: 1) a lower bound on the RA-SLAM problem's optimal value and 2) an approximate solution of the RA-SLAM problem, which can be subsequently refined using local optimization. CORA applies to problems with arbitrary pose-pose, pose-landmark, and ranging measurements and, due to using convex relaxation, is insensitive to initialization. We evaluate CORA on several real-world problems. In contrast to state-of-the-art approaches, CORA is able to obtain high-quality solutions on all problems despite being initialized with random values. In addition, we study the tightness of the SDP relaxation with respect to important problem parameters: The number of: 1) robots; 2) landmarks; and 3) range measurements. These experiments demonstrate that the SDP relaxation is often tight and reveal relationships between graph connectivity and the tightness of the SDP relaxation.

中文翻译：

---

可证明正确的范围辅助 SLAM


我们提出了第一个算法，可以有效地计算范围辅助同步定位和建图（RA-SLAM）问题的可证明最佳解决方案。机器人导航系统越来越多地采用点对点测距传感器，从而导致 RA-SLAM 形式的状态估计问题。然而，RA-SLAM 问题比传统的位姿图 SLAM 更难解决：测距传感器模型引入了非凸性，并且单一距离测量不能唯一地确定所涉及传感器之间的变换。因此，RA-SLAM 推理对初始估计很敏感，但缺乏可靠的初始化技术。我们的方法可证明正确的 RA-SLAM (CORA)，利用 RA-SLAM 的新型二次约束二次规划公式将 RA-SLAM 问题放松为半定规划 (SDP)。 CORA 使用黎曼阶梯法有效求解 SDP； SDP 解决方案提供：1) RA-SLAM 问题最优值的下界，以及 2) RA-SLAM 问题的近似解，随后可以使用局部优化对其进行细化。 CORA 适用于任意姿势-姿势、姿势-地标和测距测量的问题，并且由于使用凸松弛，对初始化不敏感。我们在几个现实问题上评估 CORA。与最先进的方法相比，CORA 能够获得所有问题的高质量解决方案，尽管使用随机值进行初始化。此外，我们研究了 SDP 松弛对于重要问题参数的紧密度： 1) 机器人的数量； 2）地标； 3) 范围测量。 这些实验表明，SDP 松弛通常是紧密的，并揭示了图连通性与 SDP 松弛的紧密度之间的关系。