Graph-Based Cooperative Localization Using Symmetric Measurement Equations

Dhiraj Gulati, Feihu Zhang, Daniel Clarke, Alois C. Knoll

TL;DR

This paper introduces a novel method for cooperative localization (CL) in autonomous vehicles that tackles the challenging problem of data association. By employing Symmetric Measurement Equations (SMEs) within a factor graph framework, the authors achieve more accurate and robust localization, especially in environments where GPS signals are unreliable, without requiring computationally expensive data association steps.

What Problem Does This Paper Address?

Precise localization is paramount for autonomous vehicles. While individual vehicle sensors like GPS and odometry provide a baseline, cooperative localization (CL) aims to enhance accuracy by allowing vehicles to share information and leverage external infrastructure sensors (like RADAR). A major hurdle in CL is "data association": correctly matching sensor measurements to the specific vehicle they belong to, especially when multiple vehicles are present and the external sensor cannot distinguish them. This paper addresses the challenge of efficiently and accurately performing CL by fusing internal vehicle sensor data with external infrastructure sensor data, specifically focusing on overcoming the data association problem.

Methods

The core of the proposed solution lies in two key components:

  • Factor Graphs: The authors model the CL problem as a factor graph. This is a graphical representation where nodes represent vehicle states (positions and orientations) and factor nodes represent the relationships (measurements and constraints) between these states. This framework allows for efficient representation and optimization of the entire system's state.
  • Symmetric Measurement Equations (SMEs): To circumvent the data association problem with external sensors (like RADAR that provide measurements but not identities), SMEs are employed. Instead of directly associating measurements, SMEs generate "pseudo-measurements" by combining measurements from multiple vehicles using symmetric functions (e.g., sums of squares or products of measurements). These pseudo-measurements are independent of the individual vehicle identities, thus eliminating the need for explicit data association. The paper proposes using SMEs as "factors" within the factor graph.

The system then uses a non-linear least squares optimization technique (specifically, the Levenberg-Marquardt algorithm) to solve the factor graph and estimate the optimal state (position and orientation) for each vehicle.

Datasets

The paper does not introduce new datasets. Instead, it relies on simulated scenarios to evaluate its proposed method. These simulations include:

  • Two, three, and five vehicles with random or constant turn/velocity trajectories.
  • Scenarios with and without GPS measurements.
  • A specific "tunnel" scenario to simulate GPS unavailability, where the infrastructure RADAR is crucial.

The simulations are performed using the Georgia Tech Smoothing and Mapping (GTSAM) framework.

Key Insights and Ideas

  • SMEs as a Data Association Bypass: The most significant insight is that SMEs can effectively bypass the computationally intensive data association problem by creating measurements that are inherently invariant to vehicle identity. This is a clever way to leverage infrastructure sensors that provide group measurements.
  • Factor Graphs for Scalability: Using factor graphs allows the system to scale well with an increasing number of vehicles. The non-linearity introduced by SMEs is managed by the optimization framework.
  • Robustness in GPS-Denied Environments: The SME factor proves particularly valuable in scenarios where GPS is unavailable (e.g., tunnels, urban canyons). In these situations, the SME factor, derived from a precise infrastructure sensor, provides crucial localization information, preventing the fused trajectories from drifting.
  • Trade-off between Information and Simplicity: While the authors' previous "topology factor" approach also avoided explicit data association, it lost positional information by only considering inter-vehicle distances. SMEs, by contrast, incorporate more information from the external sensor, leading to better accuracy.

Key Results

  • Improved Accuracy: The proposed SME factor consistently outperforms the "no additional factor" baseline and the previous "topology factor" approach in terms of Root Mean Square Error (RMSE) across various simulation scenarios.
  • Superior Performance in GPS-Denied Scenarios: In the tunnel scenario, the system with the SME factor (using RADAR) showed significantly less trajectory drift and lower RMSE compared to the system without RADAR (relying only on odometry).
  • Scalability: The factor graph approach, combined with SMEs, demonstrates good scalability with an increasing number of vehicles, as shown in the Monte Carlo simulations.
  • Efficiency: By avoiding explicit data association, the SME approach reduces computational overhead. The paper also highlights that only measurements, not covariances, need to be exchanged between nodes, minimizing bandwidth requirements.

Limitations and Future Work

  • Coordinate Transformation: The paper acknowledges that while the SME factor addresses data association and improves accuracy, it does not resolve the coordinate transformation issue (converting external sensor measurements from its local frame to a global frame). This is assumed to be handled by the static infrastructure sensor.
  • Clutter and Missed Detections: The current simulations assume a clutter-free environment with no missed detections. Real-world scenarios often involve these challenges, which would require further investigation. The authors suggest handling clutter either at the sensor level or during the optimization process.
  • Online Execution: The current implementation uses an offline batch optimizer. Future work should explore online incremental smoothing algorithms (like iSAM2) to handle measurements arriving dynamically, enabling a "plug and play" capability for sensors.
  • Sensor Fusion: Incorporating other sensor types like inertial measurement units (IMUs) and wheel encoders into the cooperative localization model is suggested for future research.
  • External Target Tracking: Extending the framework to localize and track external targets (beyond participating vehicles) is another avenue for future work.

Summary updated

Abstract

Precise localization is a key requirement for the success of highly assisted or autonomous vehicles. The diminishing cost of hardware has resulted in a proliferation of the number of sensors in the environment. Cooperative localization (CL) presents itself as a feasible and effective solution for localizing the ego-vehicle and its neighboring vehicles. However, one of the major challenges to fully realize the effective use of infrastructure sensors for jointly estimating the state of a vehicle in cooperative vehicle-infrastructure localization is an effective data association. In this paper, we propose a method which implements symmetric measurement equations within factor graphs in order to overcome the data association challenge with a reduced bandwidth overhead. Simulated results demonstrate the benefits of the proposed approach in comparison with our previously proposed approach of topology factors.
BibTeX
@inproceedings{gulati2017graph,
  author = {Dhiraj Gulati and Feihu Zhang and Daniel Clarke and Alois C. Knoll},
  title = {{Graph-Based Cooperative Localization Using Symmetric Measurement Equations}},
  booktitle = {Sensors},
  year = {2017},
  url = {https://openreview.net/forum?id=ijgXlZib_-l},
}

Loading related content...

Figure