Classic literature reading--GCLO (Low-drift ground-constrained laser odometry for GPS-free indoor environments)

0. Introduction

Automated valet parking (AVP) is a promising application of autonomous driving technology, which aims to enable vehicles to navigate and park themselves to a target location. HD mapping plays a key role in AVP as it can provide a priori information about the target parking lot with centimeter-level accuracy. Compared with open road scenes, RTK can actually play a more critical role. However, for example, in scenes where the structure of the underground parking lot is relatively simple and there is no GPS, the lidar is easy to drift upward in the vertical direction in the underground parking lot, resulting in The mapping results are poor. The specific reason is that when the incident angle is large, the depth measured by lidar may be biased. Therefore, as the vehicle moves on the ground, the point observed from the road becomes slightly curved and the trajectory estimated by the LO drifts in the vertical direction.

For this purpose, this article "Ground-SLAM: Ground Constrained LiDAR SLAM for Structured Multi-Floor Environments" uses the The ground is used to compress the pose drift mainly caused by LiDAR measurement bias. Ground-SLAM is developed based on the well-known pose graph optimization framework. On the front end, LiDAR Odometry (LO) is used for motion estimation, and a novel sensor-centered sliding map is introduced that is maintained by filtering out outdated features based on an error propagation model . On each keyframe, the sliding map is recorded as a local map. The nearby ground is extracted and modeled as an infinite plane of landmarks in the form of a closest point (CP) parameterization. Then, associate the ground planes observed in different key frames, and integrate the ground constraints into the pose graph optimization framework to compress the pose drift of LO. Finally, loop closure detection is performed and residual errors are jointly minimized to obtain a globally consistent map.

1. Main contributions

A LO method that utilizes ground observation constraints to reduce pose drift in multi-layered indoor and flat outdoor environments is proposed.
A ground matching method is proposed to correlate ground plane landmarks by detecting sharp changes between consecutive key frames.
Experimental results based on the KITTI and HIK datasets illustrate the accuracy, and limitations in the generalization of the algorithm are discussed.

2. Symbols and Preparations

2.1 Symbols

In the following section, we represent the homogeneous transformation matrix as $T^b_a ∈ SE(3)$ ，它帧< /span> $F_a$ midpoint $p^a_i ∈ R^3$ 转换为帧 $F_b$ Neutral point. $R^b_a ∈ SO(3)$ 和
$t^b_a ∈ R^3$ are the rotation matrix and translation vector respectively. We use $L, B, W$ Display LiDAR, IMU and all local control system. For me $\Pi^a$ display line $F_a$ The closest point (CP) parameter in the infinite plane.

2.2 Prerequisites

Researchers have proposed several parameterization methods for infinite planes. The Hesse form (HF) consists of a normal direction $\vec{n}$ and the distance between the plane and the origin of the given coordinate system $Composed of d$ . HF is an overparametric method because it uses 3D vectors to represent normal vectors with 2 degrees of freedom. Therefore, in least squares optimization, it encounters the problem of singular information matrix, and the consistency of the normal vector becomes difficult to maintain. Spherical coordinates are a minimally parameterized method of representing normal vectors in terms of azimuth and elevation angles. However, when the elevation angle is equal to $±\pi/2$ , it will encounter the ambiguity problemUnit quaternion [30] is also used to represent infinite planes. However, the physical connection between quaternions and planes is unclear.
Inspired by [31], this article uses CP to represent the infinite plane. CP refers to the point on the plane closest to the origin of the given coordinate system a> $F_a$ In this paper, it is crucial to convert the parameters of the ground between different coordinate systems because the ground is modeled as an infinite planar landmark. HF makes it easy to represent plane equations, so we will use HF as an intermediate parameter for the conversion of ground parameters. Assume that in the coordinate system . when the origin of a given coordinate system is located on the plane, singularity problems will also be encountered. However, in this paper, since the ground plane is at a certain distance from the LiDAR equipped on the vehicle, the singularity problem of CP representation can be easily avoided. CP is a minimal representation method, so
$F_{a}$ One of has HF $[\vec{n}^a,d^a]$ 的平面。点 $p^a_i$ is located on the plane, then the point satisfies the plane equation, as shown in equation (1):
Insert image description here
If we know from frame $F_a$ reaching $F_b$ The transformation matrix of is $T^a_b$ ，并且 $F_b$ This is intelligent, な么が们具 $p^a_i = R^a_bp^b_i + t^a_b$ . Substituting this into formula (1), we can get formula (2).

Then we can use the equation in (3) to get $\vec{n}^b$ sum $d^b$ . After getting the transformation result expressed in HF, we can convert it to CP parameterization using (4).
Insert image description here

3. System framework (the core is to use ground constraints to limit the vertical drift of LO)

Insert image description here

Figure 3 shows a brief overview of the proposed framework. In the preprocessing module, the IMU and wheel encoder measurements are fused using the Extended Kalman Filter (EKF) method to provide high-frequency motion estimation results. By using motion estimation results, motion distortion of LiDAR scans is reduced. Dedistorted LiDAR scan points are used to estimate the relative transformation between consecutive scans using a point-to-plane ICP algorithm. To overcome the sparsity of LiDAR scans and improve the accuracy of LO, a sliding map centered on the sensor is maintained. The sensor-centered sliding map is recorded as a local map at each keyframe, and the ground plane is extracted using a weighted least squares method. Local correspondences between ground planes observed at different keyframes were determined. Then, the ground observation constraints are fused into the attitude graph optimization framework. Attitude drift is compressed, especially vertical drift caused by LiDAR measurement bias. Improved accuracy of LO result trajectories. Loop closure detection is then performed and the closed loop edges between the newly associated keyframes are added to the pose graph. Finally, the residual error is minimized and a globally consistent map is assembled. Details of key modules are introduced below.

4 Scan2map with sensor-centered sliding window (key content)

Our lidar odometry method mainly consists of two parts: a scan-to-map registration framework and an observation-based maintenance method of the sensor center sliding map. The main function of the registration framework is scan registration and data conversion. Suppose that at time $When k$ , the global attitude of the lidar is $T_{L_k}^W$ , and sliding map $M_{L_k}$ Shanghai Yu $L_k$ The frame has been maintained $w . r . t$ . Our new drawing $S_{k+1}$ at time $k + When 1$ arrives, the sensor transformation is first estimated using point-to-plane ICP [32] and Censi’s method [33] $T^{k+1}_k$ and the corresponding covariance $Σ_{T^{k+1}_k}$ . The coordinates and uncertainty covariance of the points in the sliding map can be obtained from $L_k by formulas (5) and (6)$ Frame conversion to $L_{k+1}$ 帧、其中 $p^k_i$ is a sliding map $M_{L_k}$ middle part $i$ point， $Σ_{p^k_i}$ is the corresponding covariance. $Σ_{R^{k+1}_k}$ 和 $Σ_{t^{k+1}_k}$ are the covariance matrices of the estimated rotation and translation components respectively. $J_{R^{k+1}_k}$ sum $J_{p^{k}_i}$ are the Jacobian matrices relative to the rotation components and points respectively.
Insert image description here
The observation-based maintenance method is designed for sensor-centered sliding map updates and includes the following steps:
1) First, by checking a predefined distance metric (Such as Euclidean distance, Mahalanobis distance, etc.), associate the new scan points registered with the sliding map points.
2) Then, select the associated sliding map points and reset their uncertainties to the covariance matrix of their associated new scan points, Often called observation error.
3) Points that capture considerable uncertainty in the sliding map will be filtered out . In practice, the point is eliminated whenever the trace of the corresponding covariance matrix is more significant than the threshold.
4) Finally, registered scan points that failed to be associated will be added to the sliding map as new observations.

Observation-based maintenance methods filter out points with considerable uncertainty while retaining and updating observation features. Therefore, the consistency and density of the sliding map are maintained, which is an important requirement for subsequent scan registration and motion estimation.

5. Ground constraints

Ground extraction and modeling Considering that most parking lot floors are flat, an infinite plane is used for modeling. For each keyframe, ground points are first segmented from a sensor-centered sliding window map. Then RANSAC is applied to obtain the initial CP representation based on these segmented ground point sets. The representation obtained by RANSAC is definitely noisy, so a weighted least squares problem is defined to minimize the distance from the point to the ground:
Insert image description here
Minimize the above formula through the Gauss-Newton method to obtain a suitable Plane equation expressed in CP notation:

6. Plane continuity estimation

Since the ground in this article is modeled as a planar landmark, it is necessary to associate the extracted ground planes at different locations. Figure 4 shows a classic scenario of ground correspondence estimation in a structured multi-layer indoor environment. Suppose there are several consecutive key frames $F_i$ , inside $i = 1, \dots, N$ ， $N$ is the number of keyframes. Each keyframe has an estimated pose provided by LO $T^w_i$ and the observed ground plane and its estimated parameters $\Pi^i$ harmony indeterminacy $Σ_{\Pi^i}$ , so the remaining problem is to determine the correspondence between these observed ground planes.

A straightforward solution to this problem is to relate the parameters of the ground planes by comparing them in an same coordinate system using a distance metric (e.g. Euclidean distance). However, errors in the LO accumulate, and this error is propagated into the plane parameters. Therefore, incorrect data association may occur. It is important to note that the LO will drift slightly and maintain high accuracy over short periods of time. In addition, indoor floors usually have good structures, and ground parameters change drastically at multi-layer connections, as shown in Figure 4. Therefore, we perform local ground correspondence estimation between two consecutive key frames by detecting drastic changes in ground CP parameters, as shown in equations (14) and (15), function $f$ refers to the transformation in formulas (3) and (4), $Ω_{Δ\Pi^i }$ This is the same amount of money $∆\Pi^i$ 's indefinite qualitative converse.
Insert image description here
Inside $f ()$ second finger: