Gravity-Constrained Simultaneous Localization and Mapping for suppressing map warping in complex large-scale environments

Abstract

Simultaneous Localization and Mapping (SLAM) serves as a foundational technology for autonomous systems operating within large-scale, complex environments. Traditional SLAM methodologies, however, are prone to altitude-axis distortions resulting from cumulative errors. To mitigate these issues, Gravity-Constrained SLAM (GC-SLAM) is introduced as a novel computational method that integrates gravity constraints and incremental optimisation to enhance mapping accuracy and computational efficiency. GC-SLAM incorporates a gravity constraint handling actor within the global optimisation algorithm, effectively reducing vertical-axis errors caused by accumulated drift during mapping. Furthermore, an incremental optimisation strategy is employed to manage the computational complexity associated with increasing map size. Performance evaluations of GC-SLAM are conducted on the KITTI dataset and large-scale environments, comparing its effectiveness against state-of-the-art SLAM-based algorithms, including FAST-LIO2, LIO-SAM (Lidar Inertial Odometry and SLAM), Lego-LOAM (Lightweight and Ground-optimised Lidar Odometry and Mapping), and A-LOAM (Advanced Lidar Odometry and Mapping). Experimental results demonstrate that GC-SLAM effectively suppresses vertical-axis distortions, significantly enhances localisation accuracy, and outperforms competing methods.

Keywords

complex large-scale scene mapping warpage gravity constraint incremental optimisation

1. Introduction

In the fields of robotics,^1–3 autonomous driving,^4–6 and augmented reality,^7,8 Simultaneous Localization and Mapping (SLAM) technology has garnered significant attention. SLAM enables mobile robots to achieve self-localization and construct environmental maps in unknown environments.^9,10 To enhance the robustness and adaptability of SLAM systems, researchers has focused on integrating advanced techniques into its workflow. For example Optimization algorithms^11–13 improve SLAM’s real-time map consistency; Combinatorial methods^14–16 address multi-robot SLAM coordination; Engineering applications^17–19 validate SLAM’s scalability. These integrated approaches enable SLAM to operate effectively in complex scenarios such as autonomous driving^20–22 and mobile robotics,^23–25 where dynamic environmental interactions demand high reliability.

The accuracy of positioning and mapping is a decisive factor that directly impacts the performance and reliability of robotic systems. Traditional SLAM frameworks often rely on single-sensor configurations tailored for specific scenarios. Such as LiDAR-centric systems for high-precision 3D environmental modeling,^26–28 vision and robotic control methods^29–31 and structural monitoring techniques.^32,33 However, as application scenarios grow more complex, these methods struggle to meet the increasing demands for positioning accuracy and robustness, particularly in large-scale and intricate environments.^34,35

In recent years, multi-sensor fusion has emerged as a dominant research direction, with various strategies proposed to enhance performance.^36–38 Technologies in this area utilize data integration from multiple sensors to address traditional lasers^39–41 as well as SLAM systems that fuse vision,⁴² improving their reliability and accuracy across diverse applications, for example, in multi-task execution^43,44 and multi-bot,^45,46 as well as in task provisioning.^47,48

Cartographer⁴⁹ employs a sparse pose graph optimisation method, integrating Lidar, IMU, wheel odometer, and GPS as factor nodes. It establishes constraints through pose relationships and incorporates back-end loop closure factors for optimisation, achieving notable success in 2D laser SLAM with widespread engineering applications. Similarly, Lidar Odometry and Mapping (LOAM)³⁹ adopts multi-sensor fusion for 3D mapping, leveraging surface and edge feature extraction to reduce computational complexity. Its separation of processing structures ensures real-time, high-precision localization and mapping.

Building on LOAM, methods such as Lightweight and Ground Optimized LOAM (LeGO-LOAM),⁴⁰ Robust LOAM (R-LOAM),⁵⁰ Fast LOAM (F-LOAM),⁵¹ and Lidar-Inertial Odometry with GPS-SLAM (LIO-SAM)⁴¹ have achieved robust results in common scenarios. However, these approaches often face challenges in complex, large-scale environments. Cumulative errors can result in height-direction warping, undermining map accuracy and reliability and, in some cases, preventing successful map construction. Addressing these issues—reducing cumulative errors and suppressing map distortion while maintaining computational efficiency-remains a critical challenge in SLAM research.

This study introduces a novel SLAM computational method designed to address the warping problems observed in mainstream algorithms when applied to complex, large-scale environments. The primary contributions of the proposed approach are outlined as follows:

A new SLAM algorithm, Gravity-Constrained SLAM (GC-SLAM), is developed by integrating gravity constraints and incremental optimisation techniques. This computational method enhances the robustness and precision of simultaneous localization and mapping in complex, large-scale environments.

A gravity constraint factor is introduced, utilising IMU data to correct point cloud data and eliminate accumulated errors along the vertical axis, thereby resolving the warping issues prevalent in mainstream algorithms for large-scale, complex environments. Additionally, incremental optimisation methods are employed to mitigate the computational demands associated with expanding map sizes, reducing overall computational complexity.

The effectiveness of the GC-SLAM computational method is validated through comparisons with existing mainstream SLAM systems. Experimental results indicate that GC-SLAM not only suppresses map warping in complex, large-scale environments but also delivers substantial improvements in positioning accuracy across both common and challenging scenarios.

2. Related work

Currently, most mainstream SLAM algorithms are based on graph optimisation.^40,41,52 These algorithms perform global optimisation through back-end loop closure detection to eliminate cumulative errors. However, in large-scale, complex scene reconstruction, the long distances involved in loop detection prevent these algorithms from addressing cumulative errors promptly, resulting in map warping.

