A Robust and Reliable Positioning Method for Complex Environments Based on Quality-Controlled Multi-Sensor Fusion of GNSS, INS, and LiDAR

Highlights

What are the main findings?

• A robust and reliable positioning method based on quality-controlled multi-sensor fusion of GNSS, INS, and LiDAR is proposed for large-scale tunnel, urban, campus, and mountain environments.
• The proposed algorithm reduces the risk of degradation and demonstrates superior adaptability in large-scale, long-duration complex environments, significantly enhancing both reliability and robustness.

What is the implication of the main finding?

• This study can improve the reliability and robustness of positioning of unmanned vehicles in real-world complex environments.
• This study provides a new method for solving the localization of lidar point cloud in scenes with sparse features and repeated features.

Abstract

The multi-source fusion localization algorithm demonstrates advantages in achieving continuous localization. However, its reliability and robustness could not be guaranteed and still with some insufficiencies in complex environments, especially for severe occlusions and low-texture scenes in non-cooperative scenarios. In this paper, we propose a GNSS/INS/LiDAR multi-source fusion localization framework. To enhance the algorithm’s performance, availability of different sensors is evaluated quantitatively through GNSS/INS status detection, and LiDAR-data-feature repeatability quality control is implemented at the front end. Both the variability of the standard deviation of differences of features and the standard deviation of real-time features are designed as major considerations and proposed to characterize the repeatability of 3D point clouds of LiDAR. The prior probability of the sensor covariance within the factor graph improves the algorithm’s fusion weight adjustment capability. Finally, a GNSS/INS/LiDAR multi-sensor positioning test platform is developed, and experiments are conducted in sheltered and semi-sheltered environments, such as urban, tunnel, campus, and mountainous environments. The results show that, compared with state-of-the-art methods, the proposed algorithm exhibits superior adaptability, significantly enhancing both reliability and robustness in four different typical real, complex environments, and our algorithm improves the robust running time by 44% in terms of availability in large-scale urban tests. In addition, the algorithm demonstrates superior positioning accuracy compared with those of other methods, achieving a positioning accuracy (RMSE) of 0.18 and 0.21 m in large-scale, long-duration urban and mountainous settings, respectively.

1. Introduction

High-precision and reliable positioning is essential for robots, unmanned vehicles, and autonomous driving. However, satellite navigation positioning [1,2] is subject to limitations in terms of availability and stability in shaded areas, such as tunnels, urban canyons, and mountainous regions. It has been established that this poses something of a challenge with regard to satisfying the real-time continuous positioning requirements of platforms operating within unknown environments [3]. The multi-source fusion positioning system [4,5] can be effectively integrated into autonomous navigation systems such as unmanned vehicles, making it a focal point in the research domain. In complex environments, the deployment of unmanned platforms necessitates the assurance of elevated levels of mobility, safety, operational reliability, and positioning accuracy. In general, multi-sensory data quality is known to be subject to variation when collected in different environments. Satellite navigation systems are susceptible to signal multipath interference in sheltered environments. The issue of sparse features and feature repetition has been identified as a significant challenge in the realm of SLAM-based LiDAR systems, particularly in real-world environments. This challenge manifests in scenarios such as large-scale scene switching and narrow tunnels, where degradation frequently occurs and can result in the collapse of the positioning system. Consequently, it is imperative to implement a rigorous data quality control process for multi-sensors to enhance the reliability of multi-source fusion systems.

Currently, sensors that are utilized within the domain of fusion positioning systems encompass a diverse array of technologies, including satellite navigation systems, inertial navigation systems, LiDAR, optical cameras, and odometers. In comparison with single-sensor approaches, multi-sensor fusion positioning offers enhanced stability and reliability. The utilization of LiDAR-inertial odometry (LIO) has been demonstrated to exhibit significant advantages over the application of LiDAR-SLAM, particularly with regard to online calibration, laser odometry state prediction, and inertial navigation error correction. Furthermore, it is also very necessary to integrate other absolute positioning factors, such as GNSS and UWB. The incorporation of absolute position information has been demonstrated to effectively improve the system’s ability to calibrate positioning deviations, thereby enhancing the overall practicality and robustness of the navigation solution. VINS-Mono [6] is a classical real-time VIO (Visual–Inertial Odometry) framework, demonstrating strong performance in autonomous state estimation and feedback control for UAVs. ORB-SLAM3 [7], which relies on feature point matching, also supports tight coupling between vision and inertial measurement units (IMU). However, optical flow algorithms and feature point matching methods all struggle to achieve ideal performance in variable weather conditions, large-scale outdoor environments, and low-texture scenes. LiDAR, with its capabilities of three-dimensional ranging, high range accuracy, and low sensitivity to illumination variations, has the advantage of application in large-scale, long-duration complex environments.

The utilization of Kalman filter techniques for the integration of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) has been thoroughly evidenced in the development and application of navigation systems [8,9]. With the assistance of network augmentation systems, pseudorange differential positioning solutions combined with inertial short-term accuracy provide nearly ideal complementary characteristics for integrated navigation and positioning technology. In complex environments such as urban canyons [10], frequent GNSS signal outages can significantly degrade the positioning accuracy and robustness of tightly-coupled GNSS/INS integration. Communication interruptions further complicate RTK convergence and lead to a substantial decline in satellite navigation accuracy.

