A GNSS/LiDAR/IMU Pose Estimation System Based on Collaborative Fusion of Factor Map and Filtering

Abstract

One of the core issues of mobile measurement is the pose estimation of the carrier. The classic Global Navigation Satellite System/Inertial Measurement Unit (GNSS/IMU) integrated navigation scheme has high positioning accuracy but is vulnerable to Global Navigation Satellite System (GNSS) signal occlusion and multipath effect. Simultaneous Localization and Mapping (SLAM) is not affected by signal occlusion, but there are problems such as error accumulation and scene degradation. The multi-sensor fusion scheme combining the two technologies can effectively expand the scene coverage and improve the positioning accuracy and system robustness. However, the current scheme has some limitations. On the one hand, GNSS plays a less important role in most SLAM systems, only for initialization or as a closed-loop factor and other auxiliary work. On the other hand, in the fusion method, most of the current systems only use filtering or graph optimization, without taking into account the advantages of both. Aiming at pose estimation for mobile carriers, this paper combines the advantages of the global optimization of the factor graph and the local optimization of filtering and proposes a GNSS-IMU-LiDAR Constraint Kalman Filter (abbreviated as GIL-CKF), which has the characteristics of full coverage and effectively improving absolute accuracy and high output frequency. The scheme proposed in this paper consists of three parts. Firstly, an extensible factor map is used to fuse the positioning information from different sources, including GNSS, IMU, LiDAR, and a closed-loop map, to maintain a high-precision SLAM system, and the output is used as Multi-Sensor-Fusion-Odometry (MSFO). Then, the scene is divided according to the GNSS quality factor, and a Scene Optimizer (SO) is designed to filter GNSS pose and MSFO. Finally, the results are input into the Extended Kalman Filter (EKF) together with the original IMU data for further optimization and smoothing. The experimental results show that the integration of high-precision GNSS positioning information with IMU, LiDAR, a closed-loop map, and other information through the factor map can effectively suppress error accumulation and improve the overall accuracy of the SLAM system. The SO based on GNSS indicators can fully retain high-precision GNSS positioning information, effectively play their respective advantages of filtering and graph optimization, and alleviate the conflict between global and local optimization. SO with EKF filtering furthers optimization, can improve the output frequency, and smooth the trajectory. GIL-CKF can effectively improve the accuracy and robustness of pose estimation and has obvious advantages in multi-sensor scene complementarity, partial road section accuracy improvement, and high input frequency.

1. Introduction

Compared with traditional station measurement, the Mobile Mapping System (MMS) realizes dynamic measurement, avoids manual handling and installation between stations, and greatly improves production efficiency and operation freedom [1]. The realization of Mobile Mapping measurement depends on the positioning navigation unit. Therefore, ensuring the accurate positioning of sensors in motion is the premise of the effective implementation of mobile measurement. GNSS/IMU integrated navigation technology is a classic positioning navigation scheme for mobile measurement that has been widely used in the surveying and mapping industries and achieved good results. In high-precision measurement, centimeter-level dynamic positioning can be achieved by using DGNSS (Differential GNSS)/IMU integrated navigation based on differential carrier phase observations [2,3,4]. However, GNSS is difficult to obtain as a fixed solution in weak signal areas, and positioning accuracy degrades to decimeter or even meter levels [5]. Simultaneous Localization and Mapping (SLAM) is a popular solution to the positioning problem in weak GNSS signal environments. The principle of SLAM is to establish the surrounding environment model by using lasers, cameras, and other sensors and to estimate its motion state by calculating the relative relationship between the current observation point and the environment [1,6,7,8,9,10].

SLAM technology and GNSS/IMU integrated navigation technology are effectively complementary in the scene, which provides the possibility for accurate pose estimation in a wide range of scenarios. A large number of researchers have conducted a lot of work on how to combine the two. Based on GNSS/IMU integrated navigation technology, some scholars try to introduce the relative positioning information of LiDAR to make up for the deficiency of weak GNSS signal area positioning. A direct method is using LiDAR/IMU as a supplement to GNSS in weak GNSS signal environments, such as indoors and within cities. LiDAR and GNSS alternated to assist the IMU in providing integrated navigation results, which constituted a GNSS/IMU/LiDAR integrated navigation system [11]. When the GNSS signal is good, SLAM is used to synchronously observe, locate, and update the three-dimensional position of the environmental feature mark. When the GNSS signal is blocked, the position estimation of the carrier is obtained by the relative relationship between the carrier and the environmental feature mark, and the IMU parameters are corrected to suppress the IMU cumulative error [12]. Essentially, this type of work focuses on unifying the GNSS and SLAM coordinate systems. GNSS and SLAM are alternately used as the main odometers to expand the scene, but they are independent of each other. There is no information fusion, which is actually loosely coupled. In terms of a deeper information fusion, J. G. et al., use LiDAR/IMU to achieve relative positioning and mapping. When high-precision GNSS is available, GNSS information is used as the “loop” in SLAM to optimize the trajectory by global optimization [13]. This, to a certain extent, has improved the positioning accuracy of the SLAM system. More groundbreaking work is the tight coupling factor graph model [14,15], which incorporates GNSS and other multi-type positioning technologies as different probability factors into a probabilistic factor graph framework to obtain the global optimal solution by calculating the maximum posterior probability of the joint probability of all factors. Based on the factor graph model, an open-source optimization library called GTSAM was developed. Based on GTSAM, T. et al. further constructed a complete set of a multi-sensor factor graph fusion SLAM system called LIO-SAM [16]. Compared with those that are loosely coupled, this method combines GNSS high-precision global positioning information and SLAM relative positioning information closely through graph optimization, which inhibits the error accumulation of the SLAM system and achieves greater results. At the same time, Baidu proposes the MSF (Multi-Sensor Fusion) scheme, which takes GNSS, IMU, LiDAR, and visual cameras as sub-positioning information and fuses all of this information through an error-state Kalman filter [17]. The overall fusion realizes the error control of the pose estimation system and improves its robustness. This is a filtering fusion method that realizes Multi-Sensor Fusion at the information level and achieves better results.

