Bayesian Localization in Real-Time using Probabilistic Maps and Unscented-Kalman-Filters

Localization for Autonomous Driving

Abstract

In this paper, based on the fusion of Lidar and Radar measurement data, high-definition probabilistic maps, and a tailored particle filter, a Real-Time Monte Carlo Localization (RT_MCL) method for autonomous cars is proposed. The lidar and radar devices are installed on the ego car, and a customized Unscented Kalman Filter (UKF) is used for their data fusion. Lidars are accurate in determining objects' positions and have a much higher spatial resolution. On the other hand, Radars are more accurate in measuring objects velocities and perform well in extreme weather conditions. Therefore, the merits of both sensors are combined using the UKF to provide pole-like static-objects pose estimations that are well suited to serve as landmarks for vehicle localization in urban environments. These pose estimations are then clustered using the Grid-Based Density-Based Spatial Clustering of Applications with Noise (GB-DBSCAN) algorithm to represent each pole landmarks in the form of a source-point model to reduce computational cost and memory requirements. A reference map that includes pole landmarks is generated off-line and extracted from a 3-D lidar to be used by a carefully designed Particle Filter (PF) for accurate ego-car localization. The particle filter is initialized by the combined GPS+IMU reading and used an ego-car motion model to predict the states of the particles. The data association between the estimated landmarks by the UKF and that in the reference map is performed using Iterative Closest Point (ICP) algorithm. The proposed pipeline is implemented using the high-performance language C++ and utilizes highly optimized math and optimization libraries for best real-time performance. Extensive simulation studies have been carried out to evaluate the performance of the RT_MCL in both longitudinal and lateral localization.