As illustrated in Figure 1, the FAST-LIO2 mapping of the park dataset exhibits a significant warping issue. Regions A and B in Figure 1 correspond to the same physical location but are misaligned due to cumulative errors along the gravity direction. This issue is not exclusive to FAST-LIO2; in extensive and complex environments, warping problems are observed as a common challenge across mainstream SLAM algorithms.^53–55

Figure 1.

FAST-LIO2 mapping on park dataset.

In recent years, several studies have applied gravity as a constraint in SLAM. For example, VINS-Mono⁵⁶ aligns the Structure from Motion (SFM) results with the IMU integral in a loosely coupled manner, corrects the gravity vector, and then rotates it to the z-axis to obtain the rotation relationship between the world coordinate system and the camera coordinate system. Kubelka⁵⁷ proposed Kubelka proposed a tightly-coupled variant of the Iterative Closest Point (ICP) algorithm⁵⁸ for the front-end of LiDAR SLAM. This algorithm utilizes the observable degrees of freedom in the IMU orientation to obtain pose estimates aligned with the gravity vector, simplifies the 6-degree-of-freedom (DOF) pose estimation to 4 DOF, and reduces mapping drift. Verified by experiments in large-scale outdoor environments and the DARPA Subterranean Challenge, compared with the standard 6-DOF ICP, the new algorithm can reduce the localization drift. However, this method only considers the gravity vector at the front-end of the SLAM process. Recently, PALoc⁵⁹ used gravity measurements from the IMU as a constraint factor to optimise the map globally, generating high-quality ground truth trajectories, but only under the zero velocity update condition. In these related works, gravity constraints were applied either to the front-end or the back-end under specific conditions.

In contrast, the aim is to incorporate gravity constraint factors into the back-end without being restricted to specific conditions. For mapping in complex and large-scale scenarios, gravity constraint factors are proposed to be integrated into the factor graph. By leveraging IMU data, point cloud data can be corrected to eliminate accumulated errors along the vertical direction. This approach addresses the issue of delayed optimisation caused by excessively long loop detection distances.

Building on previous work, the proposed approach integrates gravity constraints into back-end optimisation, leveraging an incremental approach to address computational complexity. iSAM³⁷ first introduced incremental optimisation, using Bayesian networks to efficiently merge new observations with historical data, performing optimisation only on the affected parts. This method reduces computational complexity and enables the application of SLAM technology in large-scale environments. Building on this, iSAM2³⁸ uses a Bayesian tree structure to improve the algorithm’s efficiency and flexibility. Additionally, algorithms such as LIO-SAM, Cartographer, PL-VIO,⁴² and Kintinuous⁶⁰ also employ incremental optimisation techniques.

In summary, most current mainstream SLAM algorithms are based on graph optimization. They rely on backend loop closure detection for global optimization to eliminate cumulative errors. Only a few algorithms use IMU data to correct map warping in specific environments or at the front-end of the SLAM system. The above-mentioned solutions provide the foundation and feasibility for the algorithm in this paper. However, in practical engineering applications, there are still situations such as the inability to perform global optimization due to the lack of loop closures and complex and changeable environments. Moreover, due to the limited accuracy of industrial sensors, correcting only at the front-end will still lead to error accumulation. Considering the computational demands introduced by the gravity constraint factor and the increased complexity as the map expands, incremental optimisation is applied to reduce computational overhead and improve the system’s real-time performance. Additionally, the gravity constraint factor can be frequently invoked to eliminate accumulated errors.

3. Gait optimisation design

The architecture of the proposed method is presented, consisting of five components: Lidar-Inertial Odometry (LIO), Loop Closure Detection, Factor Graph, Incremental Optimisation, and Gravity Direction Extraction, as shown in Figure 2. The LIO component fuses sensor data to obtain laser odometry. The Loop Closure Detection function determines whether the robot has revisited a previously encountered location. The Factor Graph algorithmic component uses this information as constraints to construct a graph model. By incorporating data from the Factor Graph component, GPS data, and the Gravity Direction Extraction function, Incremental Optimisation is triggered to refine the graph model, yielding the optimal trajectory and map.

Figure 2.

Architecture of the GC-SLAM algorithm.

3.1. Lidar-inertial odometry

Inspired by LIO-SAM⁴¹ and LEGO-LOAM,⁴⁰ the LIO component of the method presented in this paper follows a similar approach, encompassing five primary algorithmic components: IMU pre-integration, LiDAR data distortion correction, point cloud feature extraction, pose prediction, and laser odometry acquisition. The data obtained from IMU pre-integration⁶¹ is used to rectify distortions in the LiDAR data. Following this, feature extraction⁶² is performed on the corrected LiDAR data to identify both line and plane features. The pose prediction component primarily employs a nonlinear optimisation method⁴¹ to fuse data from the IMU and wheel odometry, deriving an estimated inter-frame pose, which serves as the iterative initial value for the ICP (Iterative Closest Point) matching algorithm⁵⁸ to solve for accurate laser odometry.

3.2. Loop closure detection

In this study, the Scan Context method proposed by Kim et al.⁶³ is adopted for loop closure detection, seamlessly integrating laser point cloud data with map information to determine whether the robot has revisited a known location. The Scan Context method converts point clouds into rotationally invariant images using a two-dimensional descriptor, expediting the search for potential loop closure candidates through Ring Keys. Specifically, the Ring Key of the point cloud frame to be matched is searched in the KD Tree to find similar values and their corresponding scan indices. Then, the distances between the Scan Contexts of the selected candidate frames and that of the point cloud to be matched are calculated. In the subsequent matching process, the optimal loop closure candidate is identified. In the design of this algorithm, the distance threshold for judgment and the similarity threshold are set to 2 meters and 0.4 respectively. Among the key frames that meet the conditions, the candidate frame with the highest similarity is selected for verification. The candidate frame with the highest similarity is selected for verification to ensure the accuracy of the match. Once validated, this critical information is incorporated into the SLAM system’s pose graph optimisation, reducing accumulated errors and enhancing map coherence. The Scan Context method, known for its efficiency and accuracy, significantly improves the loop closure detection capabilities of laser SLAM. Upon detection, the loop closure factor is added to the factor graph, enabling global optimisation.

