Bounding Drift in Cooperative Localisation Through the Sharing of Local Loop Closures

Abstract

Handling loop closures and intervehicle observations in cooperative robotic scenarios remains a challenging problem due to data consistency, bandwidth limitations and increased computation requirements. This paper develops a general cooperative localisation and single vehicle Visual SLAM framework that includes direct intervehicle observations and pose to pose loop closures on each vehicle with states shared as required. This fuses single vehicle SLAM with cooperative localisation and avoids data association of map data across limited communication networks. The base problem is developed as a factor graph with each vehicle solving local subgraphs that are split based on intervehicle observations. We modify the order of variable elimination in subgraphs through manipulation of the square-root of the Information matrix to extract updates that include the historic states involved in the loop closures and do not require transmission of other states not involved in the measurement or retransmission of previously shared states. We demonstrate the effect on localisation accuracy and bandwidth using data captured from a set of five robots observing each other and landmarks compared to both single vehicle SLAM, pure cooperative localisation and a centralised solution.