High-Precision Motion Compensation for LiDAR Based on LiDAR Odometry

Abstract

LiDAR plays a pivotal role in the field of unmanned driving, but in actual use, it is often accompanied by errors caused by point cloud distortion, which affects the accuracy of various downstream tasks. In this paper, we first describe the feature of point cloud and propose a new feature point selection method Soft-NMS-Select; this method can obtain uniform feature point distribution and effectively improve the result of subsequent point cloud registration. Then, the point cloud registration is completed through the screened feature points, and the odometry information is obtained. For the motion distortion generated in a sweep, the prior information of the LiDAR’s own motion is obtained by using two linear interpolations, thereby improving the effect of motion compensation. Finally, for the distortion caused by the motion of objects in the scene, Euclidean clustering is used to obtain the position and normal vector of the center point of the point cloud cluster, and the motion pose of the object is calculated according to the offset between adjacent sweeps and eliminated distortion. Essentially, our method is a general point cloud compensation method that is applicable to all uses of LiDAR. This paper inserts this method into three SLAM algorithms to illustrate the effectiveness of the method proposed in this paper. The experimental results show that this method can significantly reduce the APE of the original SLAM algorithm and improve the mapping result.

1. Introduction

As the most important sensor in the robot perception system, LiDAR plays an indispensable role in object detection, localization, and mapping in the field of unmanned driving. According to its working principle, LiDAR can be divided into conventional LiDAR, solid-state LiDAR, and hybrid solid-state LiDAR. Among them, conventional LiDAR, with the longest development time, is also the most widely used.

However, the conventional LiDAR is limited by the working principle of its rotary scanning, so the point cloud compensation problem in one sweep must be considered. For example, Figure 1 shows the position relation of a vehicle carrying LiDAR at three moments. Assuming that these three moments are all within a LiDAR scanning cycle, for obstacle , there are multiple observation positions in a sweep, which makes it difficult to accurately describe the location information of the obstacle.

Figure 1 

Distortion caused by LiDAR’s own motion. In the figure, the coordinate system on the right is the initial coordinate system of a sweep. The vehicles in the figure have multiple positions in the current sweep period, so there are multiple observation coordinate systems, resulting in the description of object under measurement in multiple observation coordinate systems, thus affecting the measurement result.

Furthermore, if objects in the scene are moved during scanning, the point cloud of the moving object will be distorted, as shown in Figure 2. At this point, while the LiDAR is moving, the object to be measured is also moving, which causes the measurement deviation to increase. Most LiDAR downstream tasks choose to ignore the impact of this problem. For example, in the well-known LOAM [1] framework, only the distortion caused by LiDAR movement is calibrated. However, we find that the negative impact of the distortion caused by the movement of objects in the scene on various applications of LiDAR cannot be ignored.

Figure 2 

Distortion caused by the motion of the object to be measured.

Based on the above problems, we propose a LiDAR motion compensation method. Through a new feature point selection method of point cloud, the planar entropy and linear entropy of the points are firstly calculated, and then, Soft-NMS-Select is used to obtain the feature description of the current sweep. Then, the prior information of LiDAR’s own motion was obtained by linear interpolation twice to improve the orthodontic effect. At the same time, the coordinates and normal vectors of the center point of the point cloud cluster are calculated by Euclidean clustering, and then, the movement posture of objects in the scene is estimated, and the point cloud of moving objects in the scene is also compensated, so that the scene information can be described more accurately.

In addition, we selected two laser SLAM algorithms, A-LOAM and LEGO-LOAM, as the benchmark, and compared absolute pose error (APE) to quantitatively analyze and demonstrate the effectiveness of the proposed method. The structure of the rest of this paper is as follows: in Section 2, we introduce the methods commonly used to calculate motion posture and the general motion compensation methods of laser SLAM. In Section 3, the proposed motion compensation algorithm is introduced in detail. In Section 4, we provide the experimental effect under the actual scene. The limitations and deficiencies of this method are discussed in Section 5. Finally, we summarize this article.

2. Related Works

