[Paper Review] A General Framework for Multi-vehicle Cooperative Localization Using Pose Graph

Xiaotong Shen, Hans Jørgen Andersen+4 more|arXiv (Cornell University)|Apr 5, 2017

Vehicle License Plate Recognition22 references3 citations

TL;DR

This paper presents a robust, general-purpose framework for multi-vehicle cooperative localization using pose graphs that integrates spatial relative observations from vehicle-to-vehicle (V2V) communication. By formulating vehicle identification as a linear assignment problem and resolving pose estimation ambiguities through data association, the method improves localization accuracy and precision even under communication delays, packet loss, and out-of-sequence reception, reducing position uncertainty by up to 0.46 meters and orientation uncertainty by 2.9 degrees compared to independent localization.

ABSTRACT

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.

Motivation & Objective

• To develop a cooperative localization framework that enhances localization accuracy and precision in multi-vehicle systems using spatial relative observations.
• To address the challenge of vehicle identification in V2V communication, where vehicles share the same spectrum and cannot be distinguished by signals alone.
• To ensure robustness against communication constraints such as delay, packet loss, and out-of-sequence reception in real-world driving scenarios.
• To resolve ambiguities in relative pose estimation (e.g., ±90° or ±180° yaw ambiguity) before integrating observations into the pose graph.
• To extend the applicability of pose graph-based localization by incorporating more sensing measurements, including inter-vehicle relative observations, beyond traditional odometry and map measurements.

Proposed method

• Formulates vehicle identification as a linear programming (LP) problem to match sensor observations with communicated messages, enabling accurate data association.
• Integrates spatial relative observations—derived from LIDAR or other sensors—into a pose graph after resolving pose ambiguities using cooperative estimation.
• Employs factor composition/decomposition techniques to maintain graph connectivity and robustness under delayed or lost messages.
• Uses graph marginalization via generic linear constraints (GLC) to enable real-time computation and efficient pose estimation.
• Applies a time-windowed pose graph approach (10-second window) to store and process delayed or out-of-sequence measurements.
• Combines odometry, map measurements, and inter-vehicle relative observations into a unified factor graph for optimal pose estimation.

Experimental results

Research questions

• RQ1 How can vehicle identification be reliably performed in V2V communication when vehicles use the same spectrum and lack unique transmission signals?
• RQ2 How can pose graph-based cooperative localization maintain accuracy and robustness under communication delays, packet loss, and out-of-sequence message reception?
• RQ3 What is the impact of spatial relative observations on improving localization precision in multi-vehicle systems?
• RQ4 How can ambiguities in relative pose estimation (e.g., ±90° or ±180° yaw) be resolved before integrating observations into the pose graph?
• RQ5 To what extent does the proposed framework reduce localization uncertainty compared to independent localization?

Key findings

• The proposed framework reduces average position uncertainty by 0.46 meters and orientation uncertainty by 2.9 degrees compared to independent localization (IL) across three vehicles.
• Vehicle 1 achieved 0.33 m average position uncertainty and 2.77° average orientation uncertainty with the cooperative localization (CL) method, compared to 0.80 m and 6.24° with IL.
• The system demonstrated robustness to communication delays, with maximum delay measured at 39 ms and average delay well below 10-second pose graph window, ensuring timely integration of delayed messages.
• Packet sizes across all message types were under 500 bytes, confirming low-latency communication suitable for real-time pose graph updates.
• The pose graph construction remained connected and consistent even under communication loss, thanks to factor composition/decomposition techniques.
• Real-world experiments on a road with three vehicles confirmed the framework’s effectiveness, with significant uncertainty reduction observed at key junctions like Y-Junctions and curvy road segments.