A lidar SLAM method based on loop closure detection in large-scale scenes

Abstract

The invention discloses a laser radar SLAM method based on loop detection in a large-scale scene, including: first, using the curvature of the laser radar local surface point cloud to extract features; second, weighted registration is performed on the feature points of two adjacent frames according to the point-to-line and point-to-plane registration methods to obtain the global pose of the laser radar current frame relative to the initial frame; then, the extracted feature point cloud and the global pose are passed into the loop detection module, and the global descriptor generated by the feature point cloud is used for loop detection; if the loop detection condition is met, the pose between the two frames with successful loop is obtained by using the weighted feature matching method, and all the poses between the current frame and the loop frame are added to the pose map, and global optimization is performed to obtain the optimized pose, and finally, the feature point cloud is transformed according to the optimized pose to construct a global incremental map. This method improves the pose accuracy and effectively shortens the loop detection time by weighting the point cloud information of different feature points and improving the global descriptor, and performing registration by feature matching.

Description

Technical Field

[0001] The present invention relates to a laser radar SLAM method based on loop detection in a large-scale scene, belongs to automatic driving technology, and is applicable to the fields of navigation and positioning. Background Art

[0002] SLAM (simultaneous localization and mapping) is a technology that allows a robot to start from an unknown location in an unknown environment, repeatedly observe environmental features through sensors such as cameras, lasers, and odometers to locate its own position during movement, and then build an incremental map of the surrounding environment based on its own position. Laser SLAM technology has a wide range of applications in unmanned driving, robotics, and the military industry.

[0003] In large-scale scenes, the longer the movement distance, the greater the error of the LiDAR sensor. When the mobile robot moves for a long time, the inter-frame registration can no longer accurately estimate the posture, which will lead to cumulative errors and ghost images in the established three-dimensional map. Therefore, loop detection still faces severe challenges. Most of the existing solutions to loop detection directly use a global descriptor for loop detection and ICP optimization, but they have low computational efficiency and low loop detection accuracy. In order to improve the positioning accuracy and loop detection accuracy and speed of laser SLAM in large-scale scenes, it is urgent to propose a LiDAR SLAM method suitable for large-scale scenes. Summary of the invention

[0004] To solve the above problems, the present invention discloses a laser radar SLAM method based on loop closure detection in a large-scale scene, which can effectively improve the positioning accuracy and loop closure detection speed of laser SLAM in a large-scale scene.

[0005] To achieve the above object, the technical solution of the present invention is as follows:

[0006] The lidar SLAM method based on loop detection in a large-scale scene includes the following steps:

[0007] S1: Feature extraction using the curvature of the local surface point cloud of the LiDAR;

[0008] S2: Perform weighted registration on the feature points of two adjacent frames according to the point-to-line and point-to-plane registration methods to obtain the global pose of the current frame of the LiDAR relative to the initial frame;

[0009] S3: The extracted feature point cloud and global pose are passed to the loop detection module, and the global descriptor generated by the feature point cloud is used for loop detection;

[0010] S4: using a weighted feature matching method to obtain the pose between two frames with successful loop closure, and judging whether the loop closure is successful;

[0011] S5: Add all the poses between the current frame and the loop frame to the pose graph, perform global optimization, and obtain the optimized pose; transform the feature point cloud according to the optimized pose to construct a global incremental map.

[0012] Preferably, the step S1 specifically includes the following process:

[0013] The curvature of the local surface point cloud of the laser radar is used for feature extraction. The curvature calculation formula is:

[0014]

[0015] Where c represents the curvature of the mth point cloud on the kth frame, and S represents the number of adjacent points of the mth point on the same scan line of the kth frame. In order to ensure the correct reflection of the local information of the point cloud and the real-time computing speed, it is generally taken as 10; Represents the coordinates of the mth and nth point clouds on the kth frame.

[0016] Preferably, the step S2 specifically includes the following process:

[0017] S2.1 generates a global feature submap, specifically including: for the first frame point cloud, directly store it in the feature submap; for the kth (k≥2) frame point cloud, downsample the point cloud through voxel filtering, match it with the global feature submap generated by the previous frame, use the matched pose to add the kth frame point cloud to the global feature submap generated by the previous frame, then perform threshold division on the global submap, and extract the point cloud in the appropriate area away from the lidar;

[0018] S2.2 If If it is a corner point cloud, the feature points of two adjacent frames are registered by point-to-line registration. The distance formula from point to line is expressed as:

[0019]

[0020] in, represents the i-th corner point in the laser radar coordinate system, Represents the transformation from the laser radar coordinate system to the global coordinate system, usually set to the unit matrix, represents the distance from the i-th corner point to the straight line in the global coordinate system, Represents the two closest point clouds found by the i-th corner point in the global feature submap in the lidar coordinate system;

[0021] S2.2 If If it is a surface point cloud, the feature points of two adjacent frames are registered by point-to-surface registration. The point-to-surface distance formula is expressed as:

[0022]

[0023] in, represents the i-th surface point in the laser radar coordinate system, Represents the transformation from the laser radar coordinate system to the global coordinate system, usually set to the unit matrix, represents the distance from the i-th surface point to the plane in the laser radar coordinate system, Represents the three closest point clouds found by the i-th corner point in the global feature submap in the lidar coordinate system;

[0024] S2.3 The weight model is designed according to the distance from the laser radar to the point cloud and the intensity of the point cloud. The formula is expressed as:

[0025]

[0026]

[0027]

[0028]

[0029] Among them, w i Represents the total weight of feature point cloud i participating in point cloud matching, Represents the distance weight factor from point cloud i to line or surface features, Where ψ can be e or s, indicating the distance weight from point cloud i to line or surface features. represents the distance weight factor of point cloud i, d represents the distance from point cloud i to the line or surface feature, λ represents the noise threshold, which is taken as 0.05, Represents the intensity weight factor of point cloud i, I i Represents the intensity of point cloud i, I j Represents the intensity of point cloud j in the nearest neighborhood of point cloud i, I max Indicates the maximum distance of intensity, which is a constant value of 255.

[0030] S2.4 Obtain the optimal global pose estimate T by minimizing the distance between the point and the line and the point and the surface W :

[0031]

[0032] Preferably, the step S3 specifically includes the following process:

[0033] S3.1 passes the current frame point cloud into the existing Scan Context method framework, generates a global descriptor by calculating the Scan Context, and selects candidate frames similar to the current frame;

[0034] S3.2 To ensure that the point cloud is accurate enough, only points within the laser radar measurement range of 80m are used. The point cloud of the candidate frame is segmented radially and angularly. The radial resolution is set to 1m and the angular resolution is set to 1°. A 360*80 laser radar Iris image is generated. Then the Iris image is Fourier transformed to estimate the translation between the two Iris images, and the Log-Gabor filter is used to extract binary features:

[0035]

[0036] Where G(f) represents the frequency response of the filter, f represents the center frequency of the filter, which is generated based on the LiDAR Iris image, f0 and σ represent the parameters of the filter. When extracting the image Iris features in the Log-Gabor filter, is a fixed value, take 0.75;

[0037] S3.3 calculates the Hamming distance between the LiDAR Iris 2D feature maps of the current frame and the candidate frame. If the distance is less than a certain threshold, the current frame and the possible loop frame can be obtained.

[0038] Preferably, the step S4 specifically includes the following process:

[0039] S4.1 performs step S2 on the current frame and the loop frame to calculate the pose transformation matrix between them:

[0040]

[0041] in, Indicates the pose of the current frame in the world coordinate system; represents the position of the loop frame in the world coordinate system, Represents the pose from the current frame to the loop frame;

[0042] S4.2 calculates the translation value between the two frames. If it is less than a certain threshold, the rough detection loop is successful:

[0043]

[0044]

[0045] Among them, t represents The translation matrix in , d represents the translation value between the current frame and the loop frame, and P(l) is used as a binary function to determine whether the loop is successful; threshold is set to 30 to ensure that the loop can still be roughly judged whether the loop is successful in the case of cumulative errors;

[0046] S4.3 then uses the final optimization cost score obtained by the nonlinear optimization solver Ceres solver as the loop closure criterion to accurately detect loop closure success:

[0047]

[0048] Among them, score f Represents the final optimized cost score obtained using the Ceres solver.

[0049] Preferably, step S5 specifically includes the following process:

[0050] All key frames and poses between the current frame and the loop frame are added to the pose graph, and the pose graph is globally optimized using the factor graph optimizer GTSAM to update the pose of each frame; the point cloud of each frame is corrected using the updated global pose to obtain a globally consistent incremental map.

[0051] Compared with the prior art, the present invention has the following advantages:

[0052] First, the present invention uses distance information and intensity information to weight the distance from the corner point to the straight line and the distance from the surface point to the straight line. Since only curvature is used to select feature points, there are differences in the quality of feature points. When the laser radar moves a small distance or a small angle, the feature points in the distance usually change greatly, so the distance of the feature point from the laser radar is considered as the weight coefficient; in addition, the intensity information of the laser radar is also considered. For the matching point cloud, the closer the intensity, the higher the matching similarity;

[0053] Secondly, the present invention uses the Iris feature of the laser radar to achieve highly efficient loop detection, which ensures the accuracy of loop detection and effectively improves the efficiency of detection;

[0054] Furthermore, the present invention uses a feature-based matching method between the current frame and the loop frame instead of the traditional time-consuming ICP point cloud registration, which can effectively save time; the global pose optimization is adopted for all poses before the current frame and the loop frame;

[0055] Finally, after detecting possible loop frames, the present invention proposes a two-stage loop detection method. First, after aligning the current frame and the possible loop frame to obtain the pose between the two frames, a one-stage rough detection scheme is used to effectively remove loop frames that are actually not in line with loop detection, and only loop frames within a certain range of the current frame are retained; secondly, a two-stage fine detection scheme is used to output the final cost score through the Ceres solver to find the correct loop frame within the current frame range. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a block diagram of the lidar SLAM method based on loop closure detection in a large-scale scenario of the present invention;

[0057] Figure 2 The present invention is a flow chart of the laser radar SLAM method based on loop detection in a large-scale scenario; DETAILED DESCRIPTION

[0058] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0059] Embodiment 1: A laser radar SLAM method based on loop detection in a large-scale scene according to the present invention is implemented as follows Figure 1 The experimental process is as shown in Figure 2 The process mainly includes the following steps:

[0060] Step S1, using the curvature of the laser radar local surface point cloud to perform feature extraction.

[0061] The curvature of the local surface point cloud of the laser radar is used for feature extraction. The curvature calculation formula is:

[0062]

[0063] Where c represents the curvature of the mth point cloud on the kth frame, and S represents the number of adjacent points of the mth point on the same scan line of the kth frame. In order to ensure the correct reflection of the local information of the point cloud and the real-time computing speed, it is generally taken as 10; Represents the coordinates of the mth and nth point clouds on the kth frame.

[0064] Step S2, performing weighted registration on the feature points of two adjacent frames according to the point-to-line and point-to-plane registration methods to obtain the global pose of the current frame of the laser radar relative to the initial frame;

[0065] The specific process includes the following:

[0066] Generate a global feature submap, specifically including: for the first frame point cloud, directly store it in the feature submap; for the kth (k≥2) frame point cloud, downsample the point cloud through voxel filtering, match it with the global feature submap generated by the previous frame, use the matched pose to add the kth frame point cloud to the global feature submap generated by the previous frame, then perform threshold division on the global submap, and extract the point cloud in the appropriate area away from the lidar;

[0067] like If it is a corner point cloud, the feature points of two adjacent frames are registered by point-to-line registration. The distance formula from point to line is expressed as:

[0068]

[0069] in, represents the i-th corner point in the laser radar coordinate system, Represents the transformation from the laser radar coordinate system to the global coordinate system, usually set to the unit matrix, represents the distance from the i-th corner point to the straight line in the global coordinate system, Represents the two closest point clouds found by the i-th corner point in the global feature submap in the lidar coordinate system;

[0070] like If it is a surface point cloud, the feature points of two adjacent frames are registered by point-to-surface registration. The point-to-surface distance formula is expressed as:

[0071]

[0072] in, represents the i-th surface point in the laser radar coordinate system, Represents the transformation from the laser radar coordinate system to the global coordinate system, usually set to the unit matrix, represents the distance from the i-th surface point to the plane in the laser radar coordinate system, Represents the three closest point clouds found by the i-th corner point in the global feature submap in the lidar coordinate system;

[0073] The weight model is designed according to the distance from the laser radar to the point cloud and the intensity of the point cloud. The formula is expressed as:

[0074]

[0075]

[0076]

[0077]

[0078] Among them, wi Represents the total weight of feature point cloud i participating in point cloud matching, Represents the distance weight factor from point cloud i to line or surface features, Where ψ can be e or s, indicating the distance weight from point cloud i to line or surface features. represents the distance weight factor of point cloud i, d represents the distance from point cloud i to the line or surface feature, λ represents the noise threshold, which is taken as 0.05, Represents the intensity weight factor of point cloud i, I i Represents the intensity of point cloud i, I j Represents the intensity of point cloud j in the nearest neighborhood of point cloud i, I max Indicates the maximum distance of intensity, which is a constant value of 255.

[0079] The optimal global pose estimation T is obtained by minimizing the distance between the point and the line and the point and the surface. W :

[0080]

[0081] Step S3, passing the extracted feature point cloud and global pose into the loop detection module, and performing loop detection using the global descriptor generated by the feature point cloud;

[0082] The specific process includes the following:

[0083] The point cloud of the current frame is passed into the existing Scan Context method framework, the global descriptor is generated by calculating the Scan Context, and the candidate frames similar to the current frame are selected;

[0084] To ensure that the point cloud is accurate enough, only points within the 80m measurement range of the LiDAR are used. The point cloud of the candidate frame is segmented radially and angularly. The radial resolution is set to 1m and the angular resolution is set to 1°. A 360*80 LiDAR Iris image is generated. The Iris image is then Fourier transformed to estimate the translation between the two Iris images, and the Log-Gabor filter is used to extract binary features:

[0085]

[0086] Where G(f) represents the frequency response of the filter, f represents the center frequency of the filter, which is generated based on the LiDAR Iris image, f0 and σ represent the parameters of the filter. When extracting the image Iris features in the Log-Gabor filter, is a fixed value, take 0.75;

[0087] Calculate the Hamming distance between the LiDAR Iris 2D feature map of the current frame and the candidate frame. If it is less than a certain threshold, the current frame and possible loop frames can be obtained.

[0088] Step S4, using a weighted feature matching method to obtain the pose between two frames with successful loop closure, and determining whether the loop closure is successful;

[0089] Perform step S2 on the current frame and the loop frame to calculate the pose transformation matrix between them:

[0090]

[0091] in, Indicates the pose of the current frame in the world coordinate system; represents the pose of the loop frame in the world coordinate system, Represents the pose from the current frame to the loop frame;

[0092] Calculate the translation value between the two frames. If it is less than a certain threshold, the rough detection loop is successful:

[0093]

[0094]

[0095] Where t represents The translation matrix in , d represents the translation value between the current frame and the loop frame, and P(l) is used as a binary function to determine whether the loop is successful; threshold is set to 30 to ensure that the loop can still be roughly judged whether the loop is successful in the case of cumulative errors;

[0096] The final optimization cost score obtained by the nonlinear optimization solver Ceres solver is then used as the loop criterion to accurately detect loop success:

[0097]

[0098] Among them, score f Represents the final optimization cost score obtained using the Ceres solver

[0099] Step S5, adding all the poses between the current frame and the loop frame to the pose map, performing global optimization, and obtaining the optimized pose; transforming the feature point cloud according to the optimized pose to construct a global incremental map.

[0100] All key frames and poses between the current frame and the loop frame are added to the pose graph, and the pose graph is globally optimized using the factor graph optimizer GTSAM to update the pose of each frame; the point cloud of each frame is corrected using the updated global pose to obtain a globally consistent incremental map.

[0101] The technical means disclosed in the scheme of the present invention are not limited to the technical means disclosed in the above-mentioned implementation mode, but also include technical schemes composed of any combination of the above technical features.

Claims

1. A lidar SLAM method based on loop detection in a large-scale scene, characterized in that: The method comprises the following steps: S1: Feature extraction using the curvature of the local surface point cloud of the LiDAR; S2: Perform weighted registration on the feature points of two adjacent frames according to the point-to-line and point-to-plane registration methods to obtain the global pose of the current frame of the LiDAR relative to the initial frame; S3: The extracted feature point cloud and global pose are passed to the loop detection module, and the global descriptor generated by the feature point cloud is used for loop detection; S4: using a weighted feature matching method to obtain the pose between two frames with successful loop closure, and judging whether the loop closure is successful; S5: Add all the poses between the current frame and the loop frame to the pose graph, perform global optimization, obtain the optimized poses, transform the feature point cloud according to the optimized poses to build a global incremental map, Wherein, the step S1 specifically includes the following process: The curvature of the local surface point cloud of the laser radar is used for feature extraction. The curvature calculation formula is: Where c represents the curvature of the mth point cloud on the kth frame, S represents the number of adjacent points of the mth point on the same scan line of the kth frame. In order to ensure the correct reflection of the local information of the point cloud and the real-time computing speed, S is generally set to 10; Represents the coordinates of the mth and nth point clouds on the kth frame; The step S2 specifically includes the following process: S2.1 generates a global feature submap, specifically including: for the first frame point cloud, directly store it in the feature submap; for the kth (k≥2) frame point cloud, downsample the point cloud through voxel filtering, match it with the global feature submap generated by the previous frame, use the matched pose to add the kth frame point cloud to the global feature submap generated by the previous frame, then perform threshold division on the global submap, and extract the point cloud in the appropriate area away from the lidar; S2.2 If If it is a corner point cloud, the feature points of two adjacent frames are registered by point-to-line registration. The distance formula from point to line is expressed as: in, represents the i-th corner point in the laser radar coordinate system, Represents the pose transformation from the laser radar coordinate system to the global coordinate system. The initial value is the unit matrix. represents the distance from the i-th corner point to the straight line in the global coordinate system, Represents the two closest point clouds found by the i-th corner point in the global feature submap in the lidar coordinate system; S2.2 If If it is a surface point cloud, the feature points of two adjacent frames are registered by point-to-surface registration. The point-to-surface distance formula is expressed as: in, represents the i-th surface point in the laser radar coordinate system, Represents the transformation matrix from the laser radar coordinate system to the global coordinate system, and the initial value is the unit matrix. represents the distance from the i-th surface point to the plane in the laser radar coordinate system, Represents the three closest point clouds found by the i-th corner point in the global feature submap in the lidar coordinate system; S2.3 The weight model is designed according to the distance from the laser radar to the point cloud and the intensity of the point cloud. The formula is expressed as: Among them, w i Represents the total weight of feature point cloud i participating in point cloud matching, Represents the distance weight factor from point cloud i to line or surface features, Where ψ can be e or s, indicating the distance weight from point cloud i to line or surface features. represents the distance weight factor of point cloud i, d represents the distance from point cloud i to the line or surface feature, λ represents the noise threshold, which is taken as 0.05, Represents the intensity weight factor of point cloud i, I i Represents the intensity of point cloud i, I j Represents the intensity of point cloud j in the nearest neighborhood of point cloud i, I max Indicates the maximum distance of intensity, which is a constant value of 255. S2.4 Obtain the optimal global pose estimate T by minimizing the distance between the point and the line and the point and the surface W : Among them, ρ is the robust kernel function, N edge Indicates the number of corner points, N surf Indicates the number of noodles; The step S3 specifically includes the following process: S3.1 passes the current frame point cloud into the existing Scan Context method framework, generates a global descriptor by calculating the Scan Context, and selects candidate frames similar to the current frame; S3.2 To ensure that the point cloud is accurate enough, only points within the laser radar measurement range of 80m are used. The point cloud of the candidate frame is segmented radially and angularly. The radial resolution is set to 1m and the angular resolution is set to 1°. A 360*80 laser radar Iris image is generated. Then the Iris image is Fourier transformed to estimate the translation between the two Iris images, and the Log-Gabor filter is used to extract binary features: Where G(f) represents the frequency response of the filter, f represents the center frequency of the filter, which is generated based on the LiDAR Iris image, and f0 and σ represent the parameters of the filter. When extracting the image Iris features in the Log-Gabor filter, is a fixed value, take 0.75; S3.3 calculates the Hamming distance between the LiDAR Iris 2D feature maps of the current frame and the candidate frame. If the distance is less than a certain threshold, the current frame and the possible loop frame can be obtained. The step S4 specifically includes the following process: S4.1 performs step S2 on the current frame and the loop frame to calculate the pose transformation matrix between them: in, Indicates the pose of the current frame in the world coordinate system; represents the pose of the loop frame in the world coordinate system, Represents the pose from the current frame to the loop frame; S4.2 calculates the translation value between the two frames. If it is less than a certain threshold, the rough detection loop is successful: Where t represents The translation matrix in , d represents the translation value between the current frame and the loop frame, and P(l) is used as a binary function to determine whether the loop is successful; threshold is set to 30 to ensure that the loop can still be roughly judged whether the loop is successful in the case of cumulative errors; S4.3 then uses the final optimization cost score obtained by the nonlinear optimization solver Ceres solver as the loop closure criterion to accurately detect loop closure success: Among them, score f represents the final optimization cost score obtained using the Ceres solver, The step S5 specifically includes the following process: All key frame poses between the current frame and the loop frame are added to the pose graph, and the pose graph is globally optimized using the factor graph optimizer GTSAM; the updated global pose is used to correct the point cloud of each frame to obtain a globally consistent incremental map.

Images