When solving motion pose, it is often necessary to match the point cloud between adjacent frames. As one of the mainstream registration algorithms, the ICP algorithm [2, 3] iteratively optimizes the conversion between source point cloud and target point cloud by minimizing the error measure between the two point clouds. Based on the matching of the ICP algorithm, other scholars further proposed point-to-line ICP [4], point-to-point ICP [5], and generalized ICP [6], making great contributions to the accuracy of point cloud registration. As the accuracy of point cloud registration is improved, the ICP algorithm is used in LiDAR odometer to reduce the accumulated error of continuous registration by combining the closed-loop mechanism and pose construction process [7]. The ICP algorithm is improved by means of downsampling and point cloud matching rejection, and the location drift of LiDAR odometer is reduced obviously. Although the ICP algorithm can be very accurate in point cloud conversion, it has a slightly insufficient requirement on real-time performance in automatic driving. Therefore, in order to improve the efficiency of the ICP algorithm, some articles use parallel computing for operation [8–11]. When ICP is used for matching scanning, if the scanning motion is relatively slow, serious motion distortion will occur. Therefore, researchers use laser intensity return to create visual images and match visually different features between images [12] to restore the motion of ground vehicles [13–16]. However, these methods require dense point cloud images. In [17–19], sparse point cloud images can be used for motion recovery, and point clouds can be used to match the geometric structure of local point clusters [19].

After completing interframe matching, LiDAR motion compensation is required. Mainstream laser SLAM algorithms [20–22] unify the point cloud within a sweep to a timestamp, and this unified time point is often the start time of the scan. Then, using the result of the last interframe laser odometer as the motion between the current two frames and assuming that the current frame is also moving at a uniform speed, we can also estimate the position and pose of each point relative to the starting time. That is, the motion of to frame and to frame is one to one, and the pose transformation of to frame is used as the pose transformation of to frame, and the pose transformation of each point of to frame can be calculated. However, this method does not accurately describe the motion posture of the current frame and lacks motion compensation for the objects to be measured in the scene. Therefore, based on the shortcomings of the above method, this paper improves the LiDAR motion compensation method and uses twice linear interpolation to obtain more accurate motion posture of the current frame. Using the description of the center point of the object category and the normal vector to solve its motion posture, complete the distortion removal of the moving object.

3. Methodology

3.1. Selection of Point Cloud Feature Points

The LiDAR used in this paper is Velodyne 16, and the laser points under a sweep reach more than 30,000. If all points are used for point cloud registration, it will cause great computing overhead. Therefore, we use the idea of LOAM for reference and select key points in the scene to participate in the subsequent registration work. In addition, before the feature description of the point cloud in a sweep, this paper did not adopt any downsampling filtering method as the pretreatment, because we found that the filtering method would reduce the compensation effect of subsequent moving objects.

Before calculating the feature description of point cloud, PCA should be performed on the processed point, and the eigenvalues of its covariance matrix should be calculated, which are arranged in descending order as follows: , , and . Its planarity is described as follows:

It can be seen from the above formula that the flatness depends on the size of the third principal component in the overall composition. Approximately, as the value of the third principal component changes from large to small, the value of the flatness characteristic increases from small to large. This means that in point cloud space, part of the point cloud space with a larger value is closer to the plane, and part of the point cloud space with a smaller R value has a steeper change. Similarly, the linearity of point cloud can be described as

This indicates that part point clouds with a larger value in point cloud space are more concentrated in one direction, and part point clouds with a smaller value are more divergent in a certain direction. Figure 3 shows the key points selected by and as feature description, respectively.

Figure 3 

Key point effect diagram. Among them, the red point represents the screening point of plane entropy value, and the yellow point represents the screening point of linear entropy value.

In addition, this paper finds that the introduction of entropy of local feature description can improve the consistency of the selection of key points of adjacent frames, because entropy represents the uncertainty of information, so selecting a larger entropy value can screen out more significant neighborhood features, which is specifically expressed as

According to the above analysis, points with significant features are characterized by low flatness or linearity and high entropy. Therefore, by introducing flatness and linearity into the above equation, the flatness entropy and linearity entropy can be obtained as follows:

Therefore, the feature point selection problem can be described as where is the key point collection. In order to ensure the real-time performance of the algorithm, a total of 10 points are selected for a sweep, and 5 points are selected by plane entropy value and linear entropy value, respectively. In addition, in order to ensure the uniform distribution of key points, the idea adopted in LOAM is to divide the scanning region of LiDAR and select several feature points in each subregion for subsequent registration. However, in some special scenarios, the distribution of key points is more concentrated.

See Figure 4. Key points in area 2 and area 3 do not cover a large number of scene features, so the accuracy of subsequent registration will be reduced. In this paper, a Soft-NMS-Select method is proposed to solve the above problems. Firstly, a neighborhood radius is determined, and the intersection volume between each sphere centered on key points is calculated. Considering that, using NMS directly will cause the algorithm to suppress most significant feature points according to the sphere radius in some extreme scenarios. Meanwhile, low feature points are selected.

Figure 4 

Schematic diagram of scanning area division effect.

Therefore, this paper uses a soft method to suppress nonmaxima to achieve uniform selection. The pseudo-code of Soft-NMS-Select is shown in Algorithm 1.

Input: key points set (C), radius , volume threshold Output: new key points set ()={ }   Create {}   C = C.sorted(max min)   whileC.size > 0 or =10 do   .append()   ifdo   .append()   C.remove()   elsedo   ifdo   .append()   C.remove()   else do   .append() C.remove()   end if   end if   end while   return key points set ()

Algorithm 1 

Soft-NMS-Select.

Using different radii , 5 to 10 to 15, 20 key point distribution is shown in Figure 5. It can be seen from the figure that a relatively uniform distribution of key points can be obtained by using for urban road scenarios. In addition, it can be seen from the figure that key points are mainly distributed at locations with drastic changes in the scene, such as corners and bulges. Because these locations often cover the uniqueness of the current scene, the key points calculated by using the above feature description can well represent the information of the current sweep and meet the requirements of subsequent point cloud registration.

(a)

(b)

(c)

(d)

Figure 5 

Filtering effects of key points in scenarios with different thresholds. (a–d) The key point filtering results when is 5, 10, 15, and 20. It can be seen from the figure that when is 5, the distribution of key points in the scene is still redundant; when , the value of key points is too sparse, which is not conducive to subsequent feature point matching.

3.2. Dedistortion Based on LiDAR Odometer

After obtaining the key points of the point cloud, odometer information needs to be calculated according to the registration result, so as to obtain the LiDAR movement posture at each time. The key points at moment and moment are, respectively, denoted as and . The motion posture of point cloud is denoted as where is the rotation matrix and is the translation matrix. At this point, the matching key point problem is essentially a point-to-point ICP problem. The point-pair matching is shown in Figure 6.

Figure 6 

Matching diagram of cloud key points of adjacent frame points.

It can be described as follows:

Then, singular matrix is constructed by two decentralized point clouds, respectively, and SVD is performed on to obtain the current optimal translation matrix :

The optimal solution can be obtained through continuous iteration .

Generally, the use of laser speedometer for LiDAR is an important premise to the distortion of the uniform model assumption, the assumption of LiDAR between adjacent sweep movements is uniform, and with a moment of motion being equal, using represented the moment point cloud motion matrix; the movement of the middle time gesture by linear interpolation can be represented as

The above formula can be used to calibrate the point cloud at the intermediate time to the coordinate system at with a uniform velocity model, so as to realize LiDAR motion compensation. In this paper, it is found through experiments that higher compensation accuracy can be obtained by linear interpolation of rotation matrix and shift matrix , denoted as . For the translation matrix , since the first-order difference can approximately represent the change trend of the function at the current point, the translation change at moments can be used to approximately deduce the translation matrix at , and then, linear interpolation can be used to obtain the translation change at intermediate moments to obtain higher accuracy:

As for the rotating attitude, classical Euler angle is used in this paper, so the Euler angle-rotation matrix transformation needs to be carried out using Rodrigues’ formula and represents the Euler angle of LiDAR motion at the moment :

Let represent the point cloud at time , so the motion posture after two linear interpolation can be described as follows:

