A 3D LiDAR Odometry Optimization Method for Planar Roads

Abstract

To address the Z-direction drift problem during 3D LiDAR outdoor road mapping, the present invention discloses a 3D LiDAR odometry optimization method suitable for planar roads. The method comprises: extracting a frame of point cloud, removing point cloud distortion, and performing point cloud matching to obtain the robot's offset between two frames; proposing a road ground point extraction method to extract road seed points from the extracted ground point cloud; projecting the seed point set into a global coordinate system and using the singular value decomposition method to perform plane fitting on the seed point set to establish a plane model; using the position coordinates obtained from point cloud matching as predicted values, and the position coordinates calculated from the fitted plane model as observed values, performing Kalman filtering, and correcting the LiDAR odometry in real time. The present invention can effectively optimize the Z-axis drift problem generated when a mobile robot maps on a flat road, ensuring the accuracy of the mobile robot's mapping.

Description

Technical Field

[0001] The present invention relates to the field of robotics, in particular to the field of mobile robot positioning and mapping, and relates to a 3D laser radar odometer optimization method suitable for planar roads. Background Art

[0002] Mobile robot positioning and mapping, a core technology in autonomous driving research, has garnered extensive research. Compared to mapping and positioning using a single radar sensor, multi-sensor fusion algorithms improve mapping tolerance and accuracy. These algorithms utilize sensors such as inertial sensors, visual sensors, radar sensors, and GPS sensors. However, on smooth roads, multi-sensor fusion algorithms suffer from insufficient Z-axis observation information from 3D lidar and relatively large Z-axis errors due to the influence of gravity on inertial devices. This results in Z-axis drift during mapping, resulting in suboptimal mapping results.

[0003] To address the above issues, the paper "LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain" proposes a method for extracting ground point clouds, which has the following problems:

[0004] (1) After extracting the ground point cloud, only edge points and surface point features are used for point cloud registration, without considering the error of the Z axis of the current frame;

[0005] (2) When the map is too large, the accumulated Z-direction error causes the path deviation to increase, resulting in loop detection failure and unsatisfactory mapping results. Summary of the Invention

[0006] The present invention proposes a method for optimizing the lidar front-end odometer by constraining the Z-direction deviation by fitting ground point clouds. Plane fitting is performed by extracting a ground seed point cloud set, and the position coordinates obtained by point cloud matching are used as predicted values. The position coordinates calculated by the fitted plane model are used as observed values ​​for Kalman filtering and real-time correction of the lidar odometer.

[0007] To achieve the above objectives, the present invention provides a 3D laser radar odometer optimization method applicable to planar roads, comprising the following steps:

[0008] S1, extract a frame of point cloud and use the IMU odometry information to remove point cloud distortion;

[0009] S2,extract the ground point cloud and match the non-ground point cloud with the non-ground point cloud of the previous frame to obtain the offset R,t of the robot between the two frames;

[0010] S3. Propose a road ground point extraction method to extract road seed points from the extracted ground point cloud;

[0011] S4, projecting the set of seed points into the global coordinate system and fitting the ground point cloud using the singular value decomposition method to obtain a plane model;

[0012] S5. A method for optimizing 3D lidar odometry using Kalman filtering is proposed;

[0013] S3 proposes a road ground point extraction method to extract road seed points from the extracted ground point cloud, which specifically includes the following sub-steps:

[0014] S31, set the vehicle head direction to the X axis, the road threshold to 2, and extract the road point cloud set P from the distortion-compensated ground point cloud frame according to the road threshold. * , the specific formula is as follows:

[0015] P * (||P y ||<2) (1)

[0016] S32, point cloud collection P * Sort by height to get the point cloud set P sorted , P N To collect P from point cloud sorted Extract the set of N point clouds with the smallest height from the point cloud set P. N The average height of N i is the height value of the i-th point, and the specific formula is as follows:

[0017]

[0018] S33, set the height of the laser radar from the ground to T, and determine the original point cloud set P * The point cloud with a height less than T+μ is used as the seed point cloud set P seeds .

[0019] The S4 projects the set of seed points into the global coordinate system and uses the singular value decomposition method to fit the ground point cloud to obtain a plane model, which specifically includes the following sub-steps:

[0020] S41. Given that the coordinates of the seed point cloud are relative to the current vehicle body coordinate system, project the seed point cloud to the global coordinate system. The specific formula is as follows:

[0021]

[0022] in, is the coordinate of the seed point cloud in the global coordinate system, is the rotation and translation matrix of the current robot coordinate system relative to the previous frame robot coordinate system, is the rotation and translation matrix of the robot relative to the global coordinate system in the previous frame;

[0023] S42. Solve the fitted plane model based on the seed point cloud set, and add constraints to minimize the sum of the distances from the plane to all points.

[0024] S5 proposes a method for optimizing a 3D lidar odometer using Kalman filtering, which specifically includes the following sub-steps:

[0025] S51, convert the position coordinates of the robot in the current frame into the position coordinates of the robot in the global coordinate system The specific formula is as follows:

[0026]

[0027] in, is the position coordinate of the robot in the global coordinate system in the current frame, Point cloud matching results in S3;

[0028] S52. Since the Z-axis deviation is caused by the gravity acceleration in the inertial device, it is assumed that the X-axis and Y-axis deviations can be ignored. The X-axis and Y-axis coordinates of the robot in the global coordinate system are brought into the plane model established in S4 to obtain the observed values.

[0029] S53, the position coordinates of the robot obtained by matching the point cloud in the global coordinate system As the predicted value, the global position coordinates calculated by the plane model fitting in the previous frame are used as the observation value to perform Kalman filtering and correct the lidar odometry in real time.

[0030] The present invention has the following beneficial effects:

[0031] 1. This invention proposes a seed point extraction method that incorporates road threshold judgment to achieve accurate extraction of road points on the road section that the vehicle will subsequently pass through, avoiding the processing of huge point cloud data, improving calculation speed, and enhancing ground point recognition accuracy;

[0032] 2. The method described in the present invention uses ground point extraction and fitting of the road plane to constrain the Z-direction drift during mapping. The ground fitting model is used as the observation value, the point cloud matching value is used as the prediction value, and Kalman filtering is performed to make the Z-direction value as close to the true value as possible. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0034] Figure 1 Flowchart of the 3D lidar odometry optimization method for planar roads. DETAILED DESCRIPTION

[0035] In order to make the purpose, technical solutions and advantages of the present invention more clear, the following Figure 1 , the present invention is described in further detail.

[0036] S1. Extract a frame of point cloud and use the IMU odometry information to remove point cloud distortion. This includes the following sub-steps:

[0037] S11, extract a frame of lidar point cloud, t s , t n , t e They are the start time, current time and end time of a frame of data respectively. Since the IMU operating frequency is higher than the laser radar, the current time t n Find the pose calculated by IMU at two moments before and after, where the moments before and after IMU are t k , t k+1 , the corresponding vehicle posture is and The specific formula is as follows:

[0038]

[0039] in, t k The position information calculated by the accelerometer at the moment, γ k ,θ k , ψ k t k Attitude information calculated by the gyroscope at all times, t k+1 Position information calculated by the accelerometer at the moment γ k+1 ,θ k+1 , ψ k+1 t k+1 Attitude information calculated by the gyroscope at all times;

[0040] S12. Calculate the vehicle posture at the current moment through interpolation based on the solved posture. The specific formula is as follows:

[0041]

[0042] Among them, b l and b r is the interpolation coefficient, is the vehicle posture at the current moment;

[0043] S13. Compensate the point cloud distortion based on the interpolated pose. The specific formula is as follows:

[0044]

[0045] in, is the vehicle’s posture at the initial moment, P n is the coordinate of the laser point at the current moment, The coordinates of the laser point at the current moment after compensation for distortion.

[0046] S2. Extract the ground point cloud and match the non-ground point cloud with the non-ground point cloud of the previous frame to obtain the robot offset R, t between the two frames; specifically, it includes the following sub-steps:

[0047] S21, extract two frames of non-ground point cloud, select two points that can be correlated with each other, that is, N x =N y , Point cloud for this frame, For the point cloud of the previous frame, set the equations for rotation and translation. The specific formulas are as follows:

[0048] Target:

[0049]

[0050] Where E(R,T) is the rotation and translation relationship between the two frames of point cloud, E1(R,T) and E2(R,T) are equations related only to rotation and translation respectively, and R is the desired rotation matrix;

[0051] S22. Use the SVD method to solve the rotation matrix R in E1(R,T). The specific formula is as follows:

[0052]

[0053] max{E′1(R,T)}=max{Trace(RH)} (7)

[0054]

[0055]

[0056]

[0057] Among them, ux and u y are the centroids of point sets X and Y respectively, Σ H 、V H They are the diagonal matrix and unitary matrix decomposed by H matrix, respectively. According to the constraints, we can get

[0058] S23. Use the rotation matrix R to solve the translation amount T according to the translation-related equation E2(R,T). The specific formula is as follows:

[0059] E2(R,T)=||u x -Ru y -T|| 2 (11)

[0060] T=u x -Ru y (12)

[0061] S3. A road ground point extraction method is proposed to extract road seed points from the extracted ground point cloud, which specifically includes the following sub-steps:

[0062] S31, set the vehicle head direction to the X axis, the road threshold to 2 meters, and extract the road point P from the distortion-compensated point cloud P according to the road threshold. * , the specific formula is as follows:

[0063] P * (||P y ||<2) (13)

[0064] Among them, ||P y || is the distance from the point to the X-axis plane;

[0065] S32, point cloud collection P * Sort by height to get the point cloud set P sorted , P N To collect P from point cloud sorted Extract the set of N point clouds with the smallest height from the point cloud set P. N The average height of N i is the height value of the i-th point, and the specific formula is as follows:

[0066]

[0067] S33, set the height of the laser radar from the ground to T, and determine the original point cloud set P * The point cloud with a height less than T+μ is used as the seed point cloud set P seeds .

[0068] S4. Projecting the set of seed points to the global coordinate system and fitting the ground point cloud using the singular value decomposition method to obtain a plane model, specifically including the following sub-steps:

[0069] S41. The coordinates of the seed point cloud set are known to be the coordinates of the point cloud relative to the current vehicle body coordinate system. The seed point cloud set is projected to the global coordinate system. The specific formula is as follows:

[0070]

[0071] in, is the coordinate of the seed point cloud in the global coordinate system, is the rotation and translation matrix of the current robot coordinate system relative to the previous frame robot coordinate system, is the rotation and translation matrix of the robot relative to the global coordinate system in the previous frame;

[0072] S42. Fitting a plane equation based on the seed point cloud set in the global coordinate system. To minimize the sum of the distances from the plane to all points, constraints need to be added. The specific formula is as follows:

[0073]

[0074]

[0075]

[0076] Among them, A, B, C, and D are the plane parameters to be determined, (x i ,y i ,z i ) is a point cloud set The coordinates of the i-th point in , Point Cloud Collection The average coordinates of all points in ;

[0077] S43. Establish the objective function of the fitting based on the matrix form of the subtraction between the average coordinate equation and the fitting plane equation. The specific formula is as follows:

[0078]

[0079] Here, let the left matrix be defined as F matrix, the right matrix be defined as X matrix, the objective function of fitting be min||FX||, and the constraint condition be ||X||=1;

[0080] S44. Perform singular value decomposition on the F matrix and solve for X. The specific formula is as follows:

[0081] F=UΣV T (20)

[0082] min||FX||=min||ΣVT X|| (21)

[0083] Where Σ is a diagonal matrix with singular values ​​on the diagonal elements, U and V are unitary matrices, so the optimal solution is X = V

[001] T .

[0084] S5. A method for optimizing a 3D lidar odometry using Kalman filtering is proposed, which specifically includes the following sub-steps:

[0085] S51, convert the position coordinates of the robot in the current frame into the position coordinates of the robot in the global coordinate system The specific formula is as follows:

[0086]

[0087] in, is the position coordinate of the robot in the global coordinate system in the current frame, Point cloud matching results in S3;

[0088] S52. Since the Z-axis deviation is caused by the gravity acceleration in the inertial device, it is assumed that the X-axis and Y-axis deviations can be ignored. The X-axis and Y-axis coordinates of the robot in the global coordinate system are brought into the plane model established in S4 to obtain the observed values.

[0089] S53, the position coordinates of the robot obtained by matching the point cloud in the global coordinate system As the predicted value, the global position coordinates calculated by the plane model fitting in the previous frame are used as the observation value to perform Kalman filtering and correct the lidar odometry in real time.

[0090] The above description is merely a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Any person skilled in the art may, without departing from the scope of the technical solution of the present invention, make some changes or modifications to equivalent embodiments using the technical contents disclosed above. However, any simple modification, equivalent change and modification made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall still fall within the scope of the technical solution of the present invention.

Claims

1. A 3D LiDAR odometry optimization method for planar roads, characterized by: S1, extract a frame of point cloud and use the IMU odometry information to remove point cloud distortion; S2,extract the ground point cloud and match the non-ground point cloud with the non-ground point cloud of the previous frame to obtain the offset R,t of the robot between the two frames; S3. Propose a road ground point extraction method to extract road seed points from the extracted ground point cloud; S4, projecting the set of seed points into the global coordinate system and fitting the ground point cloud using the singular value decomposition method to obtain a plane model; S5. A method for optimizing 3D lidar odometry using Kalman filtering is proposed; S3 proposes a road ground point extraction method to extract road seed points from the extracted ground point cloud, which specifically includes the following sub-steps: S31, set the vehicle head direction to the X axis, the road threshold to 2, and extract the road point cloud set P from the ground point cloud after distortion compensation according to the road threshold. , the specific formula is as follows: (1) in, is the distance from the point to the X-axis plane; S32. Road point cloud collection Sort by height to get the point cloud set P sorted , P N To collect P from point cloud sorted Extract the set of N point clouds with the smallest height from the point cloud set P. N The average height of N i is the height value of the i-th point, and the specific formula is as follows: (2) S33, set the height of the laser radar from the ground to T, and determine the road point cloud The point cloud with a height less than T+μ is used as the seed point cloud set P seeds ; The S4 projects the set of seed points into the global coordinate system and uses the singular value decomposition method to fit the ground point cloud to obtain a plane model, which specifically includes the following sub-steps: S41. Given that the coordinates of the seed point cloud are relative to the current vehicle body coordinate system, project the seed point cloud to the global coordinate system. The specific formula is as follows: (3) in, is the coordinate of the seed point cloud in the global coordinate system, 、 They are the rotation and translation matrices of the current robot coordinate system relative to the previous frame robot coordinate system, 、 are the rotation and translation matrices of the robot relative to the global coordinate system in the previous frame; S42, solving a fitted plane model based on the seed point cloud set, and adding constraints to minimize the sum of distances from the plane to all points; S5 proposes a method for optimizing a 3D lidar odometer using Kalman filtering, which specifically includes the following sub-steps: S51, convert the position coordinates of the robot in the current frame into the position coordinates of the robot in the global coordinate system The specific formula is as follows: (4) in, is the position coordinate of the robot in the global coordinate system in the current frame, 、 、 Point cloud matching results in S3; S52. Since the Z-axis deviation is caused by the gravity acceleration in the inertial device, it is assumed that the X-axis and Y-axis deviations are negligible. The X-axis and Y-axis coordinates of the robot in the global coordinate system are brought into the plane model established in S4 to obtain the observed values. ; S53, the position coordinates of the robot obtained by matching the point cloud in the global coordinate system As the predicted value, the global position coordinates calculated by the plane model fitting in the previous frame are used as the observation value to perform Kalman filtering and correct the lidar odometry in real time.

Images