Localization plays a vital role in unmanned platforms. The feature-based LiDAR odometry has excellent real-time performance but sufers accumulated error. The localization based on global matching can provide an unbiased estimation of states, whereas it requires an amount of calculation. This paper proposed a joint framework that fuses the LiDAR, IMU, and pre-build prior map to estimate the unmanned platform's six-degrees-of-freedom (DOF) states to solve the accumulated error problem under the premise of ensuring real-time. This work divided the system into two main threads. The one is utilizing the high-frequency feature-based LiDAR odometry coupled with IMU to localize. And the other is using the low-frequency global matching to correct the odometry. We link the two threads through sub-map optimization built atop the factor graph. As the global matching can provide unbiased estimation, and feature-based LiDAR odometry is with little calculation, the system can achieve both accuracy and real-time performance. The proposed framework is extensively validated with experiments under two lfatforms and achieves excellent performance.
Localization is essential for most unmanned platforms such as automatic vehicles, moving robots,
and unmanned air vehicles. In tradition, the localization usually relies on the collaborative
cooperation of GPS and INS[
There are usually two ways to implement odometry. One is SLAM, and the other is localization in a pre-built prior map through global matching.
LiDAR SLAM plays a pivotal role in unmanned platforms, which have witnessed rapid
progress in the past few years[
The global match is to match the point cloud of the current LiDAR frame against the prior map
to estimate the current pose. The prior map is built previously by fusing the LiDAR odometry,
GPS, and loop closure. Although the GPS signal is not fully covered, the system can establish
a high precision prior map thanks to the globally consistent optimization. Usually, the global
matching can be expressed as a nonlinear optimization problem, and it has specific requirements
for a large number of calculations. Therefore it decreases the real-time performance and the
frequency of localization. In addition, an excellent initial pose is also requested for global
matching. However, Most of the existing methods utilize the linear model to estimate the initial
pose, which will result in low precision and low robustness. The environmental change due to
the dynamic object or seasonal variation will also afect the performance of global matching[
Aiming at the above problems, we propose a joint framework that fuses the LiDAR odometry with global matching to estimate the six-degrees-of-freedom (DOF) state for unmanned platforms. The LiDAR odometry obtained from feature-based scan matching is fast but with cumulative error. The global localization obtained from global matching is slow, but it can ofer unbiased estimation with high precision. So the proposed framework utilizes the high-frequency LiDAR odometry coupled with IMU to localize and the low-frequency global matching to correct it so that the system can achieve both accuracy and real-time performance. We link the LiDAR odometry and the global matching through sub-map optimization atop the factor graph. The LiDAR odometry can also provide a tremendous initial pose for global matching. In addition, the LiDAR odometry matching against the current sub-map can efectively decrease the error result from the global matching in the changing environment. Because the prior map needs to be established in advance, my project is most suitable for specific scenes such as underground parking, scenic spots, and factory areas. Thanks to the global localization from the prior map, the system can achieve high-precision positioning without relying on GPS information so that the system can be applied to indoor localization tasks.
The main contributions of our work can be summarized as follows: • We proposed a joint framework that utilizes the high-frequency LiDAR odometry coupled with IMU to localize and the low-frequency global matching to correct it. And, We link the LiDAR odometry and the global matching through sub-map optimization atop the factor graph. In this way, the system can achieve both accuracy and real-time performance. • The proposed framework is extensively validated with experiments in two environments and achieves excellent performance in precision and robustness.
Scan matching is the most important part of lidar odometry, and many algorithms deal with it;
Iterative closest point (ICP)[
Recently, LiDAR odometry is typically coupled with IMU for de-skewing the point cloud
and joint optimization[
Global matching is essential for localization. In [
The mathematical symbols used in this paper are defined in Table 1. We define the skew symmetric matrix of a vector ∈ R3 using (· )∧:
⎡ 0 = ⎣ 3 − 2 − 3 0 1
Similarly, we can map a skew symmetric matrix to a vector in R3 using the operator (· )∨. We use ∈ (3), ∈ R3, and = [|] ∈ (3) to represent the rotation matrix, the position vector, and the transformation matrix, respectively. It is worth mentioning that the transformation matrix from the reference coordinate to the object coordinate can also describe the object’s pose in the reference coordinate. We denote the world frame by W, and it is the same as East, North, and Up (ENU) coordinates. We denote the platform body frame by B that is the same as the IMU frame. We also define the GPS frame by G and the LiDAR frame by L. Assume that the parameters in frame A can be wrote as (· ) and the transformation from frame B to frame A can be denoted as (· ). The state of the platform can be defined as: = [⊤, ⊤, ⊤, ⊤]⊤ where ∈ R3 denotes the speed, = [, ] denotes the IMU bias, ∈ R3 is the acceleration bias, and ∈ R3 is the gyroscope bias.
This paper proposed a framework that fuses the LiDAR odometry, the IMU preintegration, and the global localization to estimate the six degrees-of-freedom(DOF) pose. The architecture of the proposed framework is shown in Figure.1, and it mainly consists of four modules: Scan Matching, IMU preintegration, Global Matching, and Sub-map Optimization. We marked them blue in the figure. (1) (2) IMU
LiDAR Offline Mapping
Prior Map
IMU Preintegration
Initial Guess Scan Matching
LiDAR Odometry Initial Guess
Is Keyframe? Keyframe Is Covariance
large? Global Matching
IMU Preintegration
Factor LiDAR Odometry
Factor Reigster to Sub-map Global Localization
Factor
S u b m a p O p tii m z a ti o n
Update
IMU bias Sub-map
The Scan Matching module provides the LiDAR odometry by matching the current LiDAR
scan to the sub-map. In our system, we follow the feature-based scan matching method proposed
in [
We follow the scan matching method proposed in [
When the LiDAR frame F is received, we first calculate the roughness value of each point by comparing it to the consecutive neighbor points in the same scan. Then the edge and planar features will be extracted according to the roughness value. The points with large values are classified as the edge features and denoted as and the points with small roughness are classified as the planar feature and denoted as . The features of the LiDAR frame F are in frame L. Then the extracted features {, } are matched to the features from sub-map as − 1 = {︀ − 1, − 1︀} . − 1 gathers the features of the previous keyframes, and the features are transformed to W by LiDAR pose: − 1 = ¯ − 1 ∪ − 2 ∪ ... ∪ − − 1
¯ ¯ − 1 = ¯ − 1 ∪ − 2 ∪ ... ∪ − − 1
¯ ¯
where ¯ and ¯ are the transformed edge and planar features in W. The distance of features
between the current scan and sub-map are {︁(˜ ,), (˜ , )}︁, , ∈ , , ∈ ,
˜ is the transformation of keyframe F to be estimated, and the detail definition of distance
can be seen in [
∑︁ ,∈
(˜ , ) ⎫ ⎪ ⎬ ⎪ ⎭ (3) (4) equation:
The initial guess of ˜ can be obtain from the IMU preintegration in Section 5. Through manifold optimization[20] and GaussNewton method, we can solve it to obtain the transformation ^
. Given the extrinsic parameters between L and B as , then we can calculate the pose of platform ^ and the relative transformation Δ^ − 1, between the F− 1 and F through the = ^ ^
( )⊤ Δ^ − 1, = (^ − 1)⊤^ can be written as:
The Δ^ − 1, can be seen as the measurement from LiDAR, assumed that it’s noise is , it Δ^ − 1, = Δ− 1,( ∧− 1,) The residual errors of scan matching can be defined as: = [ − 1,] = ((Δ− 1,)⊤Δ^ − 1,) ∨
To avoid repeatedly integrating caused by the changes in initial conditions when the lidar
odometry pose has been optimized, we adopt preintegration of the inertial measurements
introduced by [
Firstly, taking the accelerator bias , gyroscope bias and the additive noise into consider, the IMU measurements formula can be written as follow: ^ = ⊤( − ) + + ^ = + + .
Where deonte one sampling time. Where the denotes the gravity vector in W. The and denotes the additive noise in acceleration and gyroscope measurements, them can be seen as the Gaussian white noise as ∼ (0, 2), ∼ (0, 2). Combining the integration of the IMU measurements with the motion formula of platform, and we discretize and iterate it for intervals between sampling times and , we have: − 1 = − 1 ︂[ = − 1 = 1 = ∏︁ ((^ − −
)∧Δ) = + Δ + ∑︁ (^ − −
)Δ = + ∑︁ Δ + Δ2 + 1 2 (^ − − )Δ2 .
︂] (5) (6) (7) (8) (9)
Where the Δ is the interval between two consecutive IMU measurements. We can defined the preintegrated items as: measurements Δ^ , Δ^ , Δ^ can be obtained as:
Assuming that the bias remains constant between sampling times and , the preintegrated Δ =⊤ . .
. Δ =⊤( − − Δ ) Δ =⊤( − − △ − 2 1
Δ2 ) Δ^ =. ∏− ︁1 ((^ − )∧Δ) = Δ (( )∧) = = = Δ^ =. ∑− ︁1 Δ^(^ − Δ^ =. ∑− ︁1 ︂[ = Δ + .
)Δ = Δ + 1 2 Δ^Δ + Δ^ · (^ − )Δ2 ︂] (10) (11) (12)
The global matching procedure mainly provides the no drift pose estimation for sup-map modules through matching online lidar scan against the prior map, which is built ofline.
Ofline mapping plays an essential role in our system because it provides the absolute
correction data to eliminate the drift. As a result, the quality of the prior map will directly influence
the precision of the localization in online procedures. The ofline mapping procedure follows
the SLAM method proposed in [
We utilize the NDT (Normal-Distributions Transform) method proposed by Martin
Magnusson[
Firstly, we grid the prior map into a mass of 3x3 small cells. For each cell, it is assumed that there are points =1,· , ∈ R3 in it and the points conform to the normal distribution relationship. Therefore, the normal distribution parameter can be calculated based on the points in the cell, and we can easily get the probability density function (PDF) of it: = 1 ∑︁ =1
∑︁ = 1 ∑︁ ( − )( − )
=1
Where the and ∑︀ denote the mean and the covariance of the normal distribution, respectively. The probability density function of the cell can be written as: () =
1 (2 ) 32 √︀|∑︀| − (− ) ∑︀− 1(− ) 2
When a lidar scan F = {=1,· ,} is about to match against the prior map, the goal is to ifnd out its translation of lidar to maximize the likelihood that all of the lidar scan points will be on the cell of reference points cloud map. Therefore, combining the probability density function of the cell in (14), the likelihood function can be easily written as: (13) (14) (15) (16) (17)
In practical application, the NDT method is already integrated into the PCL library[21] and can be used conveniently.
According to the Section 4, the sub-map can be defined as the collection of the features of previous keyframes, and the features are transformed to W by LiDAR pose. Therefore, we only ℎ : = ∏︁ ( )
=1
It’s worth noting that the is the probability density function of the cell which the th point located. This’s a nonlinear optimization problem, and we can use the Newton optimization to solve it and obtain the optimized translation of LiDAR. The initial guess can be obtained from the LiDAR odometry in Section 4. Through the transformation of the coordinate system = ( )⊤, we can obtain the pose estimation of platform as ^ . Assumed that the noise of is , we have:
The residual errors of scan matching can be defined as:
^ = (( )∧) = [ ] = (()⊤^ )∨ ...
xnW 1 need to optimize the states of all the keyframes in the sub-map. The state need to be optimized can be defined as:
= {, +1, · · · , } we utilize the factor graph to model this problem. Fig. 2 provides a brief illustration of optimization procedure. The keyframe state can be seen as the node, and we fusing the LiDAR odometry factor, IMU preintegration factor and the global matching factor for optimizing. The residual error function can be writeen as: ∈ + ∑︁ ‖( )‖2}.
∈
∈ min { ∑︁ ⃦⃦ (, )⃦⃦ 2
+ ∑︁ ⃦⃦ (, )⃦⃦ 2
The , and are residuals for LiDAR odometry, IMU preintegration and global matching, which are defined in Section 4, Section 5 and Section 6 respectively.
In addition, to decrease the calculation, we utilize an unfixed sliding window to register the keyframes for the sub-map, and we also use a simple but eficient way for marginalization. We register the keyframe into the window when it is received. If the covariance of the latest keyframe is larger than a set threshold, we perform a global match on it. Then, the keyframes before the second oldest NDT factor will be marginalized out of the window. Because the NDT has already provided the prior value, we don’t need to convert measurements corresponding to marginalized states into a prior. When the global matching of the latest keyframe is done, the optimization process is performed. After that, the LiDAR odometry and the IMU bias will be updated according to the optimization results.
Due to the long time consuming of global matching and optimization, the optimization process will lag. However, the global matching, the optimization process, and the LiDAR odometry belong to diferent threads, so the optimization lag does not afect the operation of LiDAR odometry. Since LiDAR Odometry is processed by matching the scan to the sub-map, the accuracy of Lidar Odometry can be improved by optimizing the sub-map. Meanwhile, thanks to the existence of a global matching factor in sub-map optimization, LiDAR odometry can achieve unbiased estimation.
The sub-map optimization can be represented in pseudocode as Algorithm. 1. (18) (19)
We collected several data sets on the WHUT campus to analyze the proposed framework qualitatively and quantitatively. The sensor suite used in our work includes an ouster OS1-32 and an RTK GPS module, the OS1-32 has a built-in IMU, and the internal parameters are calibrated by the method proposed in [22], the RTK GPS provides the ground-truth for quantitative analysis. For validation, we collected six diferent datasets across two platforms: A-M, A-T-1, A-T-2, B-M, B-T-1, and B-T-2. The A and B mean the data sequences A and B, respectively, the M means the mapping datasets, and the T means the test datasets. The platform of A data sequence is the low-speed Logistics vehicle, the lidar is installed atop it, and the RTK GPS is installed at the same horizontal position. The platform of the B data sequence is the high-speed electric vehicle, and the lidar is installed ahead of it, so we only get half of the points and meet the extreme condition. The platforms are illustrated in Fig.3.
Our method and comparisons are implemented in C++ and executed on a JETSON TX2 with the framework of Robot Operating System (ROS)[23] in Ubuntu Linux.
Firstly, we utilized the A-M and B-M for mapping, the result is shown in table.2, and the trajectory with mapping error is shown in Fig.4.
As shown in Fig.4(a), GPS data is missing on some roads in data sequence A, and those were taken out of the analysis. The average speed within a data sequence is the same. The data sequence B only has half the point cloud, and the moving speed is faster. As a result, its accuracy is relatively low. It can be seen that the accuracy of the mapping is approximately in centimeters and meet the requirements.
(a) (b)
Then, we used the datasets A-T-1, A-T-2, B-T-1, and B-T-2 to analyze the online location module
on the prior map they built, respectively. Our method is comparable to the LIO method from
[
It can be seen that the LIO method has the most significant error cause it sufers from drift. The NDT method used the linear model to estimate the initial guess based on the assumption of the short-term velocity invariance. The imprecise initial guess results in a bigger error, especially in cases like a sharp turning point and Speed-up Speed-down, which will cause the failure like the dataset B-T-1. In contrast, our method achieves better precision in most datasets due to the fusion of LIO and NDT methods. However, our approach also relies on NDT to eliminate accumulated errors, so it has no significant improvement in accuracy than the NDT method but significantly improves robustness, especially in extreme cases.
In this paper, we propose a precision and robust LiDAR localization framework that combines LiDAR odometry with NDT-based global matching. We link them through a sub-map optimization built atop the factor graph. Through the experimental analysis, it has been shown that our method can perform well. It’s worth noting that the problem discussed in this paper does not involve estimating the initial position on the map, which is usually given by GPS or related algorithms in engineering. Since GPS is not needed for the proposed localization system, so the system is also suitable for indoor environments. However, due to the limitation of experimental conditions, the experiments in this paper are carried out outdoors. Follow-up indoor experiments are required. Although we have achieved some phased results, there is still a lot of work to do, such as the influence of environmental changes on localization and the dynamic update of the map. We will handle those problems in future work. universitet, 2009. [19] The three-dimensional normal-distributions transform : an eficient representation for registration, surface analysis, and loop detection, renewable energy (2009). [20] F. Dellaert, M. Kaess, et al., Factor graphs for robot perception, Foundations and Trends® in Robotics 6 (2017) 1–139. [21] R. B. Rusu, S. Cousins, 3d is here: Point cloud library (pcl), in: 2011 IEEE international conference on robotics and automation, IEEE, 2011, pp. 1–4. [22] W. Gao, imu_utils, https://github.com/gaowenliang/imu_utils, 2018. [23] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, A. Y. Ng, Ros: an open-source robot operating system, in: ICRA workshop on open source software, volume 3, Kobe, Japan, 2009, p. 5.