Consistent decentralized cooperative localization for autonomous vehicles using LiDAR, GNSS, and HD maps

To navigate autonomously, a vehicle must be able to localize itself with respect to its driving environment and the vehicles with which it interacts. This study presents a decentralized cooperative localization method. It is based on the exchange of local dynamic maps (LDM), which are cyber-physical representations of the physical driving environment containing poses and kinematic information about nearby vehicles. An LDM acts as an abstraction layer that makes the cooperation framework sensor-agnostic, and it can even improve the localization of a sensorless communicating vehicle. With this goal in mind, this study focuses on the property of consistency in LDM estimates. Uncertainty in the estimates needs to be properly modeled, so that the estimation error can be statistically bounded for a given confidence level. To obtain a consistent system, we first introduce a decentralized fusion framework that can cope with LDMs whose errors have an unknown degree of correlation. Second, we present a consistent method for estimating the relative pose between vehicles, using a two-dimensional LiDAR (light detection and ranging) with a point-to-line metric within an iterative-closest-point approach, combined with communicated polygonal shape models. Finally, we add a bias estimator to reduce position errors when nondifferential GNSS (global navigation satellite system) receivers are used, based on visual observations of features geo-referenced in a high-definition map. Real experiments were conducted, and the consistency of our approach was demonstrated on a platooning scenario using two experimental vehicles. The full experimental data set used in this study is publicly available.