MAC: Maximizing Algebraic Connectivity for Graph Sparsification

Simultaneous localization and mapping (SLAM) is a critical capability in autonomous navigation, but memory and computational limits make long-term application of common SLAM techniques impractical; a robot must be able to determine what information should be retained and what can safely be forgotten. In graph-based SLAM, the number of edges (measurements) in a pose graph determines both the memory requirements of storing a robot's observations and the computational expense of algorithms deployed for performing state estimation using those observations, both of which can grow unbounded during long-term navigation. Motivated by these challenges, we propose a new general purpose approach to sparsify graphs in a manner that maximizes algebraic connectivity, a key spectral property of graphs which has been shown to control the estimation error of pose graph SLAM solutions. Our algorithm, MAC (for maximizing algebraic connectivity), is simple and computationally inexpensive, and admits formal post hoc performance guarantees on the quality of the solution that it provides. In application to the problem of pose-graph SLAM, we show on several benchmark datasets that our approach quickly produces high-quality sparsification results which retain the connectivity of the graph and, in turn, the quality of corresponding SLAM solutions.

Certifiably Correct Range-Aided SLAM

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.

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.

On Reference Solutions to Non-Gaussian SLAM Factor Graphs

Many real-world applications of simultaneous localization and mapping (SLAM) require approximate inference approaches, as exact inference for high-dimensional non-Gaussian posterior distributions is often computationally intractable. There are substantial challenges, however, in evaluating the quality of a solution provided by such inference techniques. One approach to solution evaluation is to solve the non-Gaussian posteriors with a more computationally expensive but generally accurate approach to create a reference solution for side-by-side comparison. Our work takes this direction. This paper presents nested sampling for factor graphs (NSFG), a nested-sampling-based approach for posterior estimation in non-Gaussian factor graph inference. Although NSFG applies to any problem modeled as inference over a factor graph, we focus on providing reference solutions for evaluation of approximate inference approaches to SLAM problems. The sparsity structure of SLAM factor graphs is exploited for improved computational performance without sacrificing solution quality. We compare NSFG to two other sampling-based approaches, the No-U-Turn sampler (NUTS) and sequential Monte Carlo (SMC), as well as GTSAM, a state-of-the-art Gaussian SLAM solver. We evaluate across several synthetic examples of interest to the non-Gaussian SLAM community, including multi-robot range-only SLAM and range-only SLAM with ambiguous data associations. Quantitative and qualitative analyses show NSFG is capable of producing high-fidelity solutions to a wide range of non-Gaussian SLAM problems, with notably superior solutions than NUTS and SMC. In addition, NSFG demonstrated improved scalability over NUTS and SMC.

Prioritized Planning for Cooperative Range-Only Localization in Multi-Robot Networks

We present a novel path-planning algorithm to reduce localization error for a network of robots cooperatively localizing via inter-robot range measurements. The quality of localization with range measurements depends on the configuration of the network, and poor configurations can cause substantial localization errors. To reduce the effect of network configuration on localization error for moving networks we consider various optimality measures of the Fisher information matrix (FIM), which have well-studied relationships with the localization error. In particular, we pose a trajectory planning problem with constraints on the FIM optimality measures. By constraining these optimality measures we can control the statistical properties of the localization error. To efficiently generate trajectories which satisfy these FIM constraints we present a prioritized planner which leverages graph-based planning and unique properties of the range-only FIM. We show results in simulated experiments that demonstrate the trajectories generated by our algorithm reduce worst-case localization error by up to 42\% in comparison to existing planning approaches and can scalably plan distance-efficient trajectories in complicated environments for large numbers of robots.

Network Localization Based Planning for Autonomous Underwater Vehicles with Inter-Vehicle Ranging

Localization between a swarm of AUVs can be entirely estimated through the use of range measurements between neighboring AUVs via a class of techniques commonly referred to as sensor network localization. However, the localization quality depends on network topology, with degenerate topologies, referred to as low-rigidity configurations, leading to ambiguous or highly uncertain localization results. This paper presents tools for rigidity-based analysis, planning, and control of a multi-AUV network which account for sensor noise and limited sensing range. We evaluate our long-term planning framework in several two-dimensional simulated environments and show we are able to generate paths in feasible time and guarantee a minimum network rigidity over the full course of the paths.