The quality control strategy can be divided into two areas: the front end and the back end. The front end mainly involves feature extraction and sensor status monitoring. Researchers commonly employ the ICP (Iterative Closest Point) algorithm [11,12] as a front-end registration method. The proposed odometry estimation approach is based on point-to-point ICP integrated with an adaptive threshold for correspondence matching. However, an accurate initial registration value is a necessary condition of the ICP algorithm. HDL_GRAPH_SLAM [13,14] is a straightforward yet effective framework that utilizes NDT(Normal Distributions Transform) matching and integrates IMU, GNSS, and ground detection information as additional constraints for graph optimization. However, due to insufficient inter-frame matching accuracy, the positioning accuracy remains suboptimal. Additionally, the feature detection method imposes limitations on its adaptability to large-scale scenarios. Currently, the LOAM, F-LOAM, LeGO_LOAM [15,16,17] algorithm framework, which performs matching and positioning by extracting edge and planar features from LiDAR scans, has gained significant popularity. The approach utilizes high-frequency but low-precision lidar odometry in conjunction with low-frequency mapping corrections to achieve loosely-coupled real-time positioning and mapping. Nevertheless, the aforementioned algorithms are deficient in terms of quality control methodologies for feature sparsity and feature repeatability. This deficiency directly results in the occurrence of matching errors and degradation on a frequent basis.

The quality control process, which is based on back-end optimization, primarily concentrates on fusion methods and weight distribution strategies. Factor graph optimization (FGO) [18] has been shown to be advantageous when confronted with large-scale SLAM problems based on sparse structure optimization. The method based on graph optimization has the capacity to be compatible with different types of sensors and to achieve flexible mathematical modeling. Furthermore, it has been demonstrated to exhibit enhanced numerical stability. Consequently, factor graph optimization has emerged as the prevailing back-end optimization approach for multi-source fusion localization. The tightly-coupled LIO-SAM [19] system, as a state-of-the-art algorithm framework with superior performance, incorporates lidar-inertial odometry as relative measurement values and GNSS factors as absolute measurement values into the FGO to achieve global constraints, but this algorithm suffer from degradation in feature sparsity and repetitive scenarios. FAST_LIO [20] and FAST_LIO2 [21] enhance the existing state estimation framework by introducing a novel Kalman gain calculation formula and an Extended Iterative Error State Kalman Filter (ESIKF) based on LINS [22], which simplifies the Jacobian matrix and improves computational efficiency. Additionally, algorithms such as LVI_SAM [23] and FAST-LIVO [24] have been proposed for LiDAR-IMU-Vision tightly coupled odometry. In these approaches, LIO and VIO operate independently; while VIO primarily serves as a backup to replace degraded LIO performance, the actual improvement in positioning accuracy predominantly relies on the LIO performance. Although the aforementioned algorithm can improve robustness in certain well-illuminated testing environments through visual compensation techniques, significant challenges exist when applied to large-scale real-world scenarios: these challenges include relative localization error drift, slam degradation in feature repetition scenarios, and sudden false GNSS absolute localization forced constraints due to multipath effects.

This paper focuses on the mainstream framework of multi-source fusion localization algorithms, with an emphasis on reliability and robustness in real, complex environments. However, none of the above algorithms effectively evaluate the status of GNSS factors, nor do they employ strategies to avoid lidar odometry degradation in scenarios with sparse or repetitive features. On the other hand, existing algorithms cannot provide sensor weight evaluations and lack the ability to dynamically adjust to multi-source sensors. This paper proposes a GNSS/INS/LiDAR multi-source fusion positioning framework that incorporates quality control mechanisms to monitor the front-end and optimizes the back-end fusion methodology. In detail, the ‘quality-controlled mechanism’ can be divided into two logical parts. (1) At the front end, the paper proposes a new feature repeatability detection algorithm to avoid LIO degradation in repetition scenarios based on the standard deviation variability of differences of features and the standard deviation variability of features. Sensor availability is evaluated through GNSS/INS availability status monitoring and LiDAR data feature repeatability detection. (2) In terms of back-end optimization, the inverse weight of the uncertainty model is provided. The prior probability of sensor covariance within the factor graph improves the fusion weight adjustment capability of the algorithm, reducing the risk of large-scale drift and degradation.

2. System Overview

2.1. The Primary Contributions of This Paper

This paper primarily focuses on achieving robust positioning in complex environments by leveraging a framework that integrates satellite navigation, inertial navigation, and LiDAR, and the system framework is shown in Figure 1.

(1)

The uncertainty measurement weights in real time and quality-control fusion positioning framework are proposed, which include GNSS/INS integrated navigation for absolute position measurement and LIO (LiDAR-Inertial Odometry) for relative motion estimation.

(2)

The front-end is augmented with a GNSS availability status monitoring model and a LIO quality control model, enhancing the adaptability of sensors in complex environments.

(3)

A verification platform has been developed for satellite navigation, inertial navigation, and LiDAR, enabling synchronized timing among sensors. Real-time processing is facilitated by software implemented on the platform computer.

(4)

Experiments are conducted in real-world environments, including mountainous areas, tunnels, campus, and urban environments, and the robustness and reliability of the algorithm are verified on the experimental vehicle platform.

2.2. Platform System

