A hierarchical approach to the Multi-Vehicle SLAM problem

In this paper we present a novel hierarchical solution to the Multi-Vehicle SLAM (MVSLAM) problem by extending the recently developed random finite set (RFS) based SLAM filter framework. Instead of fusing control and measurement data at each time step, we introduce a RFS Single-Vehicle SLAM based sub-mapping process, where each robot periodically produces a local sub-map of its vicinity and communicates the resultant sub-map along with the sequence of applied control commands for further fusion into a higher level MVSLAM algorithm, reducing the required network bandwidth and computational power at the fusion node. Our solution is based on the factorization of MVSLAM posterior into a product of the vehicle trajectories posterior and the landmark map posterior conditioned on the vehicle trajectory. The landmarks and the measurements are modelled as RFSs, instead of using data from exteroceptive sensors, measurements are derived from the produced local sub-maps. The vehicle trajectories posterior is estimated using a Rao-Blackwellised particle filter, while the landmark map posterior is estimated using a Gaussian mixture (GM) probability hypothesis density (PHD) filter.