Real-Time Autonomous Vehicle Localization Based on Particle and Unscented Kalman Filters

Abstract

In this paper, a real-time Monte Carlo localization (RT_MCL) method for autonomous cars is proposed. Unlike the other localization approaches, the balanced treatment of both pose estimation accuracy and its real-time performance is the main contribution. The RT_MCL method is based on the fusion of lidar and radar measurement data for object detection, a pole-like landmarks probabilistic map and a tailored particle filter for pose estimation. The lidar and radar are fused using the unscented Kalman filter (UKF) to provide pole-like static-object 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 algorithm to represent each pole landmark in the form of a source-point model to reduce computational cost and memory requirements. A reference map that includes pole landmarks is generated offline and extracted from a 3-D lidar to be used by a carefully designed particle filter for accurate ego-car localization. The particle filter is initialized by the fused GPS + IMU measurements 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 the iterative closest point algorithm. The RT_MCL 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 localization and lateral localization.