3.3. Factor graph

As shown in Figure 3, the factor graph consists of observation factors, motion factors, and prior factors. The observation factors provide absolute pose constraints for the robot state and include gravity factors, loop closure factors, and GPS factors. These factors indicate the constraint relationship between poses generated by the robot motion model, which is primarily composed of laser odometry poses. The prior factors represent prior knowledge of the system state and are used to initialise or fix the system state.

Figure 3.

GC-SLAM factor graph.

3.4. Gravity direction extraction

To address the issue of map distortion, a Gravity Direction extraction component has been designed. This algorithmic component extracts the gravity direction from the IMU, compares it with the gravity direction in the map, and uses the comparison as a gravity constraint factor for optimisation.

The gravity constraint factor is generated as follows. The map pose at the $i$ -th step is represented as $T_{i} = (R_{i}, t_{i})$ , where $R_{i} \in S O (3)$ is the rotation matrix, and $t_{i} \in R^{3}$ is the translation vector. The sequence of poses from SLAM is ${T_{0}, T_{1}, T_{2} \dots \dots, T_{n}}$ . The IMU provides the gravity direction as $g_{i m u} \in R^{3}$ , which in the IMU coordinate system is typically $[0, 0, - 1]$ . When the orientation is correct, the world frame gravity direction $g_{w o r l d} \in R^{3}$ should align with the gravity direction from the IMU. For each pose $T_{i}$ , the rotation matrix $R_{i}$ should align the transformed IMU gravity direction $R_{i} g_{i m u}$ with the world gravity direction $g_{w o r l d}$ . The error is defined as:

r_{g} (R_{i}) = R_{i} g_{i m u} - g_{w o r l d} .

(1)

3.5. Incremental optimisation

Incremental optimisation achieves efficient computation and real-time response by optimising only new or changed data, while maintaining the scalability of the system and avoiding redundant computation typically caused by global optimisation. This method not only improves the efficiency of processing large-scale data, but also reduces the complexity of implementation. When no loop closure is detected, incremental optimisation is performed using GPS data, gravity data, and LiDAR odometry pose data. The proposed algorithm can suppress map warping and optimise positioning at a frequency of 10 Hz. The set of state variables is denoted as $x_{i}$ . Each state variable can be expressed as $x_{i}$ , with the overall goal of minimising the weighted sum of squares of all factor residuals $r_{opt}$ , which is given by:

r_{opt} = min_{x} \sum_{k} | | r_{k} (x) | |_{Λ_{k}}^{2} .

(2)

where

r (x) = {r_{Pose}, r_{g}, r_{Loop}, r_{GPS}}

is the residual function of the

k

-th factor, and

Λ_{k}

is the covariance matrix of the

k

-th factor. By performing a first-order Taylor expansion of the residual function

r_{k} (x)

at the current estimate

x^{(n)}

, it results that:

r_{k} (x) \approx r_{k} (x^{(n)}) + J_{k} δ x .

(3)

where

J_{k}

is the Jacobian matrix of the residual function

r_{k} (x)

x^{(n)}

, and

δ x = x - x^{(n)}

is the increment between the current estimate and the actual state. Thus, the optimisation problem can be rewritten as:

r_{opt} = min_{δ x} \sum_{k} | | r_{k} (x^{(n)}) + J_{k} δ x | |_{Λ_{k}}^{2} .

(4)

Expanding this:

\begin{aligned} r_{opt} & = min_{δ x} \sum_{k} (r_{k} {(x^{(n)})}^{T} Λ_{k} r_{k} x^{(n)} + 2 r_{k} {(x^{(n)})}^{T} Λ_{k} J_{k} δ x) \\ + min_{δ x} \sum_{k} δ x^{T} {J_{k}}^{T} Λ_{k} J_{k} δ x . \end{aligned}

(5)

Since the term that does not contain $δ x$ has no effect on the optimisation result, it can be omitted. The remaining part is of a quadratic form:

r_{opt} = min_{δ x} \sum_{k} (2 r_{k} {(x^{(n)})}^{T} Λ_{k} J_{k} δ x + δ x^{T} {J_{k}}^{T} Λ_{k} J_{k} δ x) .

(6)

This can be further organised as:

r_{opt} = min_{δ x} b^{T} δ x + \frac{1}{2} δ x^{T} H δ x .

(7)

where

b = - 2 \sum_{k} {J_{k}}^{T} Λ_{k} r_{k} (x^{(n)})

is the gradient vector and

H = \sum_{k} {J_{k}}^{T} Λ_{k} J_{k}

is the information matrix. The state variables are updated iteratively as:

x^{(n + 1)} = x^{(n)} + δ x .

(8)

This process is repeated until $δ x$ is small enough to indicate convergence. The flow of the incremental algorithm is shown in Algorithms 1 and 2. Incremental optimization, as an improvement of nonlinear optimization under specific conditions, performs well in the Simultaneous Localization and Mapping (SLAM) problem. In the specific implementation of the algorithm, the ideas of iSAM2³⁸ are adopted, and algorithms such as the Bayesian tree structure, dynamic sub-graph updating, fluid relinearization, and incremental state variable reordering are used. During each optimization, it solves the sub-graph affected by newly added variables and observations, and only updates and solves the affected part of the variables. This effectively reduces the computational burden of nonlinear optimization, making high-frequency real-time optimization possible. Moreover, its accuracy is consistent with that of batch optimization.