As shown in Figure 2, the vehicle platform incorporated a satellite navigation receiver, a 6-axis MEMS inertial navigation system and a 16-thread mechanical LiDAR. The LiDAR scanning frequency was 10 Hz, while the IMU output frequency was 100 Hz. The 1PPS signal from the satellite navigation receiver was synchronized with the UTC output time and its frequency was doubled to 10 Hz and 100 Hz, respectively, providing time synchronization services for the LiDAR and IMU and ensuring temporal alignment among the three sensors. Data processing was carried out using a small computer equipped with a 4-core Intel i7-8gen CPU.

2.3. Unified Coordinate System

First, transfer the IMU coordinate system to the LiDAR coordinate system, and then convert the GNSS/IMU combined absolute positioning results from WGS84 coordinate system (latitude, longitude, elevation) to the UTM (Universal Transverse Mercator) grid coordinate system. Finally, the GNSS results are transferred to the world coordinate system by fusing the original IMU with the extended Kalman filter estimator.

3. Fusion Methods

3.1. GNSS/INS Factor

GNSS raw observations can be utilized to obtain latitude, longitude, and elevation via RTK. GNSS/INS integrated navigation methods exhibit complementary characteristics. Therefore, integrated navigation is essential in multi-source fusion systems in complex environments.

GNSS/INS loose coupling usually uses a Kalman filter to address the system’s nonlinearities. The primary error parameters of the inertial sensors are incorporated into the system state vector of the Kalman filter. The GNSS/INS integrated navigation is based on the INS error equation [25]:

$$\begin{array}{l}\mathit{\delta }\dot{\mathit{r}}=-{\mathit{\omega }}_{\mathit{e}\mathit{n}}\times \mathit{\delta }\mathit{r}+\mathit{\delta }\mathit{v}\\ \mathit{\delta }\dot{\mathit{v}}=-({\mathit{\omega }}_{\mathit{i}\mathit{e}}-{\mathit{\omega }}_{\mathit{i}\mathit{n}})\times \mathit{\delta }\mathit{v}-\mathit{\delta }\mathit{\psi }\times \mathit{f}+\nabla \\ \mathit{\delta }\dot{\mathit{\psi }}=-{\mathit{\omega }}_{\mathit{i}\mathit{n}}\times \mathit{\delta }\mathit{\psi }+\mathit{\epsilon }\\ \nabla ={\mathit{u}}_{\nabla },\mathit{\epsilon }={\mathit{u}}_{\mathit{\epsilon }}\end{array}$$

(1)

$\mathit{\delta }\mathit{r},\mathit{\delta }\mathit{v}$ and $\mathit{\delta }\mathit{\psi }$ denote the position, velocity, and orientation error vectors, respectively. ${\mathit{\omega }}_{\mathit{e}\mathit{n}}$ represents the rotation angular velocity vector of the navigation coordinate system with respect to the earth coordinate system. ${\mathit{\omega }}_{\mathit{i}\mathit{e}}$ is the rotation angular velocity vector of the Earth coordinate system with respect to the inertial coordinate system. ${\mathit{\omega }}_{\mathit{i}\mathit{n}}$ represents the rotation angular velocity vector of the navigation coordinate system with respect to the inertial coordinate system. $\mathit{f}$ denotes the specific force vector measured by the accelerometer. $\nabla$ and $\mathit{\epsilon }$ denote the acceleration random error vector and the gyro random error vector. ${\mathit{u}}_{\nabla },{\mathit{u}}_{\mathit{\epsilon }}$ denotes white Gaussian noise.

For the velocity ${\mathit{v}}_{\mathit{g}}$ and position ${\mathit{r}}_{\mathit{g}}$ of the GNSS motion state at time t, the vector of the combined system is expressed as the difference between the velocity and position of IMU and GNSS. ${\mathit{v}}_{\mathit{i}}$ and ${\mathit{r}}_{\mathit{i}}$ are based on the mechanical arrangement of the IMU acceleration and angular rate:

$$\mathit{z}={\left\{\left({\mathit{v}}_{\mathit{g}}-{\mathit{v}}_{\mathit{i}}\right),\left({\mathit{r}}_{\mathit{g}}-{\mathit{r}}_{\mathit{i}}\right)\right\}}^{\prime }$$

(2)

The covariance matrix is constructed as follows:

$${\mathrm{P}}_k=\left\{\begin{array}{cc}{\sum }_{vv}& {\sum }_{vr}\\ {\sum }_{rv}& {\sum }_{rr}\end{array}\right\}$$

(3)

The extended Kalman filter [26] is used to obtain the positioning results from loosely integrated navigation data fusion.

3.2. LIO Factor

3.2.1. Distortion Correction

The LIO factor is based on the improved LIO_SAM algorithm [19]. The low scanning frequency of LiDAR necessitates consideration of the impact of the vehicle platform’s speed on data acquisition. By leveraging the high-precision and short-term advantages of the IMU, motion compensation for scanning is performed, and a short-time approximate uniform-motion compensation model is established.

$$X_i^t=q{\widehat{X}}_i^t+{\displaystyle \frac{{\tau }_i-{\tau }_t}{\tau -{\tau }_t}}{r}^t$$

(4)

Here, $\tau ,{\tau }_i,{\tau }_t$ represents a complete scanning period, scanning point time, and starting scanning time, respectively, and the point motion state matrix ${\widehat{X}}_i^t$ is the estimated value of $X_i^t$. The initial frame of lidar odometry is initialized using the rotation and translation obtained from IMU pre-integration [27]. The initial reference value for the subsequent frame is determined by adding the pose optimized through system fusion to the incremental rotation and translation obtained from IMU pre-integration at the current time.

