IMPROVEMENT OF LiDAR-SLAM-BASED 3D NDT LOCALIZATION USING FAULT DETECTION AND EXCLUSION ALGORITHM

To meet the autopilot demand of autonomous vehicle, higher automation level accompanies with higher consideration of safety factor to improve navigation accuracy. Moreover, it shall be stable under diverse environment, e.g., semi-open sky, urban, traffic jam, etc, where conventional navigation methods, the Inertial Measurement Unit (IMU) and global Navigation Satellite System (GNSS), might be limited. Thus, auxiliary sensor, the light detection and ranging (LiDAR), is applied to provide additional information to assist navigation under GNSS challenging environment, and fulfil Simultaneous Localization and Mapping (SLAM). To initially align the LiDAR point cloud, initial pose is generated by Extended Kalman Filter (EKF) through Loosely Coupled (LC) scheme, assisting with motion constraints, including Zero Velocity Update (ZUPT), Non-Holonomic Constraints (NHC), and Zero Integrated Heading Rate (ZIHR) function. With point cloud after initial alignment, registration method applied in this research is point to distribution based-Normal Distribution Transform (P2D-NDT), with scan to dynamic map matching. However, pure LiDAR-SLAM estimated solution remains faults in each measurement, which will propagate through computation and leads to false navigation outcome. Therefore, this paper proposed Fault Detection, Isolation, and Exclusion (FDIE) scheme to exclude the faults in each step of LiDAR-SLAM process. The final estimated solution is compared to robust reference data, the results turn out that convention navigation method work well under stable GNSS signal environment, while significant accuracy enhancement is achieved with NDT and FDE under large initial pose offset, such as GNSS signal blocked area.