Lidar SLAM-Aided Vehicular Navigation System for GNSS-Denied Environments

: Typically, in situations where Global Navigation Satellite System (GNSS) signals are unavailable, navigation systems rely on integrating GNSS and inertial navigation system (INS) data. While such integration can provide accurate positioning during short GNSS signal outages, it cannot sustain prolonged GNSS outages. The reason for this is that the system's performance depends solely on the INS and can result in significant errors over time. To address this issue, additional onboard sensors are necessary. This study proposes a navigation system that integrates INS and LiDAR simultaneous localization and mapping (SLAM) using an extended Kalman filter (EKF). The system was tested using the raw KITTI dataset in various outdoor driving scenarios without GNSS signals. It is shown that the proposed system significantly outperformed the INS-only system, with an average RMSE improvement of around 93% and 58% in the horizontal and the up directions, respectively.


INTRODUCTION
The field of vehicular navigation research and development faces several obstacles, of which precision positioning is a major one.To achieve precise vehicular positioning, navigation systems must provide accurate information for all driving scenarios and weather conditions.In addition, we must ensure that these systems remain operational with high accuracy and reliability, even if one sensor fails.As such, relying on a single sensor may not be sufficient to produce a reliable navigation solution.Therefore, there is a need to integrate multiple sensors to achieve robust and accurate positioning (de Ponte Müller 2017, Martínez-Díaz and Soriguera 2018).Integrated navigation systems that combine the global navigation satellite system (GNSS) and the inertial navigation system (INS) are frequently utilized because these two systems complement each other.The GNSS and INS observations are generally integrated using a Kalman filter.(Shin 2005).The INS determines the vehicle's position through mechanization, with updates from the GNSS received at a slower rate to reduce the inertial measurement unit (IMU) drift.Integration between the two systems relies on the ability of the INS to provide frequent and accurate vehicle positioning.Consequently, the INS can maintain reliable positioning during short GNSS signal outages in a closed-loop error scheme integration.However, if the GNSS outage lasts a long time, the system will depend on the INS's performance, which is prone to significant drift, particularly when using a low-cost MEMS IMU (Abd Rabbou and El-Rabbany 2015).To enhance the navigation system's performance discussed earlier, it is necessary to include more onboard sensors that can assist with navigation, which enables the system to cope with lengthy GNSS outages.LiDAR sensors are commonly employed for localization using SLAM techniques.The fundamental concept behind SLAM algorithms is to utilize a sensor to create a map of the surrounding environment while concurrently monitoring the sensor's position.Many state-of-the-art LiDAR SLAM algorithms were developed, of which the first leading one is LiDAR odometry and mapping (LOAM) (Zhang and Singh 2014 ).It was recognized as one of the leading algorithms due to its excellent performance in the KITTI benchmark (KITTI 2013).Subsequently, updated versions of LOAM were presented, including, A-LOAM (A-LOAM 2018), LeGO-LOAM (Shan and Englot 2018), Kitware SLAM (KITWARE 2020), R-LOAM (Oelsch, Karimi et al. 2021), and F-LOAM (Wang, Wang et al. 2021).These SLAM algorithms have a common goal of improving LOAM's processing time.Numerous investigations have suggested incorporating GNSS, INS, and LiDAR SLAM.One study (Chang, Niu et al. 2019) proposed a graph optimization-based integration scheme that combines GNSS/INS with LiDAR SLAM.The study aligned the GNSS/INS outcomes with the relative pose of a 3D probability map.During a one-minute GNSS signal outage, the system's performance was evaluated.The positional root-mean-square error (RMSE) in the east and north directions decreased by about 80% in comparison to GNSS/INS estimations.In (Abdelaziz and El-Rabbany 2022), a loosely-coupled integrated navigation system that aimed to fuse the INS and LiDAR SLAM through the use of an extended Kalman filter (EKF) was developed.We assessed the effectiveness of the integrated navigation system using a variety of driving situations and environmental conditions, employing the unprocessed residential and highway datasets from the KITTI dataset.This study comprised three distinct case studies.The first case study revolved around residential datasets from the raw KITTI dataset, which covered a total driving time of 48 minutes during a complete GNSS signal blackout.The second case study focused on highway datasets with a combined duration of 7 minutes, which encompassed all accessible highway datasets within the raw KITTI data.Similar to the first case study, the LiDAR SLAM system demonstrated superior positional results when dealing with extended datasets.The third case study addressed scenarios involving intermittent GNSS signal losses, simulating situations like urban canyons and tunnels on highways.The results displayed substantial improvements compared to the same datasets in the first case study, which dealt with complete GNSS signal loss.Across all driving scenarios, the integrated INS/LiDAR SLAM navigation system exhibited significant enhancements in positional accuracy for residential datasets, achieving an average Root Mean Square Error (RMSE) reduction of 88% in the horizontal component and 32% in the vertical component.The improvements for highway datasets were approximately 70% in the horizontal component and 0.2% in the vertical component.We conducted comparisons between the proposed system and three state-of-the-art LiDAR SLAM algorithms.
In this study, we propose a different EKF than the one developed by (Abdelaziz and El-Rabbany 2022).The EKF will consider the LiDAR SLAM position estimations only as measurement updates to the INS solution.The proposed navigation system was tested in different urban and rural drives of the KITTI dataset during a complete absence of the GNSS signal.

