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 non-convexity 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 (QCQP) 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 (i) a lower bound on the RA-SLAM problem's optimal value, and (ii) 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. Additionally, we study the tightness of the SDP relaxation with respect to important problem parameters: the number of (i) robots, (ii) landmarks, and (iii) range measurements. These experiments demonstrate that the SDP relaxation is often tight and reveal relationships between graph rigidity and the tightness of the SDP relaxation.

翻译：我们提出了首个能够高效计算可验证最优解的测距辅助同时定位与建图（RA-SLAM）算法。机器人导航系统正日益集成点对点测距传感器，由此催生了以RA-SLAM形式呈现的状态估计问题。然而，RA-SLAM问题比传统位姿图SLAM求解难度显著更大：测距传感器模型引入了非凸性，且单次测距测量无法唯一确定相关传感器间的变换关系。因此，RA-SLAM推理对初始估计敏感，却缺乏可靠的初始化技术。我们提出的可验证正确性RA-SLAM（CORA）方法，通过引入RA-SLAM的新型二次约束二次规划（QCQP）形式，将RA-SLAM问题松弛为半定规划（SDP）。CORA采用黎曼阶梯方法高效求解SDP；该SDP解既能（i）提供RA-SLAM问题最优值的下界，也能（ii）给出RA-SLAM问题的近似解，进而可通过局部优化进行精化。CORA适用于包含任意位姿-位姿、位姿-路标及测距测量的问题，并且由于采用凸松弛方法，对初始值不敏感。我们在多个真实场景问题中评估了CORA。与现有最优方法相比，CORA即便采用随机初始化，仍能在所有问题上获得高质量解。此外，我们研究了SDP松弛紧度与重要问题参数（即机器人数量、路标数量、测距测量数量）的关系。实验表明，SDP松弛通常具有紧致性，并揭示了图刚度与SDP松弛紧度之间的关联。