Different sensors impose different constraints on the pose estimation of mobile platforms [18]. The Multi-Sensor Fusion scheme can integrate the advantages of various sensors and effectively improve the coverage of the scene and the robustness of the system, which has gradually become a trend [19,20,21,22]. The development process of Multi-Sensor Fusion technology shows the characteristics from loose to close, which is mainly divided into two different fusion architectures: filtering fusion and graph optimization fusion [23,24]. The former corresponds to the iterative process, which marginalizes the past pose and summarizes the information obtained over time with the probability distribution [18]. The calculation speed is fast, and the output frequency is high [23,24,25]. The latter recombines historical trajectory points and observation data, retains global constraints, and performs batch processing with a higher accuracy but a lower output frequency [20,23,24,26,27]. On the other hand, due to the Gaussian distribution assumption of sensor error and the inaccurate estimation of error covariance, the fusion results of the two sensors show a decrease in accuracy compared with the single sensor (such as GNSS). Therefore, both of them face the same challenge, namely, the conflict problem of local and global optimization [19].

Aiming at the goal of Multi-Sensor Fusion pose estimation on the basis of previous studies combined with scene complementary and information fusion methods, this paper proposes an extensible and robust fusion pose estimation scheme with high precision. The scheme includes a variety of positioning sensors and a set of fusion algorithms. The main idea is to prioritize the use of GNSS positioning information, and SLAM supplements the location of the unlocked region. Through the fusion algorithm, GNSS positioning information is introduced into the SLAM system to improve the positioning accuracy and stability of the SLAM system. In the algorithm structure, this paper uses an extensible factor graph to fuse the positioning information from different sources, such as GNSS, LiDAR, and IMU, to form a Multi-Sensor Fusion Odometry (MSFO) and uses a Scene Optimizer (SO) to select the GNSS pose and MSFO results. The selected results and IMU data are input into EKF filtering to further optimize and smooth them.

2. Materials and Methods

2.1. Factor Graph Optimization

The system receives input from a LiDAR, an IMU, and a GNSS. Four types of factors are introduced to construct the factor graph: (a) an IMU preintegration factor, (b) a lidar odometry factor, (c) a GPS factor, and (d) a loop closure factor. The application of these factors is discussed in Section 3.2.1. The factor graph is a Probabilistic Graphical Model (PGM). Formally, a factor graph is a bipartite graph with two types of nodes: $\mathrm{F}=\left(\mathrm{U},\mathcal{V},ℰ\right)$, where $\mathrm{U}$ means factor nodes, $\mathcal{V}$ means variable nodes, and edge $ℰ$ always exists between factor nodes and variable nodes [14,15,28]. The variables refer to the random variables in the optimization problem, namely pose, road sign, etc. There is a certain conditional probability relationship between these variables, which provides the basic probability information in the SLAM optimization problem. This probability information can be derived from observation or prior knowledge. This relationship is represented by edges, and the probability information of these variables is aggregated by a factor, which constitutes the factor graph representation of the optimization problem. The factor is essentially a likelihood factor, representing the likelihood relationship between two variables. Specifically, there is a Markov property between the adjacent poses of the LiDAR odometer, which can be expressed by a factor [15,28]. The observational relationship between any pose point and the observed landmark can be expressed by a factor. The synchronous positioning relationship between any pose point and GNSS observations can also be expressed by a factor, as shown in Figure 1. As a probabilistic model, the factor graph has been widely used in the field of SLAM. The strict mathematical definition of the factor graph can be seen in Professor Frank’s article [28].

The factor graph can effectively fuse IMU pre-integration, a LiDAR odometer, GNSS high-precision positioning information, and SLAM closed-loop information. This scheme mainly uses the factor graph in MSFO.

2.2. EKF Filter

EKF is an extended form of the standard Kalman filter in nonlinear problems. EKF expands the nonlinear function through the Taylor expansion to linearize the nonlinear function. It then approximates the state estimation and covariance estimation of the system by the Kalman filter algorithm to filter the signal [29,30].

