This article is reproduced by the authorization of the 3D Vision Heart public number, please contact the source.

Solve vertical drift

Positioning is crucial in the context of autonomous driving. It is a fundamental module for safe and efficient navigation, enabling the vehicle to accurately determine its position in the environment. For the navigation of uneven terrain, the vehicle is bound to fluctuate when crossing rough terrain. Rigidly mounted sensors on ground vehicles are particularly susceptible to these movements, making off-road positioning an extremely challenging task.

SLAM technology can simultaneously locate the sensor's position and pose and provide a map of the environment. This approach provides an effective solution for positioning in an unknown and uneven environment.

The emphasis of this paper is to use SLAM method based on LiDAR to navigate uneven terrain. LiDAR methods typically use continuous laser scanning for local and global registration of the point cloud to estimate the precise self-motion of the sensor. These methods exhibit the advantages of insensitivity to environmental conditions, distance perception and low measurement noise in uneven outdoor scenes.

Off-the-shelf LiDAR-based SLAM typically consists of two modules: a front-end LiDAR odometer and a back-end mapping optimization. The front end provides initial pose estimation through interframe registration, while the back end fine-adjusts pose estimation and reconstructs the surrounding environment through global alignment and optimization methods. This approach enables the frame to be positioned from coarse to fine, suitable for common scenarios such as flat roads in cities. However, in uneven terrain, the LiDAR-based SLAM method applied to ground vehicles can experience non-negligible positioning drift, resulting in distorted or slanted maps. The main reason is that the pose component of the vertical direction changes significantly as the vehicle moves on the terrain surface. These changes directly lead to the reduction of the consensus set when interframe matching, making the front-end unable to provide correct initial pose estimation. Although numerous studies have made significant improvements in this area, the topic remains challenging and requires further optimized solutions.

Projects link: https://github.com/sdwyc/ROLO

To solve this problem, ROLO-SLAM [1] (Rotation-optimized LiDAR Special SLAM) is proposed: a LIDAR special SLAM framework designed to reduce vertical pose drift and accurately estimate the pose of ground vehicles. We divided the front end into three independent modules based on observations of pose estimation drift in rough terrain. At the front end, a forward position prediction is developed for rough translation estimation to achieve decoupling of rotation from translation. The precise rotation between two successive scans is then independently estimated by voxelization matching and rotation registration. The translation estimation method based on continuous time further optimizes the translation accuracy of the scan. Finally, the method is integrated into an efficient SLAM framework, combined with the alignment of the scan to the submap and the global factor map as the backend.

Major contributions:

• A forward position prediction method is proposed, which realizes the flexible decoupling between rotation estimation and translation estimation, so that rotation and translation can be estimated independently.
• A two-stage rotation and shift estimation paradigm is proposed at the front end, which uses spherical alignment and continuous time optimization to provide the exact initial pose required for back-end optimization.
• A compact LiDAR SLAM framework is built that integrates scanning-to-submap alignment and global factor map optimization to support the positioning of ground vehicles in uneven terrain.

Problem definition

Most LIDAR-based SLAM methods will produce significant vertical pose drift when the ground vehicle passes through uneven terrain. The main reasons for this problem include the following two aspects:

1. Uneven terrain surface: The vehicle's attitude, especially the roll Angle and pitch Angle, changes with the ups and downs of the terrain surface. In this case, the LiDAR sensor generates a large angular displacement in the vertical direction. However, due to the limited resolution of LiDAR in the vertical direction, these pose errors accumulate gradually.

The objective of this study is to improve the vehicle positioning accuracy in uneven terrain by using a single LiDAR sensor. To do this, we refactored the entire front end and divided it into modules to optimize pose estimation. We then introduced alignment and factor maps that scan into submaps to optimize the vehicle's pose within the map.

Concrete method

ROLO-SLAM System flow

The architecture of ROLO-SLAM is shown in Figure 3. The framework consists of two main components: a front-end LiDAR odometer module and a back-end mapping module.

