SCORE: A Second-Order Conic Initialization for Range-Aided SLAM

We present a novel initialization technique for the range-aided simultaneous localization and mapping (RA- SLAM) problem. In RA-SLAM we consider measurements of point-to-point distances in addition to measurements of rigid transformations to landmark or pose variables. Standard formulations of RA-SLAM approach the problem as non- convex optimization, which requires a good initialization to obtain quality results. The initialization technique proposed here relaxes the RA-SLAM problem to a convex problem which is then solved to determine an initialization for the original, non-convex problem. The relaxation is a second-order cone program (SOCP), which is derived from a quadratically constrained quadratic program (QCQP) formulation of the RA- SLAM problem. As a SOCP, the method is highly scalable. We name this relaxation Second-order COnic RElaxation for RA- SLAM (SCORE). To our knowledge, this work represents the first convex relaxation for RA-SLAM. We present real-world and simulated experiments which show SCORE initialization permits the efficient recovery of quality solutions for a variety of challenging single- and multi-robot RA-SLAM problems with thousands of poses and range measurements. I. I N T RO D U C T I O N Range-aided simultaneous localization and mapping (RA- SLAM) is a key robotic task, with applications in planetary [1], subterranean [2], and sub-sea [3]–[5] environments. RA- SLAM combines range measurements (e.g., distances between acoustic beacons) with measurements of relative rigid trans- formations (e.g., odometry or pose-landmark measurements) to estimate a set of robot poses and landmark positions. RA-SLAM differs from pose-graph simultaneous localiza- tion and mapping (SLAM) in that the sensing models of range measurements induce substantial difficulties for state estimation. However, ranging is a valuable sensor modality and further developments in RA-SLAM could substantially advance the state of robot navigation. The state-of-the-art formulation of SLAM problems is as maximum a posteriori (MAP) inference, which takes the form of an optimization problem. However, because robot orientations are a non-convex set, the MAP problem is non- convex. Additionally, in RA-SLAM, the measurement models of range sensing introduce further non-convexities. As a result, standard approaches to solving these MAP problems use local- search techniques and can only guarantee locally optimal solutions. Thus, good initializations to the MAP problem are key to obtaining quality RA-SLAM solutions. 1 Computer Science and Artificial Intelligence Laboratory (CSAIL), Massachusetts Institute of Technology (MIT), Cambridge, MA 02139, USA, {apapalia, jrales, kdoherty, jleonard}@mit.edu 2 Department of Applied Ocean Physics & Engineering, Woods Hole Oceanographic Institution, Woods Hole, MA 02543, USA 3 Department of Electrical & Computer Engineering, Northeastern University, Boston, MA 02115, USA, d.rosen@northeastern.edu 4 Department of Math, Northeastern University, Boston, MA 02115, USA Fig. 1. Key concepts. (A) An example multi-robot range-aided SLAM problem. Red circles are problem variables (i.e., robot poses and beacon position), green squares are relative pose measurements (e.g., odometry), and blue squares are distance measurements (e.g., acoustic ranging). (B) A second-order cone program (SOCP) is derived as a convex relaxation of the original non-convex nonlinear least-squares (NLS) formulation of the range-aided SLAM problem. The solution to the SOCP is then used to obtain the initial estimate for the NLS problem, and the NLS problem is solved to refine the SOCP initialization. (C) Illustration of contributions. The range-aided SLAM problem is first represented as a non-convex NLS problem. This work shows how to reformulate the NLS problem into a non-convex quadratically constrained quadratic program (QCQP) and how to relax the QCQP into a convex SOCP. The SOCP can be solved to obtain an initial estimate for the original NLS problem. This work approaches initialization for the non-convex MAP estimation problem through convex relaxation [6], the construction of a convex optimization problem which attempts to approximate the original problem. Convex problems can be efficiently solved to global optimality, and many convex relaxations have shown success as initialization strategies in related robotic problems [7]–[10]. This work presents SCORE as a novel methodology for initializing RA-SLAM problems. SCORE applies to 2D and 3D scenarios with arbitrary numbers of robots and landmarks. As SCORE is a SOCP, it is easily implemented in existing convex optimization libraries, and scales gracefully to large problems. We summarize our contributions as follows: • A novel, convex-relaxation approach to initializing RA- SLAM problems. • The first QCQP formulation of RA-SLAM, connecting RA-SLAM to a broader body of work [8]–[11]. • Our implementation of SCORE is open-sourced1. 1https://github.com/MarineRoboticsGroup/score 2023 IEEE International Conference on Robotics and Automation (ICRA 2023) May 29 - June 2, 2023. London, UK 979-8-3503-2365-8/23/$31.00 ©2023 IEEE 10637