EKF plays two important roles in our scheme. Firstly, it can improve the output frequency of the pose. Secondly, the update amount selected by the optimizer is switched back and forth between the GNSS pose and the LiDAR pose. Although the switching position is carefully selected, there will inevitably be slight differences. EKF can further optimize and smooth these parts.

3. Methodology

3.1. Technical Framework

In this paper, the device is built on the machine car. It consists of three hardware modules: IMU inertial measurement unit, GNSS, and LiDAR. The hardware is shown in Figure 2 and the detailed parameters are shown in

Table 1. Parameters of each hardware module.

Hardware Module	Parameters()
NovAtel GNSS	Dual antenna; measurements (raw) data rate: 20 Hz; nominal accuracy: 1 cm (RTK);
IMU	Tactical-grade; gyroscopes angular random walk < 0.2 deg/√hr; and accelerometers velocity random walk < 0.035 m/s/√hr.
LiDAR	Number of channels: 16; accuracy of ranging: ±3 cm; scanning speed: 5 Hz,10 Hz,20 Hz; horizontal field of view: 360°; and vertical field of view: −15~+15°.

.

The coordinate systems used in this paper are: the World Coordinate System (W), the IMU Coordinate System (B), and the LiDAR Coordinate System (L). This paper assumes that the IMU Coordinate System coincides with the Robot Coordinate System, and the carrier state ${\mathrm{x}}_{\mathrm{k}}$ is expressed as: ${\mathrm{x}}_{\mathrm{k}}=\left[\mathrm{R},\mathrm{p},\mathrm{v}\right]$, in which $\mathrm{R}\in \mathrm{SO}\left(3\right)$ is the rotation matrix, $\mathrm{p} \in { \mathrm{R}}^3$ is position vector, and $\mathrm{v}$ represents the velocity.

The essence of Multi-Sensor Fusion optimization is to solve the probability of optimal distribution of pose information from multiple sensors and multiple moments; the key is the fusion model. In order to make full use of the high-precision positioning information of GNSS, the positioning ability of SLAM in weak GNSS signal areas, the error control ability of graph optimization fusion, and the high-frequency positioning ability of filtering fusion, this paper adopts the design idea of combining filtering and graph optimization to propose a GNSS/IMU/LiDAR multi-constrained filtering fusion system, referred to as GIL-CKF. This scheme adopts a two-step fusion strategy. First, a scalable factor graph is used to fuse positioning information from different sources, such as GNSS, LiDAR, IMU, etc., to form a Multi-Sensor Fusion Odometer (MSFO) to achieve overall accuracy control of the SLAM system. Second, in conjunction with scene filtering, MSFO and GNSS alternately update the IMU state in EKF filtering to further smooth and optimize the local trajectory. The technical framework is shown in Figure 3.

This scheme adopts the “prediction + update” filter structure as a whole, and the update amount is specially designed. The difference is that the update amount of MSCKF is based on the re-projection error of the same visual feature point on different graphs, and the graph optimization problem is constructed. The update amount of this scheme is calculated between multi-source sensors and closed-loop information. What is more, GNSS is directly selected as the update quantity in the open area, which guarantees the accuracy of the open area simply and effectively. In the open, unblocked area, differential GNSS can provide centimeter-level, high-precision positioning information, which is fully preserved and utilized in this scheme. In the urban environment, laser SLAM has a wider range of positioning capabilities and can work effectively in open areas, in the shade, indoors, and in other environments. After fusing high-precision GNSS positioning information as an error constraint, the positioning accuracy of the SLAM system is further improved, which provides a guarantee for the quality of the update of the EKF filter in the weak GNSS environment.

3.2. Key Technology

3.2.1. Multi-Sensor Fusion Odometer (MSFO)

IMU pre-integration and optimization: the IMU pre-integral can provide a good initial value for LiDAR matching. The measured values of angular velocity ${\hat{\omega }}_{\mathrm{t}}$ and acceleration ${\hat{\mathrm{a}}}_t$ of IMU in $B$ system obey Equation (1).

