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 basedNormal 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.


INTRODUCTION
For Inertial Measurement Unit (INS)/ Global Navigation Satellite System (GNSS)/ Light Detection and Ranging (LiDAR) multi-sensor fusion, it requires initial poses to locate the position of LiDAR. In this study, we utilize Inertial Navigation System (INS) and differential GNSS (DGNSS) to output integration solution as initial pose, and apply extended Kalman Filter (EKF) as framework (Chiang et al., 2020); at the same time, faults in measurements are preliminarily isolated (Li et al., 2019). With initial pose, the relative translation and rotation relationship between each sensor frame can be converted by Direct Georeferencing (DG) into mapping frame, while mapping frame refers to the local level frame (Kai-Wei et al., 2019).
Numerous studies have been conducted to the development of LiDAR point cloud registration. The point-based Iterative Closest Point (ICP) method is widely applied, especially it can be deformed into feature-based scan matching, which can additionally reduce computational time and fulfil real-time odometry and mapping (Zhang and Singh, 2014). However, considering the remained faults involves in initial pose, it might lead to point cloud false initial alignment. In this case, Normal Distribution Transform (NDT) can cope with larger range of initial pose offset and reduce more computational time than point-based ICP (Magnusson, 2009;Magnusson et al., 2009).
With High Definition Map (HD Map) treated as fixed map for NDT scan matching can make the autonomous driving more * Corresponding author: Y.-T. Chiu practical (Carballo et al, 2020;Liu, Wang, and Zhang, 2020). But the construction of HD Map is time-consuming, labor-wasting, and costly, which is not applicable immediately for each place in the world. Under this circumstance, Autoware launched an opensource software, the Eagleye, which can operate under the circumstances with and without the assistance of HD Map. In this research, we tried to simulate the situation while the vehicle driving from the environment with HD Map to without HD Map. To this end, reliable reference is provided for the first epoch as initial guess to simulate the circumstance with HD Map. As for the rest of the epochs, it only relies on initial poses for LiDAR-SLAM-based navigation system.
To the state-of-the-art extent, seldom paper deals with error drift in height after NDT registration, which will lead to false mapping results. Meanwhile, there's lacking reliable reference data to validate the accuracy of estimated navigation solution. Therefore, this paper raises three contributions as follows: 1) To meet feasible cost in application of LiDAR for future development in autonomous vehicle applications, the lowcost LiDAR, single VLP16 is selected.
2) To isolate the faults in INS/ GNSS/ LiDAR, the Fault Detection, Isolation and Exclusion scheme (FDIE) is applied in the process of generating initial poses and LiDAR-SLAM-based estimated solution.
3) To evaluate the estimation navigation solution, a robust reference data is obtained by integrating navigation grade IMU and GNSS with forward and backward smoothing process, which is realized by utilizing commercial INS/GNSS processing software, the Inertial Explore (IE).

Sensor Fusion Scheme
The proposed LiDAR-SLAM-based navigation structure is illustrated in Figure 1. The Initial guess for LiDAR-SLAM Navigation Estimation includes the one of INS/ GNSS integrated solution from Extended Kalman Filter (EKF) or after Rauch-Tung-Striebel (RTS) smoother for navigation and mapping purpose, respectively (Särkkä, 2008;Chiang et al., 2009), which predicts and updates the velocity, position, and attitude information; meanwhile, it feedbacks the bias and scale factor from EKF back to INS mechanism. The LiDAR-SLAM process includes point cloud pre-processing, DG, and 3D NDT scan matching. The FDIE mechanism involves the two-step-functions, which are for LiDAR Odometry (LO) and LiDAR Mapping (LM).

Figure 1.
Overview of LiDAR-SLAM-based navigation system structure.

INS/ GNSS Integrated Navigation Solution
In this research, the INS and DGNSS integration system is developed utilizing Loosely Coupled (LC) scheme. The state vector for EKF input is shown in Equation 1 (Shin and El-Sheimy, 2002;Shin, 2005), each elements involves values in x, y, and z three axes.

