Certifiably Correct Range-Aided SLAM

TL;DR

The first algorithm to efficiently compute certifiably optimal solutions to range-aided simultaneous localization and mapping (RA-SLAM) problems by leveraging a novel quadratically constrained quadratic programming formulation of RA-SLAM to relax the RA-SLAM problem to a semidefinite program (SDP).

Abstract

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 connectivity and the tightness of the SDP relaxation.

Figures (8)

• Figure 1: A schematic overview of the proposed algorithm on a four robot RA-SLAM problem. (Top) the high-level flow, which, given an initial estimate, solves a semidefinite program (SDP) relaxation of the RA-SLAM problem. As the SDP solution is not necessarily feasible for the RA-SLAM problem, a final estimate to the original problem is then extracted and returned. (Bottom-Left) The Riemannian Staircase methodology used to solve the SDP, in which optimization is performed over increasingly lifted Riemannian optimization problems until a certified solution to the SDP is found. The Riemannian optimization is over a product manifold involving orthonormal frames and vectors on the unit-sphere, certification involves evaluating positive semidefiniteness of a specific matrix, and lifting is equivalent to increasing the dimensions of the product manifold. (Bottom-Right) extracting the final estimate via feasible set projection followed by Riemannian optimization.
• Figure 2: Overview of the problems stated in this paper. We summarize how each problem is derived from the preceding problem (solid arrows) and the abilities conveyed by each problem with respect to previous problems (dashed arrows). Derivations: From the MAP formulation of RA-SLAM (\ref{['prob:ra-slam-map']}), we derive a QCQP relaxation (\ref{['sec:ra-slam-as-qcqp']}). The QCQP admits a relaxation to an SDP (\ref{['prob:ra-slam-convex-sdp']}) via Shor's relaxation shor1987quadratic. The SDP can be solved as a series of rank-restricted SDPs (\ref{['prob:ra-slam-rr-sdp']}) via the Burer-Monteiro method burer03mathprog, which allows for computationally tractable solution of large-scale SDPs. Furthermore, by recognizing the manifold structure of the rank-restricted SDP constraints, the rank-restricted SDPs can be equivalently solved as manifold optimization problems (\ref{['prob:ra-slam-riemannian-staircase']}). Abilities conveyed: As the SDP is a relaxation of the MAP problem, it provides means for: (i) certifying optimality of the MAP estimate, (ii) bounding the suboptimality of a MAP estimate, and (iii) an initialization-independent approach to obtaining MAP estimates. As we can determine whether a rank-restricted SDP solves the SDP (\ref{['sec:optimality-certificates']}) and the rank-restricted SDPs can be posed as manifold optimization problems, we can efficiently solve the SDP via the Riemannian Staircase methodology (\ref{['sec:certifiably-correct-estimation']}).
• Figure 3: Drone experiment equipment: The hexcopter and ground-station used in our drone experiment.
• Figure 4: Trajectory errors across the real-world experiments. The root-mean-square error (RMSE) of the estimated trajectories are computed, with translational and rotational components treated separately. The two plots in the upper left show the results for the single robot experiments, while the two plots in the upper right show the results for the TIERS experiments. The two plots across the bottom show the results for the MR.CLAM experiments. In each plot, the trajectory errors of CORA (blue) are compared to GTSAM with various initialization strategies (see RMSE values in \ref{['tab:single-robot-errors', 'tab:multi-robot-errors', 'tab:mrclam-errors-2']}).
• Figure 5: Empirical evaluation of relative suboptimality gap. Note that the y-axis is in units of percent and that the scale of the upper row is two orders of magnitude less than the lower row. The relative suboptimality gap obtained by CORA on a series of simulated parameter sweeps as described in \ref{['sec:tightness-of-relaxation']}. Each parameter value was repeated 20 times; violin plots with strict cutoff at the minimum and maximum values are shown. (Top) the suboptimality gap for the parameter sweeps in which 200 inter-robot relative pose measurements (loop closures) were included in the problem. The case with inter-robot loop closures and no landmarks is not visible because all instances had zero relative optimality gap. (Bottom) the suboptimality gap for the parameter sweeps in which no inter-robot relative pose measurements were included.

Theorems & Definitions (1)

• Theorem 1