4. Experimental results and analysis

To evaluate the effectiveness of GC-SLAM in suppressing map warping and improving positioning accuracy, comparative experiments were conducted with LIO-SAM, Lego-LOAM, FAST-LIO2, and A-LOAM across three scenarios.

The first experiment was conducted using the Park dataset, a complex large-scale scene with few loops. This experiment aimed to verify the ability of GC-SLAM to suppress the map warping phenomenon, which is common in mainstream algorithms. Practical experience shows that map distortion exceeding 2 m significantly impacts robot navigation. The following formula was used to determine whether the map was warped, based on the value of the height error:

if Height Error {\begin{cases} > 2 m, & warp \\ < 2 m, & no warp. \end{cases}

(9)

The second experiment was conducted on the Farm dataset. While the scene is not large, it is complex due to the lack of linear features. This experiment aimed to demonstrate the significant improvement of GC-SLAM over other algorithms in small but complex environments, and to verify the effectiveness of gravity in eliminating errors along the Z-axis.

The third experiment was performed using the KITTI dataset to verify that GC-SLAM also offers significant advantages in positioning accuracy in common scenarios.

The experiments were conducted using two sets of equipment: a handheld laser scanner and a six-wheeled experimental cart. The experimental setup is shown in Figure 4. The handheld scanner was equipped with a RoboSense 16-line LiDAR, a 6-axis IMU, and a single-antenna Real-Time Kinematic (RTK) to collect data for the farm scenario. The six-wheeled cart was equipped with a VLP-16-line LiDAR, a 6-axis IMU, and dual-antenna RTK to collect data for the park scenario. Finally, the collected datasets, along with the KITTI dataset, were processed on an Intel Core i7-6600U processor.

Figure 4.

Dataset acquisition equipment. (a) Handheld laser scanner; (b) Six-wheeled experimental cart.

4.1. Park dataset

The first experimental scenario took place in a park that spans an area larger than 1000 m by 500 m, with a total length exceeding 3000 m. In this dataset, we employed a method where the robot starts from the origin, completes a loop, and returns to the origin to test the algorithm’s ability to suppress map warping. As shown in Figure 5, the six-wheeled experimental vehicle starts from the black box, collects park data along a loop, and returns to the black box.

Figure 5.

Schematic diagram of the experimental vehicle’s origin.

This represents a typical dataset encountered in industrial applications. Mapping experiments were performed using each algorithm, and the resulting mapping outcomes are presented in Figure 6.

Figure 6.

Mapping results of methods using the park dataset. (a) LEGO-LOAM; (b) LIO-SAM; (c) A-LOAM; (d) GC-SLAM.

Figure 6 shows the trajectory of each algorithm on the park dataset, with the red boxes indicating the starting and ending positions, which should coincide. Lego-LOAM, LIO-SAM, A-LOAM, and FAST-LIO exhibit significant warping issues in the altitude direction. In contrast, there is no warping in the red box for GC-SLAM, demonstrating its significant effectiveness in inhibiting map distortion. The robot starts from the black box in Figure 5, travels around the drivable roads in the park, and then returns to the black box. Since the robot completes a loop and returns to the starting point, the actual height change between the start and end points should ideally be 0. Table 1 shows the height differences (height errors) between the starting and ending points for each algorithm, which vividly reflects the degree of distortion in other algorithms. The smaller the height error, the better the algorithm’s ability to suppress map warping.

Table 1.

The warping results of the park dataset.

Method	Height error(m)	Result
LIO-SAM	30.94	warp
Lego-LOAM	11.03	warp
FAST-LIO2	28.57	warp
A-LOAM	51.23	warp
GC-SLAM	0.06	no warp

4.2. Farm dataset

The second experiment uses a farm dataset, where the acquisition path spans approximately 720 metres away from the passable road within the farm. The true trajectory of the path is provided by RTK positioning data. This dataset represents a small yet complex environment, characterised by a large number of plants and a lack of linear and planar features in the sensor data, which impacts the mapping performance. Each algorithm is built and compared using this dataset. The results of the acquisition environment and experimental trajectory are shown in Figure 7, while the error comparison results are provided in Table 2 and Figure 8. In Table 2, “Max”, “Min”, and “Rmse” represent the maximum error, minimum error, and root mean square error, respectively, between the algorithm’s trajectory and the true trajectory.

Figure 7.

Trajectory result of GC-SLAM using the Farm dataset. (a) Data acquisition environment; (b) Mapping results of farm.

Figure 8.

Trajectory of farm dataset. A-LOAM is far from the truth trajectory, LIO-SAM, Lego-LOAM and FAST-LIO2 trajectories are close to and have relatively large deviations from the truth trajectory, GC-SLAM trajectory is the closest to the truth trajectory, and GC-SLAM also performs well in the absence of RTK.

Table 2.

The ATE(m), CPU usage, and memory occupancy rate on the farm dataset.

Method	Max	Min	Rmse	CPU	Mem
LIO-SAM	1.1255	0.0166	0.4223	18.24	22.91
Lego-LOAM	1.5843	0.0279	0.5670	14.65	31.19
FAST-LIO2	2.1613	0.1703	1.3161	15.05	46.12
A-LOAM	7.4591	0.6140	3.4830	17.80	33.00
GC-SLAM	0.7349	0.0362	0.3252	18.50	34.20
(NO-RTK)
GC-SLAM	0.4772	0.0047	0.1900	19.75	36.50

These values were further analysed, and the Absolute Trajectory Errors (ATE)^64,65 were compared. The calculation formula is as follows:

A T E (t) = ‖ T_{g t} (t) - T_{e s t} (t) ‖ .

(10)

where

T_{g t} (t)

represents the true pose at time

t

T_{e s t} (t)

is the estimated pose at time

t

, and

‖ . ‖

denotes the distance between the two. The results are presented in Table 2. During the 700-metre drive, the GC-SLAM algorithm demonstrated a maximum positioning error of 0.4 metres and a root mean square error (RMSE) of only 0.19 metres. This indicates that even in small-scale scenarios, the GC-SLAM algorithm offers significant advantages in accuracy over other algorithms. This is crucial for enhancing positioning precision and the overall effectiveness of map construction.

In order to further explore what losses this algorithm brings in suppressing map warping and improving positioning accuracy, we have statistically analyzed the average values of the CPU occupancy rate and the computer memory occupancy rate during the algorithm’s operation. The results are shown in Table 2. As can be seen from the table, the accuracy of the proposed algorithm has significantly improved compared to other algorithms. However, there is a certain increase in CPU and memory (Mem) occupancy rates. Nevertheless, considering the high-efficiency processing capabilities of current computers, these increases are acceptable, and they will not affect the real-time processing ability of our algorithm.

As in Figure 9, we compares the Absolute Trajectory Error (ATE) over time for six algorithms. The results demonstrate that GC-SLAM consistently achieves the lowest ATE values throughout the entire trajectory (0–600 seconds), significantly outperforming other methods.

Figure 9.

Absolute trajectory error (ATE) over time for different algorithms.

4.3. KITTI dataset

In the third experiment, sequence 2011-09-30-drive-0027 (KITTI07) from the KITTI⁶⁶ dataset is used, which represents a common scenario for illustrating the localisation accuracy of SLAM in typical environments. GC-SLAM is tested and compared with each of the other algorithms. The displacement curves along the x, y, and z axes for each algorithm are shown in Figure 10, and the error comparison results are presented in Table 3.

Figure 10.

Plot of displacement variation of each axis in KITTI dataset. This figure shows the trajectory curves of these algorithms along the x, y, and z axes. It can be observed that the GC-SLAM algorithm (blue) performs better. (a) Trajectory comparison in the x and y directions; (b) The change of the z-axis with time t.

Table 3.

ATE analysis table of KITTI07 dataset.

Method	Max	Min	Rmse	CPU	Mem
LIO-SAM	2.0262	0.1731	1.0395	20.06	25.33
A-LOAM	2.3398	0.0264	1.022	19.52	34.70
Lego-LOAM	1.9119	0.2065	0.9945	17.35	33.52
FAST-LIO2	10.71	1.0513	6.1845	17.83	48.61
GC-SLAM	0.6749	0.1621	0.7263	21.36	38.57

As can be seen from Figure 10, the results of each algorithm are relatively close in the $x$ and $y$ axis directions. However, in the z-axis direction, the error of other algorithms is significantly greater than that of GC-SLAM, which is closest to the true value.

As shown in Table 3, the experimental results are quantitatively analysed, and it is evident that GC-SLAM demonstrates higher accuracy and superior attitude estimation capabilities compared to other algorithms, even in common scenarios.

Through three experiments, the advantages of GC-SLAM have been demonstrated, and the effectiveness of the gravity constraint method combined with incremental optimisation in suppressing map warping and improving positioning accuracy has been validated. The experimental results show that GC-SLAM not only provides exceptional mapping capabilities in complex and large-scale environments—capabilities not found in other algorithms—but also achieves significantly better positioning accuracy than the other algorithms in common scenarios.

As presented in Table 4, we have conducted a statistical significance analysis to evaluate the differences between GC-SLAM and other algorithms. The results demonstrate that GC-SLAM significantly outperforms other methods in terms of localization accuracy and map quality.

Table 4.

Mean value $\pm$ standard deviation for farm and KITTI07.

	LIO-SAM			Lego-LOAM			Fast-LIO2			A-LOAM			GC-SLAM
Dataset	$μ$	$σ$	W	$μ$	$σ$	W	$μ$	$σ$	W	$μ$	$σ$	W	$μ$	$σ$
Farm	0.4239	0.0129	$+$	0.5605	0.0158	$+$	1.3146	0.0374	$+$	3.4909	0.0996	$+$	0.1904	0.0067
KITTI07	1.0447	0.0282	$+$	1.0159	0.0261	$+$	0.9943	0.0281	$+$	6.1454	0.1617	$+$	0.7338	0.0209

Additionally, we carried out an ablation study to verify the roles of the key subsystems in GC-SLAM, especially the gravity constraint factor and the incremental optimization strategy. The results shown in Figure 11 and Table 5 indicate that in Figure 11, we explored the outcomes of whether the system adopted the incremental approach or not. It can be seen that as the operation scenarios expand, the processing time of the traditional optimization algorithm becomes progressively longer and fails to meet the real-time requirements of the system. In contrast, GC-SLAM can still maintain a relatively fast processing speed even when the data volume increases. Table 5 shows the effects of warp suppression for three datasets under the conditions of adding and not adding the gravitational constraint factor, as demonstrated by the Z-axis error table. It can be observed that after adding this module to the system, the map warping is effectively suppressed. In the park dataset, due to the lack of loop closures over long distances, optimization cannot be carried out, so the map warping is extremely severe, reaching 15.72 meters. However, in the farm and KITTI datasets, because of the existence of loop closures, the map drift is not that serious. Both of these subsystems play a vital role in suppressing map distortion and improving positioning accuracy. Removing either of these subsystems will lead to a significant decline in performance, particularly in complex and large-scale environments.

Figure 11.

Processing time(ms) as the amount of lidar scans.

Table 5.

Z-axis error(m) table.

Method	Park	Farm	KITTI07
GC-NoGravity	15.72	0.15	0.19
GC-SLAM	0.06	0.04	0.07

5. Discussion

Experimental results demonstrate that, compared to existing SLAM frameworks, GC-SLAM effectively suppresses vertical-axis map distortion and achieves higher localization accuracy. This section further elaborates on the implications of these results, the limitations of the proposed method, and potential avenues for future research. The primary advantages and contributions of GC-SLAM include:

Gravity Constraints as a Robust Backend Factor: Unlike previous works that only applied gravity constraints to frontend pose estimation or under specific conditions (e.g., zero-velocity updates in PALoc⁵⁹), GC-SLAM integrates gravity constraints into the back-end optimization process. This approach ensures continuous correction of vertical-axis drift, even in scenarios lacking loop closures. An ablation study (Table 5) confirms that removing gravity constraints leads to severe warping (e.g., a height error of 15.72 m in the Park dataset), underscoring their necessity in large-scale environments.

Incremental Optimization for Scalability: Traditional batch optimization methods struggle with computational complexity as map size increases. By adopting incremental optimization (inspired by iSAM2³⁸), GC-SLAM achieves real-time performance (10 Hz update rate) while maintaining accuracy comparable to full batch optimization. Figure 11 highlights the efficiency gains: GC-SLAM’s processing time remains stable even with growing data volume, unlike traditional methods that degrade significantly.

Outstanding Performance Across Various Scenarios: We have validated the robustness of GC-SLAM on datasets with different characteristics. In large-scale environments lacking loops (Park dataset), GC-SLAM reduces height error from 30 m (LIO-SAM) to 0.06 m; in feature-deprived scenarios (Farm dataset), GC-SLAM’s RMSE (0.19 m) is 3 to 18 times lower than competitors; in common scenarios (KITTI07), GC-SLAM achieves sub-meter z-axis error, outperforming methods like FAST-LIO2 (6.18 m RMSE).

Additionally, GC-SLAM has some limitations, such as increased computational overhead. While GC-SLAM’s incremental optimization mitigates computational demands, the addition of gravity constraints and frequent optimizations elevate CPU/memory usage (Tables 2 and 3). This may pose challenges for deployment on resource-constrained platforms (e.g., low-power robots or edge devices).

6. Conclusion

This paper presents GC-SLAM, a novel SLAM algorithm designed to address the warping issue in the gravity direction commonly encountered in complex and large-scale environments. By integrating incremental optimisation with a gravity constraint, GC-SLAM effectively mitigates the cumulative errors in altitude direction while maintaining computational efficiency. Operating at a frequency of 10 Hz, the proposed method not only suppresses map warping but also optimises positioning accuracy, reducing errors in the altitude direction across large and complex scenes. These advancements improve both the precision of positioning and the system’s real-time performance by efficiently managing computational resources.

Experimental results demonstrate the superiority of GC-SLAM over existing state-of-the-art methods. The proposed method outperforms current open-source solutions in both self-collected datasets from large-scale environments and well-established open-source datasets, confirming its robustness and practical applicability. The findings highlight GC-SLAM’s potential to enhance SLAM performance, particularly in scenarios where map warping and positioning accuracy are critical.

Footnotes

Acknowledgments

This work was supported by the Sichuan Science and Technology Program (2023YFG0046 and 2023NSFSC1985). We also acknowledge the Jiangsu Distinguished Professor Programme for their support.

ORCID iDs

Kaiyi Xian

Duo Liu

Gexiang Zhang

Ferrante Neri

Song Chen

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article.

Conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

References

Kubelka

Reinstein

Svoboda

. Tracked robot odometry for obstacle traversal in sensory deprived environment. IEEE ASME Trans Mechatron 2019; 24: 2745–2755.

Chen

Huang

Fitch

. Active SLAM for mobile robots with area coverage and obstacle avoidance. IEEE ASME Trans Mechatron 2020; 25: 1182–1192.

Zhai

. An efficient and robust tightly coupled framework for lidar-imu localization and mapping. In: 2023 IEEE international conference on robotics and biomimetics (ROBIO), 2023, pp.1–6.

Zhang

Chang

Zou

, et al. A robust graph-based bathymetric simultaneous localization and mapping approach for AUVS. IEEE J Ocean Eng 2024; 49: 1350–1370.

Ram

SPA

Zanetti

. Robocentric simultaneous localization and mapping. J Guid Control Dyn 2024. [Online]. https://api.semanticscholar.org/CorpusID:267078491.

González

Villar

Tan

, et al. An efficient multi-robot path planning solution using a* and coevolutionary algorithms. Integr Comput Aided Eng 2022; 30: 41–52.

Cuarán

Velasquez

AEB

Gasparino

, et al. Under-canopy dataset for advancing simultaneous localization and mapping in agricultural robotics. Int J Rob Res 2023; 43: 739–749.

Zhang

Wang

. Simultaneous localization and mapping of mobile robot based on environmental features. In: 2008 First international conference on intelligent networks and intelligent systems, 2008, pp.388–391.

Huang

Zeng

Chen

, et al. Edge robotics: Edge-computing-accelerated multirobot simultaneous localization and mapping. IEEE Internet Things J 2022; 9: 14 087.

10.

Lajoie

P-Y

Beltrame

. Swarm-SLAM: Sparse decentralized collaborative simultaneous localization and mapping framework for multi-robot systems. IEEE Robot Autom Lett 2024; 9: 475–482.

11.

Siddique

Adeli

. Hybrid harmony search algorithms. Int J Artif Intell Tools 2015; 24: 1 530 001:1.

12.

Siddique

Adeli

. Spiral dynamics algorithm. Int J Artif Intell Tools 2014; 23. [Online]. https://api.semanticscholar.org/CorpusID:34074613.

13.

Siddique

Adeli

. Harmony search algorithm and its variants. Int J Pattern Recognit Artif Intell 2015; 29: 1 539 001:1.

14.

Akhand

Ayon

Shahriyar

, et al. Discrete spider monkey optimization for travelling salesman problem. Appl Soft Comput 2020; 86: 105887.

15.

Akhand

MAH

Ayon

Shahriyar

, et al. Discrete spider monkey optimization for travelling salesman problem. Appl Soft Comput 2020; 86. [Online]. https://api.semanticscholar.org/CorpusID:209946722.

16.

Hossain

Akhand

MAH

Shuvo

MIR

, et al. Optimization of university course scheduling problem using particle swarm optimization with selective search. Expert Syst Appl 2019; 127: 9–24.

17.

Park

Adeli

. Distributed neural dynamics algorithms for optimization of large steel structures. J Struct Eng-ASCE 1997; 123: 880–888.

18.

Siddique

Adeli

. Simulated annealing, its variants and engineering applications. Int J Artif Intell Tools 2016; 25: 1 630 001:1.

19.

Wang

Zhong

Adeli

, et al. Smart bacteria-foraging algorithm-based customized kernel support vector regression and enhanced probabilistic neural network for compaction quality assessment and control of earth-rock dam. Expert Syst 2018; 35. [Online]. https://api.semanticscholar.org/CorpusID:57760418.

20.

Mailka

Abouzahir

Ramzi

, et al. Ukf slam-enhanced perception and navigation for effective area coverage in autonomous driving. In: 2024 IEEE international conference on sensing, diagnostics, prognostics, and control (SDPC), 2024, pp.200–205.

21.

Jeon

Moon

Jeong

, et al. Autonomous flight strategy of an unmanned aerial vehicle with multimodal information for autonomous inspection of overhead transmission facilities. Comput Aided Civ Infrastruct Eng 2024; 39: 2159–2186.

22.

Kamjoo

Rostami

Fakhrmoosavi

, et al. A simulation-based approach for optimizing the placement of dedicated lanes for autonomous vehicles in large-scale networks. Comput Aided Civ Infrastruct Eng 2024; 39: 3011–3029.

23.

Zhu

Xiao

J-Q

, et al. Global path planning of wheeled robots using multi-objective memetic algorithms. Integr Comput Aided Eng 2015; 22: 387–404.

24.

Liu

Rong

Neri

, et al. Entropy-weighted numerical gradient optimization spiking neural system for biped robot control. Int J Neural Syst 2024; 34: 2450030.

25.

Huangfu

Yue

Chu

, et al. The simultaneous localization and mapping technology for intelligent robots in coal mine environments. In: 2024 20th International conference on natural computation, fuzzy systems and knowledge discovery (ICNC-FSKD), 2024, pp.1–6.

26.

Yamaguchi

Mizutani

. Quantitative road crack evaluation by a u-net architecture using smartphone images and lidar data. Comput Aided Civ Infrastruct Eng 2023; 39: 963–982.

27.

Zhang

Wang

Han

, et al. Deep learning framework with local sparse transformer for construction worker detection in 3D with lidar. Comput Aided Civ Infrastruct Eng 2024; 39: 2990–3007.

28.

Esmorís

Vilariño

Fernández-Arango

, et al. Characterizing zebra crossing zones using lidar data. Comput Aided Civ Infrastruct Eng 2023; 38: 1767–1788.

29.

Zahra

Navarro-Alarcon

Tolu

. A neurorobotic embodiment for exploring the dynamical interactions of a spiking cerebellar model and a robot arm during vision-based manipulation tasks. Int J Neural Syst 2021; 2150028. [Online]. https://api.semanticscholar.org/CorpusID:231786375.

30.

Wang

Zhang

Neri

, et al. Design and implementation of membrane controllers for trajectory tracking of nonholonomic wheeled mobile robots. Integr Comput Aided Eng 2015; 23: 15–30.

31.

Ahmad

Siddique

Tokhi

. Modelling and simulation of double-link scenario in a two-wheeled wheelchair. Integr Comput Aided Eng 2014; 21: 119–132.

32.

Park

Lee

Adeli

, et al. A new approach for health monitoring of structures: Terrestrial laser scanning. Comput Aided Civ Infrastruct Eng 2007; 22. [Online]. https://api.semanticscholar.org/CorpusID:7876896.

33.

Park

Kim

J-H

, et al. 3D displacement measurement model for health monitoring of structures using a motion capture system. Measurement 2015; 59: 352–362.

34.

Liu

Rong

Neri

, et al. Deep deterministic policy gradient with constraints for gait optimisation of biped robots. Integr Comput Aided Eng 2024; 31: 139–156.

35.

Grosset

Ndao

Fougères

A-J

, et al. A cooperative approach to avoiding obstacles and collisions between autonomous industrial vehicles in a simulation platform. Integr Comput Aided Eng 2022; 30: 19–40.

36.

Wan

Yang

Cai

, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes. In: 2018 IEEE international conference on robotics and automation (ICRA), 2018, pp.4670–4677.

37.

Kaess

Ranganathan

Dellaert

. ISAM: fast incremental smoothing and mapping with efficient data association. In: Proceedings 2007 IEEE international conference on robotics and automation, 2007, pp.1670–1677.

38.

Kaess

Johannsson

Roberts

, et al. ISAM2: incremental smoothing and mapping with fluid relinearization and incremental variable reordering. In: 2011 IEEE international conference on robotics and automation, 2011, pp.3281–3288.

39.

Zhang

Singh

. LOAM: lidar odometry and mapping in real-time. In: Robotics: science and systems, 2014, [Online]. https://api.semanticscholar.org/CorpusID:18612391.

40.

Shan

Englot

. Lego-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain. In: 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS), 2018, pp.4758–4765.