The point cloud effect after two interpolation is shown in Figure 7. Compared with single linear interpolation, the calibration effect of LiDAR carrier under acceleration or deceleration is improved because first-order difference is used to calculate the rate of change.

(a) The original point cloud

(b) Postcompensation point cloud

Figure 7 

Compensation rendering of LiDAR point cloud.

3.3. Moving Object Compensation Based on Euclidean Clustering

Another innovation of this method is to calculate the movement posture of objects scanned by LiDAR, so as to realize rough compensation. Therefore, after the key points set in the scene are extracted, the point cloud in the current sweep needs to be clustered. Due to the randomness of the scene, it is impossible to predict the number of categories in advance, so this paper selects European clustering to process the point cloud. Euclidean clustering is a clustering algorithm based on Euclidean distance; that is, the Euclidean distance between points is calculated to determine whether some point clouds belong to the same class, and the distance threshold is set to . For the point clouds after clustering, the mean values of point clouds within the class are calculated, respectively, to obtain the location of the center point of the cluster. The pseudo-code of the Euclidean clusters and extraction center is shown in Algorithm 2.

Input: Point cloud(C)={ , ... } and distance threshold Output: Point set(P)={... } and center point =(, ), which = , = , =    create P {}    create K {}    forc in Cand cnot in (P) do    find k nearest neighbour for k in K do ifandcnot in (P)then       P.append(k) end if    calculate M= return set(P), center M

Algorithm 2 

Euclidean clusters and extraction center.

The effect of realizing Euclidean clustering of objects in the scene by using the above methods is shown in Figure 8.

Figure 8 

European clustering effect diagram. Red dots represent clustering points.

The coordinate transformation of the central point set to its adjacent sweep inner central point set can be described as follows:

The actual center set is . For the matching center point pair , is the threshold of minimum matching distance. If , it is approximately considered that no absolute motion has occurred in the corresponding category. Therefore, no motion state estimation is performed on the point cloud set corresponding to the center point. If , then we calculate the corresponding central point translation matrix , and the scanning time of the point convergence corresponding to the central point can be obtained from the LiDAR timestamp. Assuming that the starting time of the scanning of a point convergence is and the scanning time of a point in the middle is , then the position calibration of the point is the same as the above LiDAR orthodontic principle. For the whole point convergence, it is also necessary to consider its orientation angle , so it is necessary to calculate the normal vector of the central point neighborhood and find the rotation matrix that makes the normal vector direction consistent. The overall principle is shown in Figure 9.

(a) Translational transform

(b) Rotation transformation

Figure 9 

Principle diagram of point cloud dedistortion of the moving object. The vector is the normal vector of the object. (a) shows that the coordinate offset of the center point is used to estimate the translation transformation of the object, and (b) estimates the rotation transformation of the object by the offset of the normal vector before and after the transformation.

The effect of compensating the moving objects in the scene by using the above methods is shown in Figure 10.

(a) The original point cloud

(b) Point cloud after compensation

Figure 10 

Compensation effect of the moving object. It can be seen that the distribution of point cloud of vehicles in (a) has obvious distortion, while that in (b) has obvious change.

Since this method needs to solve the motion posture of each point cloud cluster after clustering in the scene, resulting in too much computing overhead, it is necessary to limit the point cloud cluster for different scenes. Since an unmanned driving scene is taken as the benchmark in this paper, the point cloud cluster is set as

4. Result

In this part, we first use three SLAM algorithms, A-LOAM, LEGO-LOAM, and T-LOAM, as the benchmark for experimental comparison. Then, we use the point cloud dedistortion method proposed in this paper, respectively, for the original versions of the above three algorithms and obtain their trajectories to quantitatively analyze the performance differences between the proposed method and the original method. The experimental site was selected in the campus environment, the PIX unmanned vehicle carrying VLP-16 and RTK was used to obtain external information, and the algorithm was implemented in Ubuntu 18.04 (ROS) under AMD 3950X. All the algorithms are implemented in C++.

In the campus scene experiment, we avoided the driverless car driving through the sparse environment and ensured the presence of a certain number of moving objects (such as pedestrians and bicycles) in the scene. In other words, stable feature points can be obtained for interframe matching, while some moving objects exist, and then, distorted point clouds are generated.