The velocity ${\mathit{v}}_{\mathit{t}+\Delta \mathit{t}}$, position ${\mathit{r}}_{\mathit{t}+\Delta \mathit{t}}$, and rotation ${\mathit{q}}_{\mathit{t}+\Delta \mathit{t}}$ state estimation models are calculated based on IMU measurements.

$$\begin{array}{l}{\mathit{v}}_{\mathit{t}+\Delta \mathit{t}}={\mathit{v}}_{\mathit{t}}+\mathit{g}\Delta \mathit{t}+{\mathit{q}}_{\mathit{t}}^{\mathit{W}\mathit{B}}\left({\mathit{a}}_{\mathit{t}}-{\mathit{b}}_{\mathit{t}}^{\mathit{a}}-{\mathit{n}}_{\mathit{t}}^{\mathit{a}}\right)\Delta \mathit{t}\\ {\mathit{r}}_{\mathit{t}+\Delta \mathit{t}}={\mathit{r}}_{\mathit{t}}+{\mathit{v}}_{\mathit{t}}\Delta \mathit{t}+{\displaystyle \frac{1}{2}}\mathit{g}\Delta {\mathit{t}}^2+{\displaystyle \frac{1}{2}}{\mathit{q}}_{\mathit{t}}^{\mathit{W}\mathit{B}}\left({\mathit{a}}_{\mathit{t}}-{\mathit{b}}_{\mathit{t}}^{\mathit{a}}-{\mathit{n}}_{\mathit{t}}^{\mathit{a}}\right)\Delta {\mathit{t}}^2\\ {\mathit{q}}_{\mathit{t}+\Delta \mathit{t}}={\mathit{q}}_{\mathit{t}}\exp \left(\left({\mathit{\omega }}_{\mathit{t}}-{\mathit{b}}_{\mathit{t}}^{\mathit{\omega }}-{\mathit{n}}_{\mathit{t}}^{\mathit{\omega }}\right)\Delta \mathit{t}\right)\end{array}$$

(5)

Here, ${\mathit{a}}_{\mathit{t}},{\mathit{\omega }}_{\mathit{t}}$ is the acceleration and angular velocity measurements at time $\mathit{t}$, respectively. ${\mathit{b}}_{\mathit{t}}^{\mathit{a}},{\mathit{n}}_{\mathit{t}}^{\mathit{a}}$ is the measurement deviation of acceleration measurement and random white noise. ${\mathit{b}}_{\mathit{t}}^{\mathit{\omega }},{\mathit{n}}_{\mathit{t}}^{\mathit{\omega }}$ is the measurement deviation of angular velocity and random white noise. $\mathit{g}$ is the acceleration of gravity in the world coordinate system $\mathbf{W}$. ${\mathit{q}}_{\mathit{t}}^{\mathit{W}\mathit{B}}$ is the rotation of the inertial body coordinate system to the world coordinate system.

3.2.2. Feature Extraction

The different types of features in a 3D point cloud are distinguished by calculating the curvature threshold $c_{th}$, thereby extracting edge points and planar point features [15]. Points with curvature values greater than the threshold are classified as edge features, while those with curvature values less than the threshold are categorized as planar features.

The odometer identifies the corresponding feature of the current feature set $F_e^t, F_p^t$ within the feature set $F_e^{t-1}, F_p^{t-1}$ of the previous scan and estimates the sensor motion by conducting continuous scan matching for point-to-edge and point-to-plane features. Let $d_e,d_p$ represent the distances from the point to the edge feature and from the point to the plane feature, respectively.

3.2.3. LiDAR Odometer

Using the 6-DoF representation, the Euler angles are transformed into a rotation matrix, and the Rodriguez formula is applied to perform the transformation.

$$R=e^{\widehat{\omega }\theta }=I+\widehat{\omega }\sin \theta +{\widehat{\omega }}^2\left(1-\cos \theta \right)$$

(6)

Here, $\theta =\left[roll,pitch,yaw\right]$, $\omega ={\displaystyle \raisebox{1ex}{$T$}\!\left/ \!\raisebox{-1ex}{$\left|T\right|$}\right.}$, $T=\left[x,y,z\right]$.

Formulas for point-to-edge and point-to-plane constraints are derived, and the nonlinear Levenberg-Marquardt algorithm [28] is employed to solve the resulting equations.

$$f_e\left(\theta ,T\right)=d_e,f_p\left(\theta ,T\right)=d_p$$

(7)

3.3. Quality Control

3.3.1. GNSS/INS Quality Control

The latitude, longitude, and elevation of GNSS measurements are obtained via RTK. However, maintaining a fixed solution for these measurements can be challenging due to the influence of sheltered environments such as terrain and trees. It is essential to evaluate the availability of GNSS measurements using a multi-system GPS/BDS signal solution monitoring system. Fortunately, most receivers are equipped with signal solution state monitoring capabilities, which can provide feedback on the GNSS analysis state to the fusion system as a prior quality control strategy. The possible solution states include the following: (1) NONE; (2) SINGLE; (3) NARROW_FLOAT; (4) NARROW_INT. Generally, the ideal solution is NARROW_INT, which can either participate in the fusion solution or be directly utilized as the final localization result. However, even if integer ambiguities remain unresolved and the positioning result corresponds to a floating-point solution (e.g., NARROW_FLOAT), it can still participate in the fusion positioning process. This paper involves utilizing the covariance of GNSS/INS integrated navigation solutions as a prior probability judgment for GNSS/INS factors.