41.

Shan

Englot

Meyers

, et al. LIO-SAM: tightly-coupled lidar inertial odometry via smoothing and mapping. In: 2020 IEEE/RSJ international conference on intelligent robots and systems (IROS), 2020, pp.5135–5142.

42.

Zhao

Guo

, et al. PL-VIO: Tightly-coupled monocular visual–inertial odometry using point and line features. Sensors 2018; 18. [Online]. https://www.mdpi.com/1424-8220/18/4/1159.

43.

Wang

Zhang

Gou

, et al. Multi-behaviors coordination controller design with enzymatic numerical P systems for robots. Integr Comput Aided Eng 2021; 28: 119–140.

44.

Djurasevic

Gil-Gala

Jakobović

. Constructing ensembles of dispatching rules for multi-objective tasks in the unrelated machines environment. Integr Comput Aided Eng 2023; 30: 275–292.

45.

Grosset

Fougères

A-J

Djoko-Kouam

, et al. Multi-agent simulation of autonomous industrial vehicle fleets: Towards dynamic task allocation in V2X cooperation mode. Integr Comput Aided Eng 2024; 31: 249–266.

46.

Díaz

Palacios

Rodríguez

, et al. An elitist seasonal artificial bee colony algorithm for the interval job shop. Integr Comput Aided Eng 2023; 30: 223–242.

