Fast and Robust 6-DoF LiDAR-Based Localization of an Autonomous Vehicle Against Sensor Inaccuracy

Abstract

Precise and real-time localization is crucial for autonomous vehicles. State-of-the-art methods utilize 3D light detection and ranging (LiDAR), inertial measurement unit (IMU), and global positioning system (GPS). However, to meet real-time constraints, these methods often limit the search space to only three degrees of freedom (DoF; <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$x$</tex-math></inline-formula>, <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$y$</tex-math></inline-formula>, and <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$heading$</tex-math></inline-formula>) and rely on prior maps and IMU for estimating the <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$roll$</tex-math></inline-formula>, <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$pitch$</tex-math></inline-formula>, and <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$z$</tex-math></inline-formula> coordinates. This reliance on maps and sensors can introduce inaccuracies if they contain errors. To achieve precise localization in scenarios where IMU or map errors are present, the <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$roll$</tex-math></inline-formula>, <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$pitch$</tex-math></inline-formula>, and <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$z$</tex-math></inline-formula> coordinates must be estimated. However, incorporating these additional dimensions into the localization process may increase the processing time, rendering it unsuitable for real-time applications. Herein, we propose a precise and robust 6-DoF LiDAR localization algorithm. Instead of directly generating all 6-DoF, the proposed algorithm generates particles based on the <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$x$</tex-math></inline-formula>, <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$y$</tex-math></inline-formula>, and <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$heading$</tex-math></inline-formula> coordinates. Subsequently, it optimizes the estimation of <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$roll$</tex-math></inline-formula>, <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$pitch$</tex-math></inline-formula>, and <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$z$</tex-math></inline-formula> coordinates of each particle while maintaining a fixed number of particles. By expanding the dimensionality in this manner, we mitigate the accuracy degradation that may occur with 3-DoF positioning when dealing with faulty sensors or maps. Experimental results demonstrate that the proposed algorithm achieves satisfactory performance even in scenarios where sensor accuracy is compromised.