3.3.2. LIO Quality Control

It is assumed that features are repeatable in the scene and show a small change in the number of features between adjacent frames. This paper newly proposes the difference and approximate derivative variability of features and the standard deviation variability of features to characterize the repeatability of features in the scene, which is used as the prior discrimination of the availability of LIO.

Edge points and plane points have been found to perform well in a variety of settings, including schools and parks. However, the resolution of LiDAR is inherently limited. During high-speed motion, the actual sparsity of plane points derived from adjacent feature points can vary significantly. For example, in environments with few features, such as tunnels, the number of effective feature points decreases substantially. This can directly compromise odometry accuracy due to feature mismatching. To address this issue, the current frame is discarded if the number of extracted feature points falls below a predefined threshold, thus preventing degradation. To maintain system stability, the weight assigned to keyframes at a high risk of degradation should be reduced. Furthermore, a measure of uncertainty is introduced for matching feature value errors, in order to assess the associated risk of inaccurate feature matching during the fusion process.

The standard deviation variability of differences of features: ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}$:

$$\begin{array}{l}{\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}\in \mathit{A},\mathit{e}\mathit{p}\mathit{o}\mathit{c}\mathit{h}=\mathit{c}\mathit{a}\mathit{r}\mathit{d}(\mathit{A})\\ \mathit{d}\mathit{i}\mathit{f}{\mathit{f}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}^{\mathit{i}}={\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}^{\mathit{i}}-{\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}^{\mathit{i}-1}\\ {\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}=\mathit{s}\mathit{t}\mathit{d}(\mathit{d}\mathit{i}\mathit{f}\mathit{f})\end{array}$$

(8)

The standard deviation variability of features, ${\mathit{\sigma }}_{\mathit{A}}$, ${\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}^{\mathit{i}}$, represents the number of features at epoch $\mathit{i}$, and ${\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}^{\mathit{i}}$ equals 6 in the experiment of Section 4 below:

$${\mathit{\sigma }}_{\mathit{A}}=\mathit{s}\mathit{t}\mathit{d}(\mathit{A})/{\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}^{\mathit{i}},{\mathit{n}}_{\mathit{f}\mathit{e}\mathit{a}\mathit{t}\mathit{u}\mathit{r}\mathit{e}}\in \mathit{A},\mathit{e}\mathit{p}\mathit{o}\mathit{c}\mathit{h}=\mathit{c}\mathit{a}\mathit{r}\mathit{d}(\mathit{A})$$

(9)

3.4. Fusion

3.4.1. Factor Graph Optimization

Factor graph optimization is well-suited to large-scale problems with sparse structures in real-world complex environments [29,30,31]. The Huber kernel function is used to address potential outliers in the input data of the least squares problem that may be caused by incorrect measurements. Using the loss function effectively mitigates the influence of these outliers on the optimization process. Specifically, the LossFunction minimizes the impact of large residuals on the final optimization result by reducing the weight assigned to them. The Schur complement method is utilized for the resolution of the optimization problem. In this approach, the block diagonal part of the Hessian matrix is computed using the sparse Cholesky decomposition method, while the remaining parts are handled using the dense QR decomposition method.

The residuals of the odometer in LIO are formulated as the errors in the rotation matrix $R_i^j$ and translation vector $t_i^j$ of adjacent frame motions. Specifically, the rotation matrix is represented as a quaternion, while the translation vector corresponds to positional displacements.

$${\Delta }_l=\underset{x_i^j}{\min }{\displaystyle \frac{1}{2}}{\displaystyle \sum _i{\rho }_i\left({‖f_{R_i^j,t_i^j}\left(•\right)‖}^2\right)}$$

(10)

$$f_{R_i^j,t_i^j}\left(•\right)=\left\{q_{i x}^j,q_{i y}^j,q_{i z}^j,q_{i w}^j,t_{i x}^j,t_{i y}^j,t_{i z}^j\right\}$$

(11)

The residuals of GNSS/INS are formulated as the positional errors in the northeast-down direction between adjacent frames within the UTM coordinate system. The integrated navigation result from GNSS/INS is utilized as the absolute position factor.

$${\overrightarrow{\chi }}_g^k=\left\{{\overrightarrow{X}}_g^1,{\overrightarrow{X}}_g^1,\dots ,{\overrightarrow{X}}_g^k\right\}$$

(12)

$${\overrightarrow{X}}_g^k=\left[x_g^k,y_g^k,z_g^k\right]$$

(13)

3.4.2. Inverse Weight of Uncertainty

In response to the divergent performance of satellite navigation and LiDAR sensors across a range of environments, sensor weights are dynamically adjusted by defining the prior factor for the newly proposed uncertainty measurement. For the GNSS/INS factor, the covariance matrix ${\sum }_{rr}$ follows a Gaussian distribution and can therefore serve as the prior factor for uncertainty measurement in Gaussian error modeling. Position ${\mathit{r}}_{\mathit{g}}$ is the GNSS motion state, and position ${\mathit{r}}_{\mathit{I}}$ is calculated based on IMU measurements. The details are in Section 3.1.

$${\xi }_{GNSS/INS}={\sum }_{rr}=diag\left(\epsilon x_g,\epsilon y_g,\epsilon z_g\right)$$

(14)

$${\sum }_{rr}=\mathit{C}\mathit{o}\mathit{v}({\mathit{r}}_{\mathit{g}},{\mathit{r}}_{\mathit{I}})$$

(15)

In order to prevent degradation and collapse resulting from feature repetition and sparsity, forced constraints on the inverse weight of the uncertainty metric are applied for LIO. When the LIO (LiDAR-Inertial Odometry) is operational, the uncertainty measurement corresponds to the standard deviation of the current static environment. If this value exceeds the predefined threshold ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}}$ and ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{A}}}$, the uncertainty measure approaches infinity. The value of ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}}$ is 30 and ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{A}}}$ is 1.0%, respectively, in the experiment of Section 4 below. Let $\infty$ equaling 10,000 represent infinity.

$${\mathit{\xi }}_{\mathit{L}\mathit{I}\mathit{O}}=\left\{\begin{array}{c}\mathit{s}\mathit{t}\mathit{d}(\mathit{T}),{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}\ge {\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}}\& {\mathit{\sigma }}_{\mathit{A}}\ge {\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{A}}}\\ \approx \infty ,{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}<{\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}}\parallel {\mathit{\sigma }}_{\mathit{A}}<{\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{A}}}\end{array}\right.$$

(16)

$T=\left[x,y,z\right]$ is the transition matrix of LIO in the current static environment. ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}}$ denotes the threshold of the standard deviation variability of differences of features ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}$. ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{A}}}$ denotes the threshold of the standard deviation variability of differences of features ${\mathit{\sigma }}_{\mathit{A}}$. The reference threshold values were obtained by analyzing the results of feature repeatability in different environments, the details are in Section 5.

4. Experiments and Analysis

In order to verify the adaptability of the algorithm in complex environments, this study conducted positioning experiments in four representative scenarios: namely, tunnel, urban, campus, and mountainous environments. In the tunnel environment, the reliability and stability of the algorithm were assessed to evaluate the effectiveness of the quality control mechanism in scenarios characterized by sparse and repetitive features. The positioning accuracy of the algorithm was validated by comparison with existing multi-source fusion algorithms, such as FAST-LIO and LIO-SAM, between urban and campus scenes. In the mountainous environment, the continuity and robustness of the algorithm under complex conditions were examined. Concurrently, an analysis was conducted of the sparsity and repeatability of features extracted from lidar data across diverse scenarios. Furthermore, an assessment was undertaken of the impact of the quality control algorithm on sensor performance in varying environmental conditions.

4.1. Tunnel Environment Experiment

Initially, an experiment was conducted to ascertain the positioning of the tunnel environment. The vehicle successfully traversed the Tongjimen Tunnel (see Figure 3a(A,B), 1.0 km) and the Xi’anmen Tunnel (see Figure 3a(C,D), 1.84 km) at an average effective speed in excess of 40 km/h. The entire test section extended for approximately 8 km, with a total testing duration of 10 min. The positioning trajectory is illustrated in the figure. The findings demonstrate that the algorithm maintains continuous and smooth positioning in real time as the vehicle navigates through complex urban road conditions, including when entering and exiting tunnels.

As demonstrated in Figure 4, within the primary section of the inner ring road, the number of LiDAR edge and planar feature points is comparatively elevated, and the distribution of the variation in the number of feature points between successive scanning frames is discernible. The standard deviation variability of differences of planar feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{p}}$ is more than 90, and the standard deviation variability of planar feature’s ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{p}}$ average feature fluctuation rate is higher than 3.06%. The edge feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{e}}$ is more than 90, and the ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{e}}$ average feature fluctuation rate is 5.11%. The average characteristic variability ${\mathit{\sigma }}_{\mathit{A}}$ is about 2.86%; this indicates that the feature repeatability is relatively low. Conversely, in the tunnel scenario, the number of LiDAR edge and planar feature points decreases, while the distribution of the standard deviation variability of differences of feature points remains relatively stable, suggesting that the scanning frame features exhibit a degree of repetitiveness. The standard deviation variability of differences of global tunnel feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}$ is 23.73, the standard deviation variability of global tunnel feature ${\mathit{\sigma }}_{\mathit{A}}$ is about 0.79%, the planar feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{p}}$ is 21.73, and the planar feature variability ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{p}}$ is 0.78%.

The value is significantly lower than that of the urban road segment, with higher feature repeatability and lower availability of LIO. Consequently, the corresponding inverse weight is increased or even becomes unavailable. When the vehicle traverses complex urban road conditions and tunnels, the weights of LIO and GNSS/INS positioning are specifically adjusted. The inverse weight serves as the prior Gaussian noise input value. As illustrated in Figure 5, under urban road conditions visible to navigation satellites, the inverse weight of GNSS/INS integrated navigation remains at a relatively low level, with an average value not exceeding 0.1. In the tunnel, the LIO maintains a relatively high inverse weight to avoid degradation and negative positioning performance.

4.2. Urban Environment Experiment

The vehicle traversed urban roads, traffic intersections, billboards, and viaducts at an average effective speed exceeding 40 km/h in Nanjing City, covering a total distance of approximately 15 km over a test duration of 30 min. The positioning trajectory is illustrated in Figure 6. The results indicate that the algorithm maintains continuous and smooth real-time positioning even after traversing complex urban road conditions, including viaducts, with a positioning accuracy (RMSE) better than 0.18 m.

As illustrated in Figure 7 and Figure 8, the number of LiDAR edge and planar feature points is comparatively high in an urban environment, and the distribution of the difference in the number of feature points between scanning frames exhibits significant variation. The planar feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{p}}$ is close to 80, the average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{p}}$ is higher than 2.23%, the edge feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{e}}$ is more than 20, and the average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{e}}$ is 3.92%. The average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}$ is about 2.10%. The findings indicate that the reproducibility of features is diminished, the accessibility of LIO is augmented, and the associated inverse weight is diminished.

4.3. Campus Environment Experiment

The vehicle traversed the campus road, which was closed to traffic, at an average effective speed of approximately 17 km/h, covering a total distance of about 0.85 km in 3 min. The positioning trajectory is illustrated in the Figure 9. The results indicate that the algorithm maintains continuous and smooth real-time positioning even after the vehicle passes through campus road conditions, with a positioning accuracy (RMSE) better than 0.23 m.

As illustrated in Figure 10 and Figure 11, the campus road section features a rigid and regular building structure. The number of LiDAR edge and planar feature points is relatively high, and the distribution of the difference in the number of scanning frame feature points shows significant variation. Specifically, the planar feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{p}}$ exceeds 90, with an average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{p}}$ higher than 2.28%. The edge feature ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{e}}$ exceeds 25, with an average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}^{\mathit{e}}$ at 4.16%. The overall average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}$ is approximately 2.14%. These findings indicate a reduction in feature repeatability, increased availability of LIO, and a corresponding decrease in inverse weighting. Furthermore, the positioning accuracy of GNSS/INS integrated navigation diverges in road sections covered by buildings, accompanied by a synchronous increase in inverse weighting.

4.4. Mountain Environment Experiment

The vehicle finally proceeded along a winding mountain road at an average effective speed of approximately 40 km/h. The total distance covered was about 8.5 km, with the journey taking 12 min. The positioning trajectory is illustrated in Figure 12. The results indicate that the algorithm can maintain continuous and smooth positioning in real time even when the vehicle passes through complex mountainous terrain and is impeded by mountains. The positioning accuracy (RMSE) is better than 0.21 m.

As illustrated in Figure 13 and Figure 14, within a mountainous environment, the environmental structure exhibits rigidity. The number of LiDAR edge and planar feature points is relatively high, and the distribution of differences in the number of scanning frame feature points is notably distinct. Specifically, the planar feature count ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{p}}$ exceeds 110, with an average standard deviation fluctuation rate exceeding 4.20%. The edge feature count ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}^{\mathit{e}}$ is approximately 30, accompanied by an average standard deviation variability of 6.43%. The overall average characteristic variability of ${\mathit{\sigma }}_{\mathit{A}}$ is approximately 3.95%. These observations indicate a reduction in feature repeatability. However, the availability of LIO remains high, with the corresponding inverse weight being consistently low. In contrast, for some mountainous roads sheltered from GNSS signals, the positioning accuracy of the GNSS/INS integrated navigation system diverges, while the inverse weight increases synchronously.

4.5. Accuracy of Positioning and Mapping

As demonstrated in

Table 1. APE w.r.t Transition (m) [32] in different environments.

Dataset	OURS (With QC Module)			LIO_SAM (Without QC Module)			LIO_SAM (gps) (Without QC Module)				FAST-LIO			LEGO_LOAM
	RMSE	Max	Min	RMSE	Max	Min	RMSE	Max		Min	RMSE	Max	Min	RMSE	Max	Min
Tunnel (628s)	Continuous and smooth			False (16s)			False (7s)				False			False
Urban (2014s)	0.18	0.48	0.01	False (1133s)			False (165s)				False			>1000
Urban (180s)	0.14	0.24	0.11	19.73	28.98	10.03	False (74s)				27.22	63.86	3.04	143.96	196.92	35.99
Campus (180s)	0.23	0.29	0.15	1.82	2.58	1.22	0.49		0.77	0.24	4.17	8.13	1.70	1.31	1.86	0.92
Mountain (760s)	0.21	1.76	0.01	>1000			False				>1000			>1000

False (ts) denotes the system degradation at time t. QC is the abbreviation of quality control.

, the present article undertakes a comparative analysis of the algorithm in question with regard to its positioning accuracy, reliability, and robustness. This is undertaken against the backdrop of state-of-the-art algorithms, including LIO_SAM, FAST_LIO, and LEGO_LOAM. Comparative studies have been designed between scenarios with the quality-control module (OURS) and without the quality-control module (LIO_SAM or LIO_SAM (gps)), because the framework of this paper is based on the improved LIO_SAM algorithm. Firstly, with regard to reliability and robustness, the algorithm in question was demonstrated to display versatility in four distinct real-world environments, namely tunnel, urban, campus, and mountainous settings. It has been observed to exhibit robust performance, characterised by integrity and smoothness, in the aforementioned environments. In contrast, other algorithms have proven to be incapable of completing large-scale scene positioning tests, due to the presence of large-scale drifting and system degradation, with the exception of the campus environment. Compared with the LIO_SAM algorithm, our algorithm improves the robust running time by 44% in terms of availability in large-scale urban (2014s) tests. In addition, the algorithm exhibits superior positioning accuracy compared to other methods, achieving a positioning accuracy (RMSE) of 0.18 and 0.21 m in large-scale, long-duration urban and mountainous settings, respectively. Although adding GPS factors to the LIO_SAM algorithm can improve positioning accuracy in some small-scale scenarios, it can also increase the risk of system degradation caused by forced constraints.