47.

Wan

Schonfeld

, et al. Railway alignment optimization in regions with densely-distributed obstacles based on semantic topological maps. Integr Comput Aided Eng 2024; 31: 421–437.

48.

Daranda

Dzemyda

. Reinforcement learning strategies for vessel navigation. Integr Comput Aided Eng 2022; 30: 53–66.

49.

Hess

Kohler

Rapp

, et al. Real-time loop closure in 2D lidar SLAM. In: 2016 IEEE international conference on robotics and automation (ICRA), 2016, pp.1271–1278.

50.

Oelsch

Karimi

Steinbach

. R-LOAM: Improving lidar odometry and mapping with point-to-mesh features of a known 3D reference object. IEEE Robot Autom Lett 2021; 6: 2068–2075.

51.

Wang

Chen

C-L

, et al. F-LOAM: fast lidar odometry and mapping. In: 2021 IEEE/RSJ international conference on intelligent robots and systems (IROS), 2021, pp.4390–4396.

52.

Chen

Liu

. Tightly coupled 3D lidar inertial odometry and mapping. In: 2019 International conference on robotics and automation (ICRA), 2019, pp.3144–3150.

53.

Hanebeck

. Towards high-performance solid-state-lidar-inertial odometry and mapping. IEEE Robot Autom Lett 2021; 6: 5167–5174.

