Collaborative simultaneous localization and mapping for autonomous vehicles

Abstract

Among today’s autonomous robotic applications, exploration missions in dynamic, unstructured and uncertain environmental conditions are quite common. Autonomous multi-vehicle systems come in handy for such exploration missions involving larger environments due to higher efficiency, reliability and robustness that can be achieved compared to a single autonomous vehicle. In the absence of a-priori feature maps, various simultaneous localization and mapping (SLAM) algorithms are used in order to improve the navigation accuracy of single-vehicle systems as a key prerequisite for exploration and mapping missions. As for multi-vehicle systems, this process is called collaborative multi-vehicle simultaneous localization and mapping (CMSLAM). In multi-vehicle systems, additional scaling problems such as consistent data fusion, bandwidth constraints and computational requirements have to be taken into account in order to be used in practical applications. The most common single-vehicle SLAM (mono-SLAM) formulation is stochastic SLAM, in which the vehicle state and landmark locations are stored in a single vector, which is propagated as a joint posterior via a recursive prediction and update (observation) process, where sensor and vehicle dynamics are modeled in state space form. In this approach, each recursion involves solving certain additional, but crucial sub-problems outside the Bayes recursion, such as clutter filtering, data association and map management. Moreover, these conventional solutions assume several key simplifications which limit their practical applicability. The most notable assumptions are clutter free measurements, ideal landmark detection and data association, and static landmarks. Several CMSLAM algorithms have been developed in the literature by extending such mono-SLAM solutions. Unfortunately almost all those algorithms inherit above mentioned issues from their single-vehicle counterpart. In the recent years an alternative solution based on the finite set statistics (FISST) has been proposed as a radically different alternative solution to the mono-SLAM problem. In this approach, the landmark map and the measurements are represented as random finite sets (RFS), and a joint posterior containing the landmark map and the vehicle state is propagated while catering for landmark detection uncertainty, landmark survival uncertainty, data association, map management and measurement clutter filtering within the SLAM filter framework. In this thesis we propose to extend this framework in order to solve the more general CMSLAM problem. The first contribution of this thesis is two new CMSLAM algorithms, which are developed using the RFS SLAM framework. The proposed formulation is based on the factorization of the full CMSLAM posterior into a product of the vehicle trajectories posterior and the landmark map posterior conditioned on the vehicle trajectories. The vehicle trajectories are estimated using a Rao-Blackwellized particle filter. The landmark map and the measurements are represented as RFSs; their dynamics are appropriately modeled using FISST; and the landmark map posterior conditioned on the vehicle trajectories is estimated using a Gaussian Mixture (GM) implementation of a probability hypothesis density (PHD) filter. The performance of the proposed solutions is evaluated and presented by benchmarking against a FastSLAM based CMSLAM solution using a simulation and a practical experiment.