We show the two benchmark algorithms and the absolute pose error (APE) after the improved point cloud compensation stage, respectively, as shown in Figure 11.

(a) Error comparison

(b) Error estimate

(c) Error estimate

(d) Trajectory error comparison

Figure 11 

Comparison diagram of quantitative analysis of campus environmental experiment.

Because the reference algorithm lacks the correction of the distortion of moving objects in the scene, it can be found that the orthodontic algorithm proposed in this paper can effectively lower the APE of the reference algorithm and improve its positioning accuracy in any direction by comparing the figures. Meanwhile, we also compared the motion trajectory of the two benchmark algorithms and the improved algorithm reference RTK, as shown in Figure 12. It can be seen intuitively that the two benchmark algorithms can improve the coincidence degree with the real trajectory after using the orthodontic method proposed in this paper.

(a) A-loam and real trajectory comparison

(b) A-loam_new and real trajectory comparison

(c) LEGO-loam and real trajectory comparison

(d) LEGO-loam_new and real trajectory comparison

Figure 12 

Comparison diagram of the trajectory effect.

In addition, the method proposed in this paper can further improve the mapping effect. We used the improved LEGO-LOAM 3D map to overlay with the satellite map, as shown in Figure 13. It can be seen that after the improved motion compensation algorithm, more accurate 3D scene information can be obtained, and the constructed map and the actual map can be accurately matched.

(a) Satellite imagery

(b) 3D map

(c) Overlay diagram

Figure 13 

LEGO-LOAM_new map aligned with Google Earth.

Table 1 shows the ablation experiments of the proposed method. Since the motion compensation of LiDAR is loosely coupled with point cloud orthodontics of moving objects, we compare the performance of one module alone with that of the whole algorithm to illustrate the effectiveness of each module of the algorithm.

Table 1 

Comparison of ablation results.

Table 1 shows the overall performance improvement effect of the two modules on the algorithm. Due to the large number of moving objects in the scene, the interframe matching is not accurate, so the performance of the original version of A-LOAM is poor. By using the point cloud compensation method proposed in this paper, the attitude of LiDAR itself can be estimated more reliably, and the errors caused by point cloud distortion of moving objects can be calibrated to a certain extent, which makes the APE smaller, so as to obtain more accurate positioning effect.

5. Limitations

In this experiment, the feature points of the scene are used as the basis for interframe matching, and the movement posture is obtained by calculating the movement of the Euclidean clustering center point and the deviation of the normal vector, and then, the point cloud of the moving object is dedistorted. In practice, in a sparse scene, most of the feature points selected by the Soft-NMS-Select algorithm may be distributed in the point cloud cluster of moving objects. In this case, the experimental effect of this algorithm is worse than the benchmark, because point cloud matching highly depends on the selection of feature points within the scene. If most of the feature points are in motion, the odometer will be extremely inaccurate. In addition, due to the extra computational overhead brought by Euclidean clustering, the algorithm takes more time in most scenarios. However, not all objects involved in Euclidian clustering in the scene are in motion, so the algorithm has some invalid operations.

6. Conclusion

In this paper, we propose a LiDAR motion compensation method. We calculate the flatness and linearity of point cloud, introduce entropy value to screen the feature points with greater degree of information, and then optimize the odometer information of the point cloud by obtaining the prior information of its own motion through quadratic interpolation. Then, Euclidean clustering is performed for the current scanning scene, and cluster center points and normal vectors are calculated to represent moving objects. By eliminating distortion of the point cloud of moving objects, higher quality scanning results can be obtained, and positioning and mapping results can be further improved. We conducted localization and mapping experiments using VLP-16 in a real campus scene and evaluated our method with two benchmark LiDAR SLAM algorithms. The results show that the proposed method can effectively reduce the APE and improve the accuracy of pose estimation.

Data Availability

This article does not cover data research. No data were used to support this study.

Conflicts of Interest

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

Acknowledgments

The authors would like to thank the National Natural Science Foundation of China (51974229), the Shaanxi Province Science and Technology Department (2021TD-27), and the National Key Research and Development Program, China (2018YFB1703402), for their support in this research.