54.

Lin

Zhang

. LOAM livox: a fast, robust, high-precision lidar odometry and mapping package for lidars of small FOV. In: 2020 IEEE international conference on robotics and automation (ICRA), 2020, pp.3126–3131.

55.

Wang

Ding

. A robust lidar-inertial localization system based on outlier removal. In: 2021 China automation congress (CAC), 2021, pp.2420–2425.

56.

Qin

Shen

. Vins-mono: A robust and versatile monocular visual-inertial state estimator. IEEE Trans Robot 2018; 34: 1004–1020.

57.

Kubelka

Vaidis

Pomerleau

. Gravity-constrained point cloud registration. In: 2022 IEEE/RSJ international conference on intelligent robots and systems (IROS), 2022, pp.4873–4879. [Online]. https://api.semanticscholar.org/CorpusID:247748902.

58.

Besl

McKay

. Method for registration of 3-D shapes. In: Other conferences, 1992, [Online]. https://api.semanticscholar.org/CorpusID:122917928.

59.

Zheng

, et al. PALoc: Advancing SLAM benchmarking with prior-assisted 6-DOF trajectory generation and uncertainty estimation. IEEE ASME Trans Mechatron 2024; 1–12.

60.

Whelan

Kaess

Fallon

, et al. Kintinuous: spatially extended kinectfusion. In: AAAI conference on artificial intelligence, 2012, [Online]. https://api.semanticscholar.org/CorpusID:15010509.

61.

Forster

Carlone

Dellaert

, et al. Supplementary material to: IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation. 2015, [Online]. https://api.semanticscholar.org/CorpusID:14813805.

62.

Zhang

Singh

. Low-drift and real-time lidar odometry and mapping. Auton Robots 2017; 41: 401–416.

63.

Kim

. Scan context: egocentric spatial descriptor for place recognition within 3D point cloud map. In: 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS), 2018, pp.4802–4809.

64.

Niu

, et al. Tightly-coupled lidar-GNSS-inertial fusion odometry and mapping. In: 2024 43rd Chinese control conference (CCC), 2024, pp.3888–3893.

65.

Dong

, et al. R-LIOM: reflectivity-aware lidar-inertial odometry and mapping. IEEE Robot Autom Lett 2023; 8: 7743–7750.

66.

Geiger

Lenz

Urtasun

. Are we ready for autonomous driving? the KITTI vision benchmark suite. In: 2012 IEEE conference on computer vision and pattern recognition, 2012, pp.3354–3361.