= [ ] 21×1
(1) where is the position locate under Earth frame (latitude, longitude, height); is velocity records in North, East, and Downward sequence; is the attitude rotates from body frame to mapping frame; is the bias of accelerometer in IMU; is the bias of gyroscope in IMU; and are the scale factor of accelerometer and gyroscope respectively.
With state vector, it can construct two functions. One is function f, by utilizing state vector at epoch k to compute the predicted state vector of next epoch k+1. Another is function h, which can compute the measurements update of epoch k+1 by the predicted state from function f multiply with observation matrix H, observation error is also considered. Due to the reason that state transition and measurement update model are nonlinear, the previous functions shall pass through partial derivatives before putting into Kalman Filter, which can divide into two parts: prediction and measurement update. Prediction process estimates the state and noise at epoch k+1 from observation at epoch k, the estimated values from prediction are denoted with (-) superscript.
The measurement update process is conducted by updating the state and noise prediction by observation at epoch k+1. It puts the +1 as Kalman gain into consideration. When the prediction model is more reliable, the weight for residual will be smaller, and vice versa.

Fault Isolation Method for Update State
Theoretically speaking, errors in INS and GNSS measurement propagate while executing Kalman Filter, especially under GNSS challenging environment, which will lead to unreliable initial poses for LiDAR-SLAM. Therefore, faults in INS/ GNSS integrated solution shall be isolated before update state.
During GNSS measurement update, position and velocity differences between INS and GPS measurement, 1 , are computed at time +1, and is subtracted to differences from prediction state ( + 1) (Li et al., 2019).

Direct Georeferencing
To navigate and locate the position of the vehicle, it has to integrate each auxiliary sensor. Yet each of them is mounted separately on the vehicle, with each specific frame and axes. Figure 2 shows the transformation relationship within each sensor frame, which forms by transform matrix ( , ), where and are the rotation vector and translation vector measured in the subscript-frame with respect to the superscript-frame, respectively. In this research, after multiplying with transformation matrix, auxiliary data are located under uniform frame, the mapping frame. The process of transforming data from one specific frame to another, is so-called Direct Georeferencing (DG).

Figure 2. Transformation relationship between different
frames.
The INS/ GNSS integrated solution has dealt with coordinate transformation of GNSS position data from Ellipsoidal coordinates to mapping frame in IMU center, which is done in previous sensor fusion step. While the point cloud is located under LiDAR frame, with INS/ GNSS integrated solution as the initial pose for LiDAR frame, it has to compensate boresight ( ) and lever arm ( ) of three axes back to LiDAR center.

3D Normal Distribution Transform
With initial poses from INS/ GNSS integrated solution and consecutive frames of point cloud. Figure 3 is the block diagram that shows the proposed scheme of point cloud scan matching. In this research, we applied point-to-distribution NDT (P2D-NDT) as the NDT scan matching method. The scheme can be organized into four categories, which are introduced individually.

Point Cloud Pre-processing and DG process
1) Distortion Correction: Points from LiDAR are emitted and reflected by turns. However, for the points in the same scan, they are recorded with same time stamp and origin, which will cause point cloud distortion. Under this circumstance, by dividing the elements (i.e., time difference of each scan, position, and attitude transformation in three axes) with the number of point cloud. Then it can correct the distorted point cloud back to the position during scanning.
2) Extraction: For the points which Euclidean distance are larger than a certain distance, the pulses may be refracted by some obstacles in line-of-sight (LOS). Meanwhile, for the points that are too close to LiDAR, also might be refracted by adjacent sensors. Therefore, only points within region of interest (ROI) are extracted to process NDT.
3) DG process: With the initial pose from INS/ GNSS integrated solution, it transforms the previous point cloud to mapping frame with rotation and translation alignment. For current frame of point cloud, it only preliminarily aligns the rotation direction.

