Robust Localization Approach Using Hybrid Correspondence NDT and Real-Time Uncertainty Estimation

Abstract

This study proposes an approach for robust localization with low computational cost in autonomous vehicles. We introduce EKF localization that combines NDT and DR. To estimate the uncertainty due to the real environment of NDT, we utilize multiple searches from many initial positions. Because multiple searches increase processing time, we limit the arrangement of initial positions based on the Hessian matrix of the NDT score function. The initial position arrangement according to the Hessian matrix is based on the direction in which error is likely to occur. This allows us to achieve real-time uncertainty estimation within a LiDAR period of 10Hz. Moreover, we propose hybrid correspondence NDT (HC-NDT) to reduce the computational cost of each search. HC-NDT reduces unnecessary correspondences between query points and ND voxel in neighbor search. In the convergence evaluation, HC-NDT reduced the processing time and maintained the accuracy compared to NDT with multiple correspondences. In the localization evaluation, our EKF localization with HC-NDT and our real-time uncertainty estimation suppressed errors within 0.3 m in challenging environments for NDT. The execution time of our uncertainty estimation with HC-NDT was reduced by up to 50 % compared to conventional NDT.