Algorithmic Advances in Range-Aided Navigation

Abstract

This thesis contributes to the advancement of range-aided simultaneous localization and mapping (RA-SLAM) through algorithmic developments and real-world demonstrations. Broadly speaking, SLAM is the process by which an agent combines sensor measurements to simultaneously create a map of the world and localize itself within this map. SLAM has been called the ‘holy grail’ of field robotics, and in many instances it is a critical enabling capability for autonomous agents to operate in the real world. RA-SLAM is the specific case of SLAM which incorporates point-to-point distance measurements (e.g., distance measurements between an autonomous underwater vehicle and an acoustic buoy) into the inference process. The ability to leverage such measurements is desirable, as they can help in resolving ambiguities (e.g., am I in hallway A or B) and the relevant sensors are often low-cost and simple to integrate (and thus pose the potential to be widely deployed). However, there are theoretical challenges that have historically limited the reliability of RASLAM approaches. At the root of these challenges is the issue that a single range measurement does not uniquely determine the relative position between two points. In state-of-the-art RASLAM formulations, this ambiguity manifests as non-convexity in the maximum a posteriori inference problem. As a result of this non-convexity, standard local-search optimizers are highly dependent on quality initializations to obtain the correct state estimate. To address this issue of reliability, this thesis presents the first certifiably correct algorithm for RA-SLAM. This algorithm, Certifiably Correct RA-SLAM (CORA), is capable of (i) obtaining globally optimal solutions for many real-world RA-SLAM problem instances and (ii) providing certificates of correctness for these solutions. CORA leverages a novel semidefinite programming (SDP) relaxation of the RA-SLAM problem, which it solves efficiently using the Riemannian Staircase methodology. This methodology allows CORA to typically obtain globally optimal solutions faster than the existing state-of-the-art local solvers. These results expand our understanding of problems suited for efficient global solvers and highlight the key problem structures that appear necessary to develop and deploy such solvers, pointing towards exciting future directions in trustworthy model-based autonomy. We demonstrated the performance of CORA on a range of real-world RA-SLAM datasets, including a set of large-scale multi-agent experiments conducted as part of this work. In these experiments CORA reliably estimates agents’ trajectories in both single- and multi-robot settings. CORA gracefully scales to large problems consisting of multiple agents and tens of thousands of robot poses. These experiments not only validate CORA’s performance, but also fill an existing gap in open-source datasets available to the research community and provide practical insights to guide future deployments of autonomous navigation systems in large, complex environments.