$$\{\begin{array}{l}{\hat{\omega }}_t={\omega }_t+{\mathrm{b}}_t^{\omega }+{\mathrm{n}}_t^{\omega }\hfill \\ {\hat{\mathrm{a}}}_t={\mathrm{R}}_t^{\mathrm{BW}}\left({\mathrm{a}}_t-\mathrm{g}\right)+{\mathrm{b}}_t^{\mathrm{a}}+{\mathrm{n}}_t^{\mathrm{a}}\hfill \end{array}$$

(1)

where ${\omega }_t$ and ${\mathrm{a}}_t$ are the theoretical values of angular velocity and acceleration, ${\hat{\omega }}_t$ and ${\hat{\mathrm{a}}}_t,$ are actual measured value of the sensors; ${\mathrm{R}}_t^{\mathrm{BW}}$ is the transfer matrix from $\mathrm{W}$ to $\mathrm{B}$; $\mathrm{g}$ is the constant gravity vector of $\mathrm{W}$;${ \mathrm{b}}_t$ is a slowly changing bias; and ${\mathrm{n}}_t$ is white noise. Applying the IMU pre-integral method proposed by C. et al. [31,32] to obtain the relative motion ${\Delta \mathrm{v}}_{\mathrm{ij}},{ \Delta \mathrm{p}}_{\mathrm{ij}}, {\Delta \mathrm{R}}_{\mathrm{ij}}$ between two timestamps $\mathrm{i}$ and $\mathrm{j}$, as shown in Equation (2).

$$\{\begin{array}{l}{\Delta \mathrm{v}}_{\mathrm{ij}}={\mathrm{R}}_{\mathrm{i}}^{\mathrm{T}}\left({\mathrm{v}}_{\mathrm{j}}-{\mathrm{v}}_{\mathrm{i}}-{\mathrm{g}\Delta \mathrm{t}}_{\mathrm{ij}}\right)\hfill \\ {\Delta \mathrm{p}}_{\mathrm{ij}}={\mathrm{R}}_{\mathrm{i}}^{\mathrm{T}}\left({\mathrm{p}}_{\mathrm{j}}-{\mathrm{p}}_{\mathrm{i}}-{\mathrm{v}}_{\mathrm{i}}{\Delta \mathrm{t}}_{\mathrm{ij}}-\frac{1}{2}{\mathrm{g}\Delta \mathrm{t}}_{\mathrm{ij}}^2\right)\hfill \\ {\Delta \mathrm{R}}_{\mathrm{ij}}={\mathrm{R}}_{\mathrm{i}}^{\mathrm{T}}{\mathrm{R}}_{\mathrm{j}}\hfill \end{array}$$

(2)

IMU pre-integration provides a good initial value ${\mathrm{h}}_{\mathrm{ij}}^{\mathrm{b}}\left({\mathrm{x}}_{\mathrm{i}},{\mathrm{x}}_{\mathrm{j}}\right)$ for the matching operation of LiDAR. In turn, the observation value ${\mathrm{z}}_{\mathrm{ij}}^{\mathrm{b}}$ provided by LiDAR matching can also be used to construct the error term ${\mathrm{e}}_{\mathrm{ij}}^{\mathrm{LIO}}$ to restrict the error accumulation of accelerometer bias ${\mathrm{b}}_{\mathrm{t}}^{\mathrm{a}}$ and gyroscope bias ${\mathrm{b}}_{\mathrm{t}}^{\mathsf{\omega }}$ on the integration, and improve the accuracy of IMU prediction, as shown in Equation (3). For each iteration, IMU pre-integral and laser odometer (LO) can form an optimization problem, as shown in Equation (4) where $W_{ij}^{-1}$ is the covariance matrix and $E_{\mathrm{m}}$ is the prior information of historical data marginalization. The mutual optimization relationship between IMU pre-integration and LiDAR together constitutes a more accurate and robust Laser Inertial Odometer (LIO).

$$e_{ij}^{LIO}=z_{ij}^b-h_{ij}^b\left(x_i,x_j\right)$$

(3)

$$\begin{array}{c}{E}^{*}=\underset{\Xi }{\arg \min }{\displaystyle {\displaystyle \sum }_{\left(i,j\right)\in ℬ}}{e}_{ij}^{LIO}{}^{\mathrm{T}}{W}_{ij}^{-1}{e}_{ij}^{LIO}+E_{\mathrm{m}}\end{array}$$

(4)

LiDAR odometer and optimization: the matching positioning of LiDAR odometer can obtain relative positioning $z$. At the same time, GNSS positioning information $h\left(x_i\right)$ and Loop closed-loop positioning information $g\left(x_i,x_j\right)$ can construct error terms ${\mathrm{e}}_{ij}^{gnss }$ and ${\mathrm{e}}_{ij}^{loop}$, as shown in Equation (5). Through these two kinds of error terms to suppress the cumulative error of LIO historical trajectory, it constitutes a complete optimization problem, as shown in Equation (6).

$$\{\begin{array}{l}{e}_{ij}^{gnss }=h\left(x_i\right)-z_{ij}\hfill \\ e_{ij}^{loop}=g\left(x_i,x_j\right)-z_{ij}\hfill \end{array}$$

(5)

$${\Xi }^{*}=\underset{\Xi }{\arg \min } \left({\displaystyle \sum }_{\left(i,j\right)\in \Xi }{e}_{ij}^{gnss}{}^T{W}_{ij}^{-1}{e}_{ij}^{gnss}+{\displaystyle \sum }_{\left(i,j\right)\in \Xi }{e}_{ij}^{Loop}{}^T{\mathsf{\Sigma }}_{ij}^{-1}{e}_{ij}^{Loop}\right)$$

(6)

among them, $\mathsf{\Xi }\stackrel{ \mathrm{def} }{=}\left\{{\xi }_i\right\}$ is the set of all pose coordinates; $h$ is the GNSS observation equation; $g$ is the closed-loop observation equation; and $W_{ij}$ and ${\mathsf{\Sigma }}_{ij}$ are the corresponding covariance matrix.

3.2.2. Scene Optimizer (SO)

In most urban scenarios, GNSS and LIO are highly complementary in terms of a working environment. The areas with strong GNSS signals and good satellite geometric distribution usually have relatively few environmental characteristics. The areas with rich environmental characteristics often have high spatial complexity, high buildings, and more trees. The GNSS status in such areas is not good, but the LIO work is stable and the error is small. Therefore, one of the key issues is to strictly monitor and determine the current working state of GNSS. Based on the current main GNSS quality indicators, the Position Dilution of Precision (PDOP), Horizontal Dilution of Precision (HDOP), Vertical Dilution of Precision (VDOP), satellite number SATs, and positioning state quality are selected as the evaluation indicators of GNSS quality. This article obtains GNSS quality indicators from the NMEA (communication protocol standards developed by the National Marine Electronics Association) message of GNSS. Based on the quality evaluation index, this paper constructs the GNSS filter, as shown in Figure 4.

IMU bias drift is usually very accurate under the good constraints of other sensors. The classic GNSS/IMU filtering algorithm establishes a motion model based on IMU, which is used as a predictor. GNSS is regarded as the high-precision observation data and used as an update amount. Centimeter-level positioning can be achieved in a good signal environment. In weak GNSS environments, referring to this structure, the update amount is replaced by the output result of graph optimization so as to achieve higher accuracy than using filtering alone and a higher output frequency than using graph optimization alone. However, the factor map fusion optimization has the limitation of local optimal and global optimal conflicts, especially in the open unshielded zone, and the fusion optimization result MSFO is not necessarily superior to GNSS positioning. Therefore, it is not necessary to use fusion optimization results when GNSS accuracy is reliable. In this paper, the scene is subdivided based on GNSS indicators, and the scene optimizer SO is established. GNSS positioning and MSFO optimized by the factor graph are screened. When GNSS quality is stable, GNSS positioning results are preferred, as shown in Figure 5. It is necessary to note that the optimizer is an optional choice mainly used in post-processing. If it is disabled, the following EKF will be updated completely according to the result of the factor graph.

GNSS and MSFO switching is the key of the scene filter. In the actual scene, the GNSS high-precision working area is distributed in paragraphs due to the specific environmental impact. At the location where the GNSS quality changes, the scene filtering will select the GNSS positioning points $(x_g,y_g,z_g)$ and MSFO positioning points $(x_m,y_m,z_m)$ at the same time or the nearest time as the switching points, switching between MSFO and GNSS. In actual operation, the carrier re-walks from the non-signal area to the good signal area, the MSFO has been running steadily for a period of time, and a certain error has been accumulated. When the GNSS factor is obtained for error elimination, the MSFO will be adjusted. With the continuous addition of GNSS factors, MSFO will gradually move closer to GNSS. In order to avoid the obvious dislocation of the selected switching time when the gap between the two is too large, it is necessary to delay the switching. When the MSFO is fully adjusted, switching can be realized by adding proximity discrimination.

The discriminant formula is shown in Equation (7).

$$\begin{array}{c}{\left(x_g-x_m\right)}^2+{\left(y_g-y_m\right)}^2+{\left(z_g-z_m\right)}^2<K\end{array}$$

(7)

where K is the consistency threshold. The smaller the K value is, the smaller the switching point dislocation is, and the whole trajectory is smoother.

3.2.3. EKF Optimization Smoothing

The Scene Optimizer (SO) provides high-quality, low-frequency positioning information, which is further fused with high-frequency IMU information by EKF.

EKF consists of predictions and updates [33]. First, the IMU motion equation is considered, as shown in Equation (8).

$$\begin{array}{cc}{x}_k& =f\left(x_{k-1},v_k,w_k\right)\hfill \\ & \hfill \approx {\check{x}}_k+F_{k-1}\left(x_{k-1}-{\hat{x}}_{k-1}\right)+w_k^{\text{'}}\end{array}$$

(8)

where $x_k$ is the system state of time $k$; $f\left(x_{k-1},v_k,w_k\right)$ is a nonlinear state transition function; and $w_k$ is a process noise, assuming it follows a normal distribution. The right-hand form can be obtained by linearizing the updated estimate ${\hat{x}}_{k-1}$ at the previous time as a reference point, and $F_{k-1}$ is the Jacobian matrix of the motion equation. In this paper, 12-dimensional state vector $x_k=\left[p,q,v\right]$ is used, including 3-dimensional coordinates $p$, three-dimensional attitude $q,$ and velocity $v$ in 3 directions.

The first stage of the algorithm is to perform the prediction step, which predicts the current state estimation and error covariance matrix in time, as shown in Equation (9)

$$\{\begin{array}{l}{\check{P}}_k=F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_k^{\text{'}}\hfill \\ {\check{x}}_k=f\left({\hat{x}}_{k-1},v_k,0\right)\hfill \end{array}$$

(9)

where ${\hat{P}}_k$ is the estimated error covariance, and $Q_k^{\text{'}}$ is the covariance matrix of noise.

Then, consider the observation equation, as shown in Equation (10).

$$\begin{array}{cc}{z}_k& =g\left(x_k,n_k\right)\hfill \\ & \approx {\check{y}}_k+G_k\left(x_k-{\check{x}}_k\right)+n_k^{\text{'}}\end{array}$$

(10)

where $z_k$ is the measurement value at that time; $g$ is the nonlinear sensor model that maps the state to the measurement space; and $n_k$ is the measurement noise of normal distribution. The observation equation can be linearized on the predicted value ${\check{x}}_k$ of the motion equation to obtain another form. $G_k$ is the Jacobi matrix of the observation equation. Then, execute the update step, as shown in Equation (11).

$$\{\begin{array}{l}{K}_k={\check{P}}_k{G}_k^T{\left(G_k{\check{P}}_k{G}_k^T+R_k^{\text{'}}\right)}^{-1}\hfill \\ {\hat{x}}_k={\check{x}}_k+K_k\left(y_k-g\left({\check{x}}_k,0\right)\right)\hfill \\ {\hat{P}}_k=\left(1-K_k{G}_k\right){\check{P}}_k\hfill \end{array}$$

(11)

EKF plays two important roles in this scheme. Firstly, it can improve the output frequency of the pose. Secondly, the update selected by the optimizer is switched back and forth between the GNSS pose and the LiDAR pose. Although the switching position is selected, there are still differences. EKF can further optimize and smooth these localities.

4. Experiment and Analysis

In order to verify the feasibility of this scheme and analyze the actual performance of this method qualitatively and quantitatively, this paper designs a series of experiments.

4.1. Self-Collection Dataset Experiments

Based on the unmanned vehicle platform introduced in 3.1, we carried out the acquisition of data. Two datasets were collected, which have different scales and different environmental characteristics. These datasets are playground data and campus data, and the relevant information is shown in

Table 2. Experimental dataset information.

Data Name	Track Length	Acquisition Speed	Environmental Conditions
Playgrounds	400 m	1 m/s	Unobstructed
Campus	1350 m	1 m/s	Partially obscured by vegetation, buildings, etc.

.

It should be noted that all experiments were conducted offline using data collected in advance. Specific tests are as follows.

4.1.1. Playground Data

We collected the complete data of the 400 m playground by having the carrier car run along the runway line counterclockwise from the lower right corner. The whole field of view is open, and the number of satellites that can be received is greater than 14, so the GNSS/IMU integrated navigation post-processing results can be regarded as true values. Based on the post-processing software Inertial Explorer, we restored the trajectory of the carrier and established the point cloud, as shown in Figure 6, which are, respectively, the Google map, the carrier trajectory, and the point cloud map corresponding to the playground data.

In order to intuitively show the accuracy of different trajectories, we use these three trajectories and laser raw data for scene reconstruction. The accuracy of the point cloud indirectly reflects the accuracy of the trajectory. Lidar Odometry and Mapping [34] (LOAM), LIO, and MSFO are compared based on the positioning trajectory and mapping performance, as shown in Figure 7.

When the LO algorithm runs to the upper left corner of the playground, there is serious error accumulation and channel degradation. The trajectory recovery is greatly different from the actual situation, as shown in Figure 7a,d. When LIO returns to the original point, an error accumulation of 2.48 m is generated, resulting in point cloud stratification, as shown in Figure 7b,e. After fusing the GNSS factor, the positioning accuracy of MSFO is effectively improved. The error is 0.16 m when returning to the origin, and the point cloud has no obvious stratification, such as Figure 7c,f. The experimental results show that the MSFO system integrated with GNSS can effectively suppress the error accumulation of LIO. The drawings of the partial enlargement of LIO and MSFO are shown in Figure 8.

4.1.2. Campus Data

We collected about 1500 m of data along the campus road. Among them, some areas have a wide field of vision and a good GNSS signal. Some areas are sheltered by tall vegetation and buildings, resulting in a small number of satellites that can be received, and the received GNSS signal was weak. Due to the influence of chassis stability, some sections in the acquisition process have severe jitter and a fast steering bump. The Google map corresponding to the collected campus environment is shown in Figure 9.

Based on the campus dataset, we compared and tested the LOAM, LIO, and MSFO schemes, the factor graph, and the filtering collaborative optimization scheme in this paper. LOAM, LIO, and MSFO are compared based on their positioning trajectories and mapping performances, as shown in Figure 10.

The experimental data show that the LOAM algorithm quickly accumulates the error and runs away, as shown in Figure 10a,d. Although the LIO scheme is superior to LOAM, it also has large cumulative errors, as shown in Figure 10b,c. The overall accuracy of the MSFO scheme is significantly better than the former two, which effectively inhibits the error accumulation of the SLAM system. There is only a ghost phenomenon in the area of intense motion, as shown in Figure 10c,f. The drawings of the partial enlargement of LIO and MSFO are shown in Figure 11.

There is good scene complementarity between GNSS and MSFO, as shown in Figure 12. GNSS loses signals in many road sections and cannot be located, while MSFO can always be located effectively, as shown in Figure 12a,b. Although the GNSS/IMU-based integrated navigation scheme can temporarily provide positioning for the signal occlusion area, the accuracy will decrease, such as in areas 1 and 2 in Figure 12c, which are densely wooded trails and approximately closed corridors. The SO switches measurements between GNSS and SLAM odometry to get the preferred measurement, as shown in (d). Meanwhile, we intercept the point cloud reconstruction of local sections, as shown in (e). In the dense forest section, GNSS/IMU point cloud reconstruction is seen in the upper left corner, and MSFO point cloud reconstruction is seen in the upper right corner; the GNSS/IMU point cloud reconstruction of the closed corridor is seen in the lower left corner, and the MSFO point cloud reconstruction is seen in the lower right corner.

The final pose trajectory of the campus data GIL-CKF is obtained by further fusing the alternating update amounts of GNSS and MSFO (shown in Figure 12) with IMU in EKF, as shown in Figure 13.

In the left part of the figure, because of the serious occlusion of trees and the poor state of the GNSS signal, the SO switches the update amount to MSFO. Select one of the adjacent points to compare (point A), the horizontal difference of 0.427 m, and the elevation difference of 2.241 m. In the middle of the figure on the right is a closed corridor; the GNSS signal is partially lost, and the SO switches to MSFO. Select a pair of adjacent points to compare (point B); the horizontal difference is 1.959 m, and the elevation difference is 1.109 m. In the lower right corner of the figure, the carrier enters the open area. The scene is filtered to determine and switch to a better GNSS track. At the same time, due to the accumulation of errors in the SLAM system, MSFO continues to deviate from GNSS. One pair of adjacent points is selected for comparison (point C). The lateral distance is 0.319 m, and the elevation difference is 0.469 m. This situation is corrected by reaching the middle right. In most open areas, thanks to the high robustness of MSFO, the difference between the GNSS and MSFO trajectories is so small that they appear to fit perfectly.

For the recovery of the carrier motion trajectory, the collaborative optimization scheme of factor graph and filtering proposed in this paper achieves better results than the single GNSS/IMU integrated navigation scheme or the MSFO scheme fused the GNSS using the factor graph. It can not only suppress the error of the SLAM system in the two-step fusion, obtain a relatively more accurate pose in the weak GNSS environment, but also fully retain the high-precision positioning information of GNSS in the signal-free environment, and mitigate the conflict between local optimum and global optimum.

4.2. Utbm Dataset Experiment

In this paper, the open-source urban road dataset Utbm robocar is tested. The dataset collects about 5 km of urban road data from the vehicle [35]. The collection route passes through the city center, parks, residential areas, commercial areas, and bridges and has typical urban characteristics. This paper mainly tests and compares the LIO and MSFO algorithms.

It can be seen in Figure 14 that the MSFO integrated with GNSS can effectively suppress the error accumulation, and it still performs well on large-scale datasets.

In the Utbm dataset test, although GNSS/IMU was able to locate throughout the whole scene range, however, the GNSS/IMU integrated navigation deviated from MSFO in many local areas affected by GNSS signal fading, as seen in Figure 15a. The SO switch measurement between GNSS and MFSO to get the preferred measurement, as shown in Figure 15b.

The accuracy evaluation calculation usually takes differential GNSS positioning as a benchmark, and the output of the algorithm is compared with the absolute accuracy of differential GNSS. However, due to the rapid decline of GNSS positioning accuracy in weak GNSS signal areas, it is not appropriate to still use GNSS as a benchmark. In this paper, the alternating update quantity is used as the benchmark, and GNSS is regarded as the true value in the open signal area to evaluate the accuracy of GIL-CKF and MSFO. In the weak GNSS signal area, MSFO is regarded as the true value, and the accuracy of GIL-CKF and GNSS is evaluated. In this section, 6 points marked in Figure 16 are statistically calculated, and the results are shown in

Table 3. GNSS/MSFO/GIL-CKF relative accuracy (unit: meter).

Measurement	Points	Coords	GNSS	MSFO	GIL-CFK	GNSS-MSFO	GNSS-(GIL-CKF)	MFSO-(GIL-CKF)
MSFO	A	x	−345.058	−346.122	−346.166	1.064	1.108	0.044
		y	135.007	133.756	134.136	1.251	0.871	−0.38
		z	6.714	10.599	10.341	−3.885	−3.627	0.258
	C	x	85.709	85.134	85.115	0.575	0.594	0.019
		y	−272.204	−270.429	−270.21	−1.775	−1.994	−0.219
		z	−48.297	−48.025	−48.203	−0.272	−0.094	0.178
	F	x	3.678	3.442	3.439	0.236	0.239	0.003
		y	−270.326	−271.588	−271.126	1.262	0.8	−0.462
		z	−48.352	−48.124	−48.24	−0.228	−0.112	0.116
GNSS	B	x	42.496	42.158	42.472	0.338	0.024	−0.314
		y	281.017	281.219	280.832	−0.202	0.185	0.387
		z	−35.791	−36.235	−35.81	0.444	0.019	−0.425
	D	x	325.273	327.197	325.228	−1.924	0.045	1.969
		y	−484.906	−483.345	−484.83	−1.561	−0.076	1.485
		z	−48.351	−48.14	−48.363	−0.211	0.012	0.223
	E	x	161.932	161.896	161.922	0.036	0.01	−0.026
		y	−663.874	−664.2	−663.863	0.326	−0.011	−0.337
		z	−44.548	−44.518	−44.468	−0.03	−0.08	−0.05

.

It can be seen from the accuracy statistics that there is a large relative error between GNSS and MSFO at the same time, which usually reaches the meter level. GIL-CKF is close to the selected update amount at the same time, especially the GNSS update section, and the relative error is at the centimeter level, such as points B, D, and E in Table 3. In the MSFO update section, the relative error of GIL-CKF is also maintained at the decimeter level, such as points A, C, and F in Table 3. The relative error of GIL-CKF with different update quantities is different, which is mainly related to the covariance of the two types of update quantities. In open areas, the covariance of GNSS positioning information usually remains at the same level, whereas MSFO has the phenomenon of error accumulation and covariance growth.

At the same time, the cumulative errors of several methods involved in the experiment are also counted. The calculation method of cumulative errors is the distance value between the carrier and the end point coordinates (obtained by post-processing differential GNSS). The results are shown in

Table 4. Cumulative error (unit: meter).

Datasets	LO	LIO	MSFO	GIL-CKF
Playground dataset	>50	2.48	0.16	0.04
Campus dataset	>100	18.3	0.38	0.05
Utbm	——	>100	2.27	1.36

.

It can be seen from the cumulative error that the MSFO system integrated with GNSS can effectively suppress the error accumulation of LIO, and the GIL-CKF with alternating updates can further improve the accuracy.

5. Discussion

The pose estimation problem of mobile carriers is the core of mobile measurement. In recent years, with the continuous advancement and exploration of mobile robots and unmanned platforms in various application scenarios, the importance of pose estimation for mobile carriers has become more obvious. At the same time, the diversity of the environment brings great challenges to pose estimation. GNSS signal occlusion, multipath, SLAM scene feature degradation, and dynamic target interference are the difficulties that pose estimation techniques must overcome. This paper argues that multi-sensor fusion is an important way to deal with these challenges. In terms of multi-sensor information fusion, filtering and optimization have their own advantages and disadvantages. The combination of the two will become a consensus, and the combination will become closer. Through the experimental verification of this paper, the technical route of combining graph optimization and filtering fusion can provide a new idea for the current multi-sensor fusion, but there are still some shortcomings that can be further explored, summarized as follows:

(1) The value of the proximity threshold $K$ depends on empirical judgment. Too strict $K$ may lead to the loss of some accurate GNSS positioning information, but too loose may lead to the introduction of some inaccurate GNSS positioning information, thus affecting the accuracy of the results. More effective judgment needs to be considered later;

(2) LIO-SAM is a mature SLAM framework with perfect function and high precision. The implementation of SLAM in this paper draws on LIO-SAM and should be further studied on this basis;

(3) Our scheme is designed under the idea of combining filtering and graph optimization, and this is a loosely coupled design. Graph and filtering are independent of each other, and there is no data exchange between them. It is very fascinating to explore the tight coupling scheme of filtering and graph optimization.

6. Conclusions

Aiming at the demand for high-precision pose estimation in mobile measurement, this paper synthesizes the theoretical results of previous studies, takes the scene complementarity of multi-sensor as the foundation, and adopts the design idea of combining filtering and graph optimization to propose a GNSS/IMU/LiDAR multi-constrained filtering fusion system, GIL-CKF. In this paper, a two-step optimization strategy is adopted. Firstly, factor graph fusion is used to integrate GNSS information into the SLAM system to achieve overall accuracy control. Furthermore, this paper designs a scene optimizer SO based on GNSS quality indicators. With the cooperation of SO, GNSS and MSFO alternately fuse with IMU data in EKF filtering. The experimental equipment of this scheme integrates the Inertial Measurement Unit (IMU), GNSS, and LiDAR. The algorithm makes full use of the high-precision positioning ability of GNSS in the good signal area, the positioning ability of SLAM in the weak GNSS signal area, the error control ability of graph optimization fusion, and the high-frequency positioning ability of filtering to maximize the utilization of limited information.

The experiment with playground data shows that the fusion of high-precision GNSS positioning information and LIO through a factor graph can suppress error accumulation and improve the overall accuracy of SLAM. Experiments based on campus data and Utbm data show that the introduction of scene division based on GNSS quality indicator can retain high-precision positioning information as much as possible, alleviate the conflict between global optimization and local optimization, and improve the pose accuracy of local road sections. SO combined with EKF filtering further optimizes the local trajectory accuracy, and can improve the output frequency and smooth the trajectory. On the whole, the GIL-CKF proposed in this paper achieves better overall trajectory accuracy than a single GNSS/IMU integrated navigation system, or MSFO. It can not only suppress the error of the SLAM system in the two-step fusion to obtain a more accurate pose in the weak GNSS environment but also fully retain the high-precision positioning information of the GNSS in the unblocked environment of the signal to mitigate the conflict between local optimization and global optimization. This scheme is effective in improving pose accuracy and robustness and has the characteristics of full scene coverage, high global accuracy, and high output frequency.