As illustrated in Figure 15, with regard to the accuracy of mapping in a closed-loop campus environment, while all the comparison algorithms attain horizontal closure of real-time mapping upon returning to the initial point, there are evident mapping errors in the vertical direction. The algorithm presented in this article demonstrates a superior capacity for convergence in the elevation direction when compared to alternative algorithms.

5. Discussion

The repeatability of point cloud features is an important cause of LIO system degradation due to the inability to correctly estimate the position and orientation in the environment. Should repetitive features be correctly extracted, the methods will demonstrate their application potential in real, complex environments. The quality control strategy can be divided into two aspects: front-end and back-end. The front end of the system is primarily responsible for feature extraction or sensor status monitoring. The primary focus of back-end optimization is the fusion method and weight distribution strategies employed. In this paper, we calculate the difference and approximate derivative variability and the standard deviation variability derived from LiDAR point clouds across four real environments: tunnel, urban, campus, and mountainous environments. These calculations aim to monitor the repeatability of features within every scene and serve as a prior indicator for assessing the availability of LIO.

As shown in

Table 2. Repeatability statistics of LiDAR features for different scenes.

Dataset	Urban			Tunnel			Campus			Mountain
	Global	Planar	Edge	Global	Planar	Edge	Global	Planar	Edge	Global	Planar	Edge
${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}$	80.66	79.38	20.51	23.73	21.73	9.649	99.1	94.62	25	119.3	114	30
${\mathit{\sigma }}_{\mathit{A}}$	2.10%	2.23%	3.92%	0.79%	0.78%	4.68%	2.14%	2.28%	4.16%	3.95%	4.20%	6.43%

, the mechanical LiDAR data exhibit high repeatability in tunnel environments, with significantly lower feature variability compared to other environments such as urban and campus ones, so there are differences in the availability of LiDAR sensors in different environments. In tunnel environments, the planar feature variability is less than 22, the edge feature variability is below 10, and the average standard deviation percentage of the planar feature is less than 0.8%. Conversely, in mountainous environments, the repeatability of lidar features is relatively low and the planar feature count exceeds 110, with an average standard deviation variability exceeding 4.20%. The edge feature count is approximately 30, accompanied by an average standard deviation variability of 6.43%.

A comprehensive analysis of positioning accuracy and availability is presented in Table 1, while feature repeatability statistics are presented in Table 2. The weight of the LIO must be carefully adjusted if the reference threshold values ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}}$ and ${\mathit{Q}}_{{\mathit{\sigma }}_{\mathit{A}}}$ are used as constraints to reduce the risk of degradation by less than 30 and 1.0%, respectively. The purpose of weight adjustment is to prevent the multi-source fusion system from failing due to large-scale offsets and LIO degradation. Furthermore, the absence of weight adjustment across varying environments renders the tunnel environment susceptible to system degradation, attributable to the reproducibility of lidar features and the paucity of point clouds. In order to ensure effective quality control of the system, it is necessary to adjust the weights with stringent constraints. In contrast, in large-scale, long-duration environments such as urban and mountain canyons, lidar point cloud features exhibit significant fluctuations. This enables the utilization and complementary capabilities of LIO. Although GNSS factors can improve positioning accuracy in certain small-scale scenarios, they also increase the risk of system degradation resulting from forced constraints, such as LIO_SAM (gps). Satellite navigation systems are prone to divergence due to multipath effects. In such scenarios, real-time weight adjustment is essential to ensure reliability and robustness.

6. Conclusions

Despite the advancement of multi-source fusion positioning frameworks in enhancing positioning accuracy, the reliability and robustness of multi-source fusion positioning algorithms remain substantial challenges in real, complex environments. These include system degradation in feature repetition scenarios and sudden forcing false absolute localization constraints. To address these challenges, this paper proposes the standard deviation variability of differences of features ${\mathit{\sigma }}_{\mathit{d}\mathit{i}\mathit{f}\mathit{f}}$ and the standard deviation variability of features ${\mathit{\sigma }}_{\mathit{A}}$ to evaluate the repeatability of LiDAR features in environments. In the back end, the Gaussian uncertainty measurement weights within the factor graph are dynamically updated in real time, allowing for adaptive adjustment of sensor fusion weights in different environments. In conclusion, sensor quality control plays a crucial role in ensuring reliability and robustness of multi-source fusion localization. It is evident that, despite the capacity of GNSS factors to enhance the precision of positioning in certain limited contexts, they concomitantly engender an escalation in the vulnerability of the system to degradation, a consequence of the imposition of constraints. The quality control of GNSS positioning is also of crucial importance in the context of multi-source fusion. The development of research methods for GNSS quality control is therefore paramount, with a view to enhancing the robustness of multi-source fusion localization. Potential approaches in this regard include GNSS state assessment and error suppression based on other sensors.