First, the LiDAR scan data is corrected using the odometer data at the back end to eliminate motion distortion. At the front end, edge and plane features are extracted based on the smoothness index. Subsequently, a forward position prediction was developed for rapid initial estimates of LiDAR translations, enabling loose decoupling of rotation and migration. On this basis, the corresponding relationship of point cloud is determined by the developed voxelization method. Rotation and displacement are estimated independently, where rotation is registered by spherical alignment model and translation is obtained by continuous time optimization.

In addition, the backend builds submaps through aggregation of keyframes and optimizes them through alignment of scanned to submaps. Finally, the factor map is used to optimize the global pose and point cloud map of LiDAR.

Forward position prediction

At the front end, we achieve decoupling of rotation and migration estimates between successive scans through forward position prediction. This is achieved by eliminating translation differences between scans.

Traditional registration methods often combine rotation and displacement estimates, obscuring their independent challenges, which can lead to potential inaccuracies in vehicle attitude and position estimates. By introducing forward position prediction, we make a rough estimate of the translation between successive LiDAR scans, thus decoupling the rotation and migration estimates at the front end. This establishes a consistent benchmark for scanning positions and is expected to improve the accuracy of vehicle rotation estimates.

Voxelization matching and rotation registration

The above process is described in detail in line 8-12 of Algorithm 1.

Matching based on voxelization

Rotary registration

Through the matching process, we get the correspondence between the source point and the target voxel C. Next, the rotation between scans is calculated by aligning the points in the source point cloud with the Gaussian distribution mean Pk in the target voxel. The rotational registration model is shown in Figure 5.

Finally, the optimization problem can be solved iteratively by Gauss-Newton (GN) or Levenberg-Marquardt (LM) algorithms.

Translation optimization based on continuous time

After the forward position prediction, the translational component tk of the vehicle has been preliminarily estimated. However, due to the effect of rough terrain on pose, direct use of the initial predicted translation may lead to the accumulation of estimation errors. Therefore, we propose a continuous time-based translation optimization method to further improve the accuracy of pose estimation.

Translation model

Continuous time optimization

Back-end mapping and loopback detection

In order to reduce the cumulative error and further optimize pose estimation, we designed a mapping and loop detection module in the back end of ROLO-SLAM. The module combines local submap construction and global factor map optimization to ensure that the positioning of the vehicle remains accurate and consistent over the long run.

Local submap construction

On the back end, we build local submaps based on keyframe aggregation. Keyframes are selected LiDAR scan frames that adequately represent the motion state of the vehicle in the environment. When generating local submaps, the overlap rate between key frames needs to reach a preset threshold to ensure that each submap can contain sufficient environmental features.

The construction of local submaps can significantly reduce the computational amount of point cloud processing, and provide a reliable description of local environment for global alignment and loop detection.

Loop detection and factor graph optimization

Loop detection

The purpose of loopback detection is to identify that a vehicle has at some point revisited a previously passed location. This is done by matching the current scan to the global map. When a loop is detected, we generate a loop constraint to reduce the cumulative error.

The steps of loopback detection include:

1. Extract candidate key frames from the global map;
2. Coarse matching through fast point cloud descriptors, such as FPFH or ISS features;
3. The accurate point-to-point ICP algorithm is used to calculate the relative pose between the current scan and the historical key frame.

Factor graph optimization is usually solved by nonlinear least squares methods, such as Ceres or GTSAM, to correct the global pose of all keyframes simultaneously.

Global map generation

The global map not only accurately describes the environment, but also can be used for navigation, path planning, and subsequent location tasks.

Experimental effect

Sum up

Roro-slam is a novel SLAM framework for LiDAR, which is optimized for the pose and posture drift of ground vehicles in complex terrain. Through front-end and back-end collaborative design, our framework can effectively reduce vertical drift and improve positioning and mapping accuracy. In experiments, ROLO-SLAM demonstrated excellent performance in a variety of scenarios, especially accuracy and robustness in rugged terrain environments. In the future, we plan to introduce multi-sensor fusion into the system to further improve robustness and real-time in extreme scenarios. In addition, the development of more efficient algorithms to further optimize computing performance is also a future research direction.