LiDAR SLAM
Kitware SLAM is adopted in this study (KITWARE 2020).This algorithm is one of the original LOAM variants (Zhang and Singh 2014 ).The latter is composed of three main stages, namely point cloud registration, LiDAR odometry, and LiDAR mapping.A detailed description of the Kitware SLAM was presented in (Zhang andSingh 2014 , Abdelaziz andEl-Rabbany 2022).It is worth mentioning that the major improvements that Kitware SLAM over the original LOAM algorithm are computational efficiency because of using C++, independence from robot operating system (ROS), and the availability of software (LidarView) with a user-friendly graphical user interface (GUI) to perform LiDAR SLAM.

Coordinate Transformation
The LiDAR SLAM pose estimations are referenced to the local frame of the LiDAR sensor, specifically in reference to the first frame of the stream of LiDAR point clouds.For subsequent analysis to develop the integrated navigation model, the SLAM poses must be transformed into the WGS84 reference frame.This is graphically shown in Figure 1.The transformation sequence can be completed using a series of 4x4 homogenous transformation matrices as shown in Equations ( 1) and (2).

INS/LiDAR Integration
This paper employs an EKF to integrate the INS and the LiDAR SLAM into a single navigation solution, as illustrated in Figure 2. The EKF uses the raw IMU measurements of angular rotation and acceleration as inputs to a complete IMU mechanization process, which calculates the position, velocity, and attitude of the navigation system.The mathematical models for the mechanization process are implemented as described in (Noureldin, Karamat et al. 2012, Abdelaziz andEl-Rabbany 2022).In parallel, the Kitware SLAM algorithm processes the LiDAR point clouds to derive the vehicle's position and attitude.
During the update stage of the EKF, only LiDAR SLAM position estimations are used to update the IMU mechanization outputs, resulting in an integrated navigation solution.The updated errors are then fed back into the IMU mechanization, forming a closedloop error scheme.
The EKF mathematical and stochastic models are similar to the ones presented in (Abdelaziz and El-Rabbany 2022).The state vector is given by Equation (3), while the measurement model, measurement update vector, and design matrix are given by Equations ( 4), ( 5), and (6), respectively.

DATA SOURCE
This study utilizes the Karlsruhe Institute of Technology and Toyota Technological Institute (KITTI) dataset (Geiger, Lenz et al. 2013) as its primary data source, accessible through (KITTI 2013).The KITTI dataset is highly esteemed in the global research community for its value, and researchers worldwide can leverage it for fair comparisons between various algorithms and methods, particularly on the KITTI odometry benchmark.One of its standout features is its diverse sensor suite, enabling data capture from multiple perspectives.This suite comprises an integrated GNSS/IMU unit (OXTS RT3003), a 360-degree rotating mechanical LiDAR with 64 beams (Velodyne HDL-64E), and two Sony stereo pairs that capture both color and grayscale images.These sensors work in tandem to offer a comprehensive view of the surrounding environment during data collection.The dataset was collected in Karlsruhe, Germany, a location carefully chosen for its ability to present a wide array of real-world driving scenarios and environmental conditions, including urban, rural, and highway scenes.This diversity makes it applicable to a wide range of global driving applications.The KITTI dataset is organized into several categories, namely raw data, object detection and tracking, road segmentation, and odometry, each serving specific purposes in vehicular navigation and autonomous driving research.The raw data category contains unprocessed sensor measurements, including images, point clouds, and sensor calibration parameters.Researchers can use this data to develop and evaluate algorithms for tasks such as object detection, semantic segmentation, and depth estimation.Notably, the raw datasets are available in both timeunsynchronized and synchronized versions, with the latter being more commonly used.The object detection and tracking dataset provide annotated 2D and 3D bounding box information for objects like cars, pedestrians, and cyclists, facilitating the training and evaluation of related algorithms.The road segmentation data aids in road scene understanding by offering pixel-level annotations for road regions, a crucial aspect of autonomous navigation.Finally, the odometry dataset offers precise ego-motion estimates for assessing and refining localization and mapping algorithms.It comprises synchronized camera-LIDAR sequences with accurate ground truth pose information, though it lacks raw IMU measurements.As such, this study focuses on the raw dataset to harness the essential raw IMU measurements for the proposed navigation models.The structure of the KITTI raw dataset maintains consistency across all drives, organized into sequences, each representing a continuous driving segment, such as an urban area or a highway stretch.These sequences encompass various data modalities, including grayscale and color images, Velodyne point clouds, and GPS/IMU measurements.Calibration files for sensor alignment and synchronization purposes are also included.This research encompasses both residential and highway datasets.Table 1 provides details on the drive number, length, and elapsed drive time dataset.

ANALYSIS AND RESULTS
The first dataset that will be used is 2011_09_30_drive_0018_sync, which is an approximately 2200m drive that lasted for around 290 seconds.Figure 3 and Figure 4 present the position errors in the ENU reference frame (navigation frame) and attitude errors (roll, pitch, and yaw angles), respectively.Both figures showcase three navigation solutions, namely the INS, the LiDAR, and the integrated INS/LiDAR system.The root-mean-square error (RMSE) of these errors is shown in Table 2.The second dataset is a significantly shorter one, 2011_09_26_drive_0022_sync.The drive covered a distance of more than half a kilometre in around 82 seconds.Figures 5 and  6 present the position errors in the ENU reference frame and the errors of the attitude angles, respectively.These errors are quantified numerically in Table 3.For all drives, Table 4 shows the reduction in the RMSE values when using the integrated navigation system in comparison to the INS.It is noticeable from the table the improvement that the proposed system achieved, which yields an average improvement of approximately 93% and 58% in the horizontal and up directions, respectively.e. 2011_09_30_drive_0028_sync f. f. 2011_09_26_drive_0101_sync

CONCLUSIONS
In this study, we proposed a LC INS/LiDAR integrated navigation system using an EKF.To evaluate the system performance, we considered the raw KITI dataset, which includes data from residential and highway drives during the complete outrage of the GNSS signal.Six drives of the raw KITTI dataset were used for testing, which presented rural and urban driving environments at variant driving speeds.It was shown that the proposed integrated navigation system outperformed the INS-only system in all cases with an average RMSE improvement of around 93% and 58% in the horizontal and up directions, respectively.

Figure 2 .
Figure 2. A block diagram of the INS/LiDAR integrated navigation model integration

Figure 5 .Figure 6 .
Figure 5. Position errors (m) in the ENU reference frame, 2011_09_26_drive_0022_sync It is clear from Figure5that significant amount of error in the INS position estimations in reference to the ground truth.By contrast, the LIDAR SLAM position estimations are significantly more accurate in the east, north, and up directions.Similar to the first dataset, 2011_09_30_drive_0018_sync, the final integrated navigation solution was tuned to follow the position estimates produced by the LiDAR SLAM.However, in Figure6, it is noticeable that INS continues to provide more accurate and more stable attitude estimations in comparison to the LiDAR SLAM.As a result, the INS/LiDAR integrated solution considers the attitude provided by the INS.A similar analysis was conducted for the remaining drives of the KITTI dataset adopted and mentioned in Table1.The results for these drives, along with drives 2011_09_30_drive_0018_sync and 2011_09_26_drive_0022_sync are shown in Figure7.The same trends persisted regarding both the position and attitude of all datasets.All trajectories of the INS, LiDAR, and INS/LiDAR SLAM navigation solutions were compared to the ground truth trajectory in the ENU reference frame.It is worth mentioning that the ground truth in the KITTI dataset is provided by the integrated GNSS/INS unit (OXTS).

Figure 7 .
Figure 7.Comparison of navigation solutions vs ground truth trajectory in the ENU frame

Table 1 .
KITTI DRIVES DESCRIPTIVE INFORMATION

Table 2 .
Position and attitude error statistic -2011_09_30_drive_0018_syncIt is noticeable from Figure3that the LiDAR SLAM position estimations are significantly more accurate than the INS counterpart, which is typical behaviour for long drives.This is due to the sheer amount of drift that INS experiences after some time of driving, depending on the accuracy of the IMU.As a result, the EKF was tuned to follow the position of the LiDAR.By contrast, we can notice in Figure4that the INS estimations are more accurate than the LiDAR SLAM counterparts, and thereby it is obvious that the EKF follows the INS attitude estimations.These trends are numerically noticeable in Table2.

Table 3 .
Position and attitude error statistic -2011_09_26_drive_0022_sync