Exploiting the intrinsic structures of simultaneous localization and mapping

University of Technology Sydney. Faculty of Engineering and Information Technology.Imagine a robot which is assigned a complex task that requires it to navigate in an unknown environment. This situation frequently arises in a wide spectrum of applications; e.g., surgical robots used for diagnosis and treatment operate inside the body, domestic robots have to operate in people’s houses, and Mars rovers explore Mars. To navigate safely in such environments, the robot needs to constantly estimate its location and, simultaneously, create a consistent representation of the environment by, e.g., building a map. This problem is known as simultaneous localization and mapping (SLAM). Over the past three decades, SLAM has always been a central topic in mobile robotics. Tremendous progress has been made during these years in efficiently solving SLAM using a variety of sensors in large-scale environments. However, some of the intrinsic structures of SLAM are yet to be fully understood and utilized. This thesis is devoted to the study of some of these structures. This thesis is comprised of two main parts: In the first part, we examine the specific structure of the standard measurement models in pose-graph and feature-based SLAM. These standard models are affine with respect to the position of robot and landmarks. Consequently, given the orientation of the robot, the conditionally-optimal estimate for the robot’s and landmarks’ positions can be recovered instantly by solving a sparse linear least squares problem. We propose an algorithm to exploit this intrinsic property of SLAM, by stripping the problem down to its nonlinear core, while maintaining its natural sparsity. By taking advantage of the separable structure of SLAM, we gain a global perspective that guides the conventional local search algorithms towards a potential solution, and hence achieve a fast and stable convergence. In the second part of this thesis, we investigate the impact of the graphical structure of SLAM and several other estimation-over-graph problems, on the estimation error covariance. We establish several connections between various graph-connectivity measures and estimation-theoretic concepts. For example, we prove that, under certain conditions, the determinant of the estimation error covariance matrix of the maximum likelihood estimator can be estimated by counting the weighted number of spanning trees in the corresponding graph, without solving the optimization problem or using any information about the “geometry” of the scene (e.g., numerical values of the measurements). This surprising result shows that the graphical structure of SLAM provides a compact, but rich representation of the underlying estimation problem, that can be used—e.g., in decision making and active SLAM—to predict and reason about the resulting estimation error covariance. Consequently, we tackle the combinatorial optimization problem of designing sparse graphs with the maximum weighted number of spanning trees. Characterising graphs with the maximum number of spanning trees is an open problem in general. To tackle this problem, we establish several new theoretical results, including the monotone log-submodularity of the weighted number of spanning trees in connected graphs. By exploiting these structures, we design a complementary pair of near-optimal efficient approximation algorithms with provable performance guarantees and near-optimality certificates. We discuss several applications of our results and apply our algorithms on the measurement selection problem in SLAM to design sparse near-D-optimal (determinant-optimal) SLAM problems

Near-Optimal Budgeted Data Exchange for Distributed Loop Closure Detection

Inter-robot loop closure detection is a core problem in collaborative SLAM (CSLAM). Establishing inter-robot loop closures is a resource-demanding process, during which robots must consume a substantial amount of mission-critical resources (e.g., battery and bandwidth) to exchange sensory data. However, even with the most resource-efficient techniques, the resources available onboard may be insufficient for verifying every potential loop closure. This work addresses this critical challenge by proposing a resource-adaptive framework for distributed loop closure detection. We seek to maximize task-oriented objectives subject to a budget constraint on total data transmission. This problem is in general NP-hard. We approach this problem from different perspectives and leverage existing results on monotone submodular maximization to provide efficient approximation algorithms with performance guarantees. The proposed approach is extensively evaluated using the KITTI odometry benchmark dataset and synthetic Manhattan-like datasets.Comment: RSS 2018 Extended Versio

Energy-Aware, Collision-Free Information Gathering for Heterogeneous Robot Teams

This paper considers the problem of safely coordinating a team of sensor-equipped robots to reduce uncertainty about a dynamical process, where the objective trades off information gain and energy cost. Optimizing this trade-off is desirable, but leads to a non-monotone objective function in the set of robot trajectories. Therefore, common multi-robot planners based on coordinate descent lose their performance guarantees. Furthermore, methods that handle non-monotonicity lose their performance guarantees when subject to inter-robot collision avoidance constraints. As it is desirable to retain both the performance guarantee and safety guarantee, this work proposes a hierarchical approach with a distributed planner that uses local search with a worst-case performance guarantees and a decentralized controller based on control barrier functions that ensures safety and encourages timely arrival at sensing locations. Via extensive simulations, hardware-in-the-loop tests and hardware experiments, we demonstrate that the proposed approach achieves a better trade-off between sensing and energy cost than coordinate-descent-based algorithms.Comment: To appear in Transactions on Robotics; 18 pages and 16 figures. arXiv admin note: text overlap with arXiv:2101.1109

Present and Future of SLAM in Extreme Underground Environments

This paper reports on the state of the art in underground SLAM by discussing different SLAM strategies and results across six teams that participated in the three-year-long SubT competition. In particular, the paper has four main goals. First, we review the algorithms, architectures, and systems adopted by the teams; particular emphasis is put on lidar-centric SLAM solutions (the go-to approach for virtually all teams in the competition), heterogeneous multi-robot operation (including both aerial and ground robots), and real-world underground operation (from the presence of obscurants to the need to handle tight computational constraints). We do not shy away from discussing the dirty details behind the different SubT SLAM systems, which are often omitted from technical papers. Second, we discuss the maturity of the field by highlighting what is possible with the current SLAM systems and what we believe is within reach with some good systems engineering. Third, we outline what we believe are fundamental open problems, that are likely to require further research to break through. Finally, we provide a list of open-source SLAM implementations and datasets that have been produced during the SubT challenge and related efforts, and constitute a useful resource for researchers and practitioners.Comment: 21 pages including references. This survey paper is submitted to IEEE Transactions on Robotics for pre-approva