A General Framework for Multi-vehicle Cooperative Localization Using Pose Graph
This work addresses localization challenges for autonomous vehicles, though it is incremental as it builds on existing pose graph and factor composition methods.
The paper tackles the problem of multi-vehicle cooperative localization by integrating spatial relative observations into a pose graph to enhance accuracy and precision, demonstrating effectiveness through simulations and real-world experiments with three vehicles.
When a vehicle observes another one, the two vehicles' poses are correlated by this spatial relative observation, which can be used in cooperative localization for further increasing localization accuracy and precision. To use spatial relative observations, we propose to add them into a pose graph for optimal pose estimation. Before adding them, we need to know the identities of the observed vehicles. The vehicle identification is formulated as a linear assignment problem, which can be solved efficiently. By using pose graph techniques and the start-of-the-art factor composition/decomposition method, our cooperative localization algorithm is robust against communication delay, packet loss, and out-of-sequence packet reception. We demonstrate the usability of our framework and effectiveness of our algorithm through both simulations and real-world experiments using three vehicles on the road.