P2D-NDT Scan Matching
Based on P2D-NDT concept, it registers the points from current frame , to the distribution of previous frame ( , Σ), while the latter is previously divided by predefined voxel size.
where is point cloud and n is number of points in the specific voxel; q is the mean value and Σ is the covariance value of the voxel.
In general, the current frame in this case is so-called the moving scan. As for the previous frame, in this research, we apply the sliding window by merging previous five scans as fixed map. Therefore, it constructs the scan to map scan matching.
The scan matching works by iteratively update the transformation relationship ( , ) to match the moving scan to fixed map, where and represents the rotation and translation in three axes.
With iteratively calculate the score that represents the scan matching performance, the optimal registration outcome occurs with Newton's algorithm optimization since the − falls into global minimum.
The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2022 XXIV ISPRS Congress (2022 edition), 6-11 June 2022, Nice, France where indicates as the points in moving scan; is the Hessian matrix and is the transposed gradient vector.

Iterative Discretization Method NDT Scan Matching
The voxel size defines the resolution of NDT, which also affects the scan matching performance to a certain extent (Pang et al., 2018). As the pure NDT scan matching mentioned ahead, it utilizes the fix voxel size for one round scan matching. On the contrary, Iterative Discretization Method NDT (IDM-NDT) process the scan matching for several times with different voxel resolution. Meanwhile, the transformation matrix output from previous scan matching, is treated as the initial transform for the next round scan matching. With refined initial guess for scan matching, it will theoretically turn out a better registration outcome.

NDT Registration Mechanism
Generally speaking, although IDM-NDT can turn out more reliable registration outcome, the computational time makes it uneasily realized in real time. At the same time, some situation is robust enough to perform pure NDT. Therefore, considering the characteristic of P2D-NDT, for determination of whether to process the IDM-NDT, following shows the NDT registration mechanism that judges the moving scan situation by two conditions. 1) Number of Clusters: After ground point removal, we divide the point cloud by Euclidean distance-based method into several clusters. Through segmentation, only robust feature points are remained, which points out that NDT might perform steady under this circumstance. More details about segmentation are shown in Table 1. 2) Point Cloud Distribution Analysis: After segmentation, point cloud is divided into four directions, which are forward, backward, right-hand, and left-hand, Figure 4 shows the schematic graph of point cloud distribution analysis. Furthermore, forward and backward point clouds are gathered into along-track point cloud, while along-track means the longitudinal or the movement direction of vehicle. As for right-hand and left-hand point clouds, they are gathered into cross-track point cloud, while the cross-track means the lateral direction with respect to the right-handed system. The more even distribution of point cloud, the better the constraints work in each direction while performing NDT.

Point Cloud Clustering Settings
If the current point cloud satisfied with either one condition, which means the number of clusters or the distribution percentage of point cloud in along-track are larger than the predefined thresholds. It will go through the pure NDT scan matching to update the pose; otherwise, it will process IDM-NDT (indicated by blue arrows in Figure 3).

Fault Detection, Isolation and Exclusion Scheme
While processing NDT algorithm, the error in height and rotation angle in roll will drift over time. It will lead to error propagation and cause NDT failure, resulting from missregistration caused by large offset in translation and rotation . To this end, a FDIE mechanism (Akai et al., 2017) is added after NDT process, to detect the fault and try to exclude it out of LiDAR-SLAM-based navigation solution. The concept of FDIE applied in this research is described in Figure 5. The FDIE mechanism is operated in two-steps: the LiDAR Mapping and LiDAR Odometry, respectively.

FDI Scheme for LiDAR Mapping
Due to NDT is sensitive to the height offset, FDI for LiDAR Mapping can constraint the translation offset in height and rotation in roll, then update the transformation matrix to register the point cloud again, in order to build robust Dynamic Map for autonomous vehicle while under the circumstance without HD map assistance. More details about the constraints for height and roll is mentioned in Figure 6.

FDE Scheme for LiDAR Odometry
For the FDE in LiDAR Odometry, it makes a comparison between INS/ GNSS and INS/ GNSS/ LiDAR estimated solution, then output the relatively accurate one and treats as an estimated navigation solution of LiDAR-SLAM. Therefore, it can remove the faults generate from NDT.

Experiment Setup
Sensors for experiment setup is mounted as Figure 7 and 8 show. Horizontally mounted LiDAR, the VLP16, and the antennas are installed on the top of the vehicle. The initial guesses of INS and GNSS are received by tactical grade IMU, the Novatel PwrPak7; while the reference is collected by the navigation grade IMU GNSS, iMAR-RQH. The reference is computed by commercial INS/GNSS processing software with EKF under Tightly Coupled (TC) scheme in two-step smoothing, the forward and backward direction sequentially. For more information about the performance of two IMUs is shown in Table 2 Table 3. iMAR-RQH performance specification.

Test Field Selection
The experiment takes place around National Cheng Kung University campus, which is surrounded by tall buildings in urban area. As Figure 9 shows, the route is about 1.5 km, red line is depicted by the reference data. Hence, it forms a GNSS challenging environment, involving signal block, multipath and Non-Line-Of-Sight (NLOS) effects, leading to worse initial pose offset to a large extent.

RESULT AND DISCUSSION
According to the methods and fault detection mechanisms proposed in this research, following results are shown in sequence of fulfilling LiDAR-SLAM.  In general terms, the error in three axes is small enough. It points out that the conventional navigation method can provide accurate positioning in open sky area to a certain extent. Table 5 shows the INS/ GNSS with Fault isolation method as the initial pose for LiDAR-SLAM. However, point cloud collected by VLP16 is too sparse, which might lead to misregistration. Especially error in height will drift over time, causing the fixed frame is unrobust for scan matching. The decreasing improvement percentage shows the worse navigation after pure P2D-NDT.  Table 5. Performance analysis of initial pose with NDT.

Position
With the application of Fault Detection, Isolation and Exclusion scheme, faults in height, roll, and NDT transformation matrix are excluded. Hence, the accuracy of LiDAR-SLAMbased estimated solution is enhanced, the maximum error in height is remarkably reduced, the improvement in 2D reached 11%, 8% in 3D, as Table 6 shows. Dynamic map of the whole trajectory is depicted by LiDAR-SLAM-based estimated solution, plotted in Figure 10.  Table 6. Performance analysis of initial pose with NDT and Fault detection, isolation, and exclusion scheme. To make a contrary among each estimated solution with the reference, the whole trajectories are depicted in Figure 11. It can slightly find out that LiDAR-SLAM-based estimated solution is unstable in several environments. Figure 12 zooms in the trajectories where exist large navigation errors. As Figure 12 shows, the trajectories are stable under open sky area in (a). Yet they are misregistered in the intersections in (b), due to moving objects surrounded will disturb the matching results. In GNSS challenging environment (c) and (d), tall buildings affect the GNSS signal, which leads to initial pose offsets and cause NDT false scan matching.

CONCLUSION
This paper applies the low-cost LiDAR VLP16 for multisensor fusion scheme. In spite of the sparse point cloud, with INS/ GNSS integrated solution from EKF can provide relatively reliable initial pose for scan matching. Moreover, the proposed Fault Detection, Isolation and Exclusion method excludes the faults in each measurement, which can turn out more robust LiDAR-SLAM estimated navigation solution.
To sum up, to meet feasible requirement for autonomous vehicle, low-grade sensor fusion can fulfil this target and reach higher automation level, especially in GNSS challenging environment. For the further progress, the estimated solution The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2022 XXIV ISPRS Congress (2022 edition), 6-11 June 2022, Nice, France