A laser SLAM method integrating laser odometer and loop detection

Abstract

The present invention relates to a laser SLAM method integrating a laser odometer and loop detection, and belongs to the field of autonomous driving technology. The method comprises: extracting features using the smoothness between a point and a point in its neighborhood; matching feature points to obtain the pose of the vehicle pose in the current frame in the world coordinate system; at the same time, passing the feature points into the loop detection module to construct a global descriptor for loop detection, and outputting the point cloud of the loop frame if a loop is detected; obtaining the pose transformation between the vehicle pose in the current frame and the vehicle pose in the loop frame using an inter-frame matching method based on feature points; adding the pose transformation between the vehicle pose in the current frame and the vehicle pose in the previous frame, and the pose transformation between the vehicle pose in the current frame and the vehicle pose in the loop frame to a pose graph, performing global pose optimization, and finally outputting a new pose. The present invention reduces the computational cost while improving the effect of global pose optimization.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving and relates to a laser SLAM method integrating a laser odometer and loop detection. Background Art

[0002] Laser radar-based simultaneous localization and mapping (laser SLAM) has gradually developed into a framework with three parts, namely front-end odometer, loop detection and global optimization. Most of the existing 3D laser SLAM algorithms have achieved good results on open source datasets, but there are still some limitations. The biggest problem is that many laser SLAM algorithms only have front-end odometers and no loop detection, which leads to the accumulation of vehicle pose estimation errors as the scene expands. Although many laser SLAM methods have adopted loop detection, most of them directly apply a certain loop method to construct pose constraints, resulting in SLAM positioning accuracy that cannot achieve ideal results.

[0003] In summary, the existing laser SLAM methods have the following main problems and shortcomings:

[0004] (1) The front-end odometer cannot eliminate the accumulated error, so the positioning accuracy in large scenes is poor.

[0005] (2) Most laser SLAM methods that integrate loop detection use raw point cloud matching methods to construct the vehicle’s pose constraints between loop frames, resulting in low computational efficiency. Summary of the invention

[0006] In view of this, the purpose of the present invention is to provide a laser SLAM method that integrates laser odometer and loop detection to solve the problem that the positioning accuracy of existing laser SLAM methods is not ideal. The method combines the existing F-LOAM and ScanContext, and on this basis, performs adaptive distance judgment on loop detection, and uses a feature point-based method to calculate the posture constraints of the vehicle between a pair of loop frames, thereby reducing the computational cost and improving the effect of global posture optimization.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] A laser SLAM method integrating laser odometer and loop detection comprises the following steps:

[0009] S1: Extract features from the original point cloud input in the current frame using the smoothness between the point and the points in its neighborhood;

[0010] S2: Match the feature point cloud (plane point cloud and boundary point cloud) of the current frame with the previously established feature point cloud submap to obtain the vehicle posture of the current frame in the world coordinate system;

[0011] At the same time, the feature points are passed into the loop detection module to construct a global descriptor for loop detection. If a loop is detected, the point cloud of the loop frame is output;

[0012] S3: Obtaining the posture transformation between the vehicle posture of the current frame and the vehicle posture of the loop frame by using the inter-frame matching method based on feature points;

[0013] S4: Add the pose transformation between the current frame vehicle pose and the previous frame vehicle pose, as well as the pose transformation between the current frame vehicle pose and the loop frame vehicle pose to the pose graph, perform global pose optimization, and finally output a new pose.

[0014] Further, step S1 specifically includes the following steps:

[0015] S11: Point and its neighboring points, specifically including: Often the number of points collected by the laser radar in the horizontal direction is greater than the number of point clouds in the vertical direction. Therefore, the horizontal point cloud collected by each line of laser beam has rich features. As the i-th point cloud in the m-th line laser beam of the k-th frame laser radar, the adjacent points with the same laser beam are its neighborhood points;

[0016] S12: Smoothness calculation, specifically including: sorting the point cloud data of a laser beam in the kth frame according to the angle between it and the X-axis of the laser radar to form a ring data connected end to end, then Included The number of point clouds in the clockwise and counterclockwise directions is the same, and let As The number of neighborhood points; for each point, the smoothness is calculated as:

[0017]

[0018] Among them, c represents the smoothness of the current point.

[0019] Further, in step S2, the posture of the vehicle in the current frame in the world coordinate system is obtained, which specifically includes the following steps:

[0020] S201: constructing a feature submap, specifically including: assuming that the current frame is k, the feature map of the k-1 frame uses the position of the vehicle in the k-1 frame in the world coordinate system to project the feature point cloud of the k-1 frame into the world coordinate system, and adds it to the feature map of the k-2 frame to obtain the feature submap of the k-1 frame; wherein the point cloud data of the first frame is directly used as the feature map; the feature submap of the k-1 frame intercepts the feature point cloud of the feature map of the k-1 frame that is included in the set threshold range;

[0021] S202: Find the corresponding feature line / surface of the feature point cloud of the current frame in the feature submap, specifically including: using two 3DKD-trees to store the plane feature point cloud and the boundary feature point cloud in the feature submap respectively; using the KD-tree method to find the five nearest points in the corresponding feature point cloud for each plane feature point and each boundary feature point of the current frame, and calculate the covariance matrix formed by these five points to obtain the feature line / surface corresponding to each feature point of the current frame in the feature submap; wherein the feature line is represented as: the point on the line and the line segment direction vector The characteristic surface is represented by: the position of the plane and the normal vector

[0022] S203: The feature point cloud is matched to obtain the current frame vehicle posture in the world coordinate system, specifically including: firstly constructing the distance from the feature point to the sub-map feature line / surface as:

[0023]

[0024]

[0025]

[0026] Among them, d e (P e ) represents the distance from the feature point to the feature line in the submap, d s (P s ) represents the distance from the feature point to the feature surface in the submap, Indicates the position and posture of the vehicle in the current frame in the world coordinate system. represents the boundary points in the vehicle coordinate system, Represents a plane point in the vehicle coordinate system;

[0027] S204: Optimize the pose transformation between the current frame and the sub-map by minimizing the distance between the feature point and the feature line / surface

[0028] Further, in step S2, the feature points are passed into the loop detection module to construct a global descriptor for loop detection. If a loop is detected, the point cloud of the loop frame is output. Specifically, the following steps are included:

[0029] S211: passing the feature point cloud of the current frame into the Scan Context method framework to obtain a preliminary loop detection result;

[0030] S212: Odometry estimation using two frames of data detected as loop closure and Calculate the odometry estimate of the pose transformation matrix between them

[0031] S213: Calculate the distance between the two frames of pose transformation matrix odometer estimation value in, Respectively represent the values ​​of x, y, and z taken from the pose transformation matrix;

[0032] S214: discard loop detections with distances greater than a threshold as false detections.

[0033] The calculation formula of the threshold is: Wherein, k represents the number of frames between the current frame and the most recent loop detection frame.

[0034] Further, step S3 specifically includes the following steps:

[0035] S31: The frame that forms a loop with the current frame is referred to as a loop frame; the loop frame and its surrounding frames are projected into the world coordinate system to form a feature submap;

[0036] S32: Calculate the pose transformation matrix between the current frame and the loop frame using the same method as in S2

[0037] Further, step S4 specifically includes the following steps:

[0038] S41: Use the k-1 frame vehicle's position in the world coordinate system The position and posture of the vehicle in the world coordinate system in frame k Calculate the vehicle's pose transformation from frame k-1 to frame k And add it to the pose graph;

[0039] S42: transform the pose matrix of the current frame and the loop frame Add to the pose graph;

[0040] S43: Perform global optimization on the pose graph to obtain a new pose for each frame.

[0041] The beneficial effects of the present invention are:

[0042] (1) The present invention uses an adaptive distance judgment method to judge the loop detection detected by the Scan Context method, thereby reducing erroneous loop detection, reducing the calculation time of global optimization, and improving the positioning accuracy of SLAM.

[0043] (2) The present invention adopts a feature matching-based method to construct constraints between loop frames, thereby reducing computational costs.

[0044] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0046] Figure 1 It is a flow chart of the laser SLAM method of the present invention;

[0047] Figure 2 It is a feature submap of the current frame and loop frame in the loop closure posture constructed by the present invention;

[0048] Figure 3 The present invention constructs a ground map using the global optimized posture;

[0049] Figure 4 It is a comparison diagram of the absolute posture errors of the method of the present invention and the compared method. DETAILED DESCRIPTION

[0050] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0051] See also Figure 1 to Figure 4 , Figure 1 The following is a flow chart of a laser SLAM method integrating laser odometer and loop detection. The method includes the following steps:

[0052] Step 1: Extract features from the original point cloud input in the current frame using the smoothness between the point and its neighborhood points; specifically, including:

[0053] Step 1.1: Point and its neighboring points, specifically including: Often the number of points collected by the laser radar in the horizontal direction is greater than the number of point clouds in the vertical direction. Therefore, the horizontal point cloud collected by each line of laser beam has rich features. As the i-th point cloud in the m-th line laser beam of the k-th frame laser radar, the adjacent points with the same laser beam are its neighborhood points.

[0054] Step 1.2: Smoothness calculation, specifically including: sorting the point cloud data of a laser beam in k frames according to the angle between it and the X-axis of the laser radar to form a ring data connected end to end, then Included The number of point clouds in the clockwise and counterclockwise directions is the same, and let As The number of neighboring points. For each point, the smoothness is calculated as:

[0055] Step 2: Match the feature point cloud (plane point cloud and boundary point cloud) of the current frame with the previously established feature point cloud submap to obtain the vehicle posture of the current frame in the world coordinate system; specifically including:

[0056] Step 201: constructing a feature submap, specifically including: assuming that the current frame is k, the feature map of the k-1 frame uses the position of the vehicle in the k-1 frame in the world coordinate system to project the feature point cloud of the k-1 frame into the world coordinate system, and adds it to the feature map of the k-2 frame to obtain the feature submap of the k-1 frame. The point cloud data of the first frame is directly used as the feature map. The feature submap of the k-1 frame intercepts the feature point cloud contained in the feature map of the k-1 frame within the set threshold range.

[0057] Step 202: Find the corresponding feature line / surface of the feature point cloud of the current frame in the feature submap, specifically including: using two 3D KD-trees to store the plane feature point cloud and the boundary feature point cloud in the feature submap respectively. Use the KD-tree method to find the 5 nearest points in the corresponding feature point cloud for each plane feature point and each boundary feature point of the current frame, and calculate the covariance matrix formed by these five points to obtain the feature line / surface corresponding to each feature point of the current frame in the feature submap. Among them, the feature line is represented as: the point on the line and the line segment direction vector The characteristic surface is represented by: the position of the plane and the normal vector

[0058] Step 203: Match the feature point cloud to obtain the current frame vehicle posture in the world coordinate system: First, construct the distance from the feature point to the sub-map feature line / surface: in Represents the position of the vehicle in the current frame in the world coordinate system. Then, the position transformation between the current frame and the submap can be optimized by minimizing the distance between the feature point and the feature line / surface

[0059] Step 3: At the same time as step 2, the feature points are passed into the loop detection module to construct a global descriptor for loop detection. If a loop is detected, the point cloud of the loop frame is output; including:

[0060] Step 3.1: Pass the feature point cloud of the current frame into the ScanContext method framework to obtain preliminary loop detection results;

[0061] Step 3.2: Use the odometry estimate from the two frames detected as loop closures Calculate the odometry estimate of the pose transformation matrix between them

[0062] Step 3.3: Calculate the distance between the two frames' pose transformation matrix and the odometry estimate in Respectively represent the values ​​of x, y, and z taken from the pose transformation matrix;

[0063] Step 3.4: Discard loop detections with distances greater than the threshold as false detections. The threshold calculation formula is: Among them, k represents the number of frames between the current frame and the most recent loop detection frame.

[0064] Step 4: Use the feature point-based inter-frame matching method to obtain the posture transformation between the vehicle posture of the current frame and the vehicle posture of the loop frame. Specifically include:

[0065] Step 4.1: The frame that forms a loop with the current frame is referred to as the loop frame. The loop frame and its surrounding frames are projected into the world coordinate system to form a feature submap, such as Figure 2 As shown, the feature submaps of the current frame and the loop frame.

[0066] Step 4.2: Use the same method as in step 2 to calculate the pose transformation matrix between the current frame and the loop frame

[0067] Step 5: Add the pose transformation between the current frame vehicle pose and the previous frame vehicle pose, and the pose transformation between the current frame vehicle pose and the loop frame vehicle pose to the pose graph, perform global pose optimization, and finally output the new pose. Specifically include:

[0068] Step 5.1: Use the k-1 frame vehicle's position in the world coordinate system The position and posture of the vehicle in the world coordinate system in frame k Calculate the vehicle's pose transformation from frame k-1 to frame k And add it to the pose graph.

[0069] Step 5.1: Transform the pose matrix of the current frame and the loop frame Add to the pose graph.

[0070] Step 5.1: Perform global optimization on the pose graph to obtain a new pose for each frame.

[0071] like Figure 3 As shown, the present invention uses a map constructed by the posture after global optimization. The present invention is compared with the Simple-SC-F-LOAM method that directly combines F-LOAM with the Scan Context method to verify that the present invention has higher positioning accuracy and higher calculation efficiency. Since the Simple-SC-F-LOAM method will detect many false loops, and the present invention optimizes this point by using an adaptive distance judgment method, the SLAM positioning accuracy of the present invention is higher. Figure 4 The absolute pose errors of the present invention and the compared method on KITTI datasets 00 and 05 are shown, indicating that the SLAM positioning accuracy of the present invention is higher. In addition, too many false loops will increase the global optimization time, and the present invention uses a feature point-based method to construct the pose constraints between a pair of loop frames of the vehicle, reducing the computational cost.

[0072] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A laser SLAM method integrating laser odometer and loop detection, characterized in that: The method comprises the following steps: S1: Extract features from the original point cloud input in the current frame using the smoothness between the point and its neighborhood points, specifically including the following steps: S11: Point and its neighboring points, specifically including: As the i-th point cloud in the m-th line laser beam of the k-th frame laser radar, the adjacent points with the same laser beam are its neighborhood points; S12: Smoothness calculation, specifically including: sorting the point cloud data of a laser beam in the kth frame according to the angle between it and the X-axis of the laser radar to form a ring data connected end to end, then Included The number of point clouds in the clockwise and counterclockwise directions is the same, and let As The number of neighborhood points; for each point, the smoothness is calculated as: Among them, c represents the smoothness of the current point; S2: Match the feature points of the current frame feature point cloud with the previously established feature point cloud submap to obtain the vehicle pose of the current frame in the world coordinate system; at the same time, pass the feature points into the loop detection module to construct a global descriptor for loop detection. If a loop is detected, the point cloud of the loop frame is output; In step S2, the posture of the vehicle in the current frame in the world coordinate system is obtained, which specifically includes the following steps: S201: constructing a feature submap, specifically including: assuming that the current frame is k, the feature map of the k-1 frame uses the position of the vehicle in the k-1 frame in the world coordinate system to project the feature point cloud of the k-1 frame into the world coordinate system, and adds it to the feature map of the k-2 frame to obtain the feature submap of the k-1 frame; wherein the point cloud data of the first frame is directly used as the feature map; the feature submap of the k-1 frame intercepts the feature point cloud of the feature map of the k-1 frame that is included in the set threshold range; S202: Find the corresponding feature line / surface of the feature point cloud of the current frame in the feature submap, specifically including: using two 3DKD-trees to store the plane feature point cloud and the boundary feature point cloud in the feature submap respectively; using the KD-tree method to find the five nearest points in the corresponding feature point cloud for each plane feature point and each boundary feature point of the current frame, and calculate the covariance matrix formed by these five points to obtain the feature line / surface corresponding to each feature point of the current frame in the feature submap; wherein the feature line is represented as: the point on the line and the line segment direction vector The characteristic surface is represented by: the position of the plane and the normal vector S203: The feature point cloud is matched to obtain the current frame vehicle posture in the world coordinate system, specifically including: firstly constructing the distance from the feature point to the sub-map feature line / surface as: Among them, d e (P e ) represents the distance from the feature point to the feature line in the submap, d s (P s ) represents the distance from the feature point to the feature surface in the submap, Indicates the position and posture of the vehicle in the current frame in the world coordinate system. represents the boundary points in the vehicle coordinate system, Represents a plane point in the vehicle coordinate system; S204: Optimize the pose transformation between the current frame and the sub-map by minimizing the distance between the feature point and the feature line / surface In step S2, the feature points are passed into the loop detection module to construct a global descriptor for loop detection. If a loop is detected, the point cloud of the loop frame is output, which specifically includes the following steps: S211: passing the feature point cloud of the current frame into the Scan Context method framework to obtain a preliminary loop detection result; S212: Odometry estimation using two frames of data detected as loop closure and Calculate the odometry estimate of the pose transformation matrix between them S213: Calculate the distance between the two frames of pose transformation matrix odometer estimation value in, Respectively represent the values ​​of x, y, and z taken from the pose transformation matrix; S214: discarding loop detections with distances greater than a threshold as false detections; The threshold value is calculated as: Where k represents the number of frames between the current frame and the most recent loop detection frame; S3: Obtaining the posture transformation between the vehicle posture of the current frame and the vehicle posture of the loop frame using the inter-frame matching method based on feature points, specifically including the following steps: S31: The frame that forms a loop with the current frame is referred to as a loop frame; the loop frame and its surrounding frames are projected into the world coordinate system to form a feature submap; S32: Calculate the pose transformation matrix between the current frame and the loop frame using the same method as in S2 S4: adding the pose transformation between the current frame vehicle pose and the previous frame vehicle pose, as well as the pose transformation between the current frame vehicle pose and the loop frame vehicle pose to the pose graph, performing global pose optimization, and finally outputting a new pose, specifically comprising the following steps: S41: Use the k-1 frame vehicle's position in the world coordinate system The pose of the vehicle in the world coordinate system in frame k Calculate the vehicle's pose transformation from frame k-1 to frame k And add it to the pose graph; S42: transform the pose matrix of the current frame and the loop frame Add to the pose graph; S43: Perform global optimization on the pose graph to obtain a new pose for each frame.

Images