Slam loopback detection method and device and storage medium

Abstract

The invention discloses a slab loopback detection method and device and a storage medium, and the method comprises the steps: collecting a submap point cloud which is formed by splicing a plurality of point clouds; preprocessing the submap point cloud to remove interference information in the submap point cloud; performing feature extraction on the preprocessed submap point cloud by using the trained neural network model; carrying out sequence matching on the corresponding features of the submap point cloud and the prior position information; and finding a reasonable closed loop according to a sequence matching result. According to the method, the performance of laser slab loopback detection can be improved, accumulative errors in the slab are effectively reduced, the slab mapping precision is improved, and the mapping and positioning precision in automatic driving is higher.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving map construction, and in particular relates to a method, device and storage medium for SLAM loop detection. Background Art

[0002] Loop detection, also known as closed loop detection, is a key technology in SLAM. Its main purpose is to enable the robot to autonomously identify scenes it has been to. In the construction of autonomous driving maps, the robot's motion state is estimated by the odometer, which mainly considers the change of posture in adjacent time. When the robot moves for a long time, the errors in adjacent time will gradually accumulate, forming cumulative errors, making the deviation too large. At this time, loop detection is needed to eliminate this cumulative error and obtain a globally consistent trajectory and map.

[0003] There are two main types of loop detection methods: feature descriptor method and deep learning model method. LiDAR point cloud is a mapping of the real world. Feature descriptors extract features from laser point clouds based on some fixed rules. For example, the point cloud can be described based on rules such as the change in reflection intensity of the point and the probability distribution of the point. This is the descriptor, or the feature of the point cloud. The scene is then identified based on the similarity of the descriptors, that is, possible closed loops are found. Common methods include scancontext, lidariris, etc. Deep learning models obtain the features of point clouds through the network, and train the network with a large amount of data to enable it to have better ability to distinguish point clouds.

[0004] However, the method of extracting point cloud feature descriptors based on fixed rules performs poorly in large scenes. For example, in a two-way scene with 6 lanes or more, it does not have good resolution. Although the deep learning model has good adaptability to large scenes and excellent performance.

[0005] However, the performance of the above two methods will be greatly reduced when the environment changes. For example, road renovation, different seasons, etc., these scenes change, and the two methods cannot do a good job of identifying these scenes. However, such scene changes frequently exist in the real world, and such special scenes also need to be handled well. Summary of the invention

[0006] In order to solve the above technical problems, the present invention proposes a method, device and storage medium for SLAM loop detection.

[0007] In order to achieve the above object, the technical solution of the present invention is as follows:

[0008] In a first aspect, the present invention discloses a method for SLAM loop detection, comprising:

[0009] Step S1: Collect submap point cloud, which is composed of several point clouds;

[0010] Step S2: pre-processing the submap point cloud to remove interference information;

[0011] Step S3: Use the trained neural network model to extract features from the preprocessed submap point cloud;

[0012] Step S4: Sequentially match the corresponding features of the submap point cloud with the prior position information;

[0013] Step S5: Find a reasonable closed loop based on the sequence matching results.

[0014] Based on the above technical solution, the following improvements can be made:

[0015] As a preferred solution, step S2 includes:

[0016] Step S2.1: downsampling the submap point cloud;

[0017] Step S2.2: Use a filtering algorithm to remove redundant information in the submap point cloud after downsampling;

[0018] Step S2.3: Determine whether the submap point cloud after removing redundant information meets the data dimension requirements;

[0019] If the requirements are met, the submap point cloud is normalized;

[0020] Otherwise, the insufficient points are randomly supplemented from the submap point cloud after removing redundant information, and then normalized.

[0021] As a preferred solution, the neural network model is a pointnetvlad model.

[0022] As a preferred solution, step S4 includes:

[0023] Step S4.1: store the corresponding features of n submap point clouds as a frame sequence;

[0024] Step S4.2: Based on the prior position information, the frame sequence is calculated using the distance equation to obtain a distance matrix, where the element of each position in the distance matrix is ​​the difference value between the corresponding features of the two submap point clouds;

[0025] Step S4.3: Based on the distance matrix, find the best result matching the corresponding features of a single submap point cloud to obtain the local matching result;

[0026] Step S4.4: Based on the local matching result, find a continuous matching string in the distance matrix, and output the sequence matching result. In a second aspect, the present invention discloses a SLAM loop detection device, comprising:

[0027] Point cloud acquisition module, used to collect submap point cloud, which is composed of several point clouds;

[0028] Point cloud preprocessing module, used to preprocess the submap point cloud to remove interference information;

[0029] Point cloud feature extraction module, used to extract features from the preprocessed submap point cloud using the trained neural network model;

[0030] The sequence matching module is used to perform sequence matching between the corresponding features of the submap point cloud and the prior position information;

[0031] The closed loop determination module is used to find a reasonable closed loop based on the sequence matching results.

[0032] As a preferred solution, the point cloud preprocessing module includes:

[0033] Downsampling unit, used to downsample the submap point cloud;

[0034] The elimination unit is used to eliminate redundant information in the submap point cloud after downsampling by using a filtering algorithm;

[0035] A judgment unit is used to judge whether the submap point cloud after removing redundant information meets the data dimension requirements;

[0036] If the requirements are met, the method of normalizing the unit is executed;

[0037] Otherwise, randomly add insufficient points from the submap point cloud after removing redundant information, and then execute the normalization unit method;

[0038] Normalization unit, used to normalize the submap point cloud.

[0039] As a preferred solution, the neural network model is a pointnetvlad model.

[0040] As a preferred solution, the sequence matching module includes:

[0041] A storage unit, used to store the corresponding features of n submap point clouds as a frame sequence;

[0042] A distance matrix calculation unit is used to calculate the frame sequence based on the prior position information using the distance equation to obtain a distance matrix, wherein the element of each position in the distance matrix is ​​the difference value between the corresponding features of the two submap point clouds;

[0043] The matching unit is used to find the best result matching the corresponding features of a single submap point cloud based on the distance matrix to obtain the local matching result;

[0044] The output unit is used to find continuous matching strings in the distance matrix based on the local matching results and output the sequence matching results.

[0045] In a third aspect, the present invention discloses a storage medium storing a computer-readable program. When the computer-readable program is executed by one or more processors, any of the above-mentioned SLAM loopback detection methods is implemented.

[0046] The present invention discloses a method, device and storage medium for SLAM loop detection, which have the following beneficial effects:

[0047] First, the submap point cloud is preprocessed to remove redundant and invalid point information.

[0048] Second, potential closed loops are further screened through sequence matching to effectively reduce misidentification and improve the accuracy and recall rate of closed loops.

[0049] Third, the performance of laser SLAM loop detection is improved, which effectively reduces the cumulative error in SLAM and improves the accuracy of SLAM mapping, making the map and positioning in autonomous driving more accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0051] Figure 1 A flowchart of a method provided by an embodiment of the present invention.

[0052] Figure 2 A flowchart of submap point cloud preprocessing provided by an embodiment of the present invention.

[0053] Figure 3 A schematic diagram of sequence matching provided in an embodiment of the present invention.

[0054] Figure 4(a) is an example of a point cloud after preprocessing provided by an embodiment of the present invention;

[0055] Figure 4 (b) is an example of a point cloud before preprocessing provided in an embodiment of the present invention.

[0056] Figure 5 (a) is a diagram of loop detection results without sequence matching provided by an embodiment of the present invention;

[0057] Figure 5 (b) is a diagram of loop detection results without sequence matching provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0058] The preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0059] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0060] Using ordinal numbers “first,” “second,” “third,” etc. to describe common objects merely indicates that different instances of similar objects are involved and is not intended to imply that the objects so described must have a given order in time, space, order, or in any other manner.

[0061] In addition, the expression of “comprising” an element is an “open” expression, which merely means that corresponding components or steps exist, and should not be interpreted as excluding additional components or steps.

[0062] In order to achieve the purpose of the present invention, some embodiments of the method for slam loop detection are as follows: Figure 1 As shown, the method includes:

[0063] Step S1: Collect submap point cloud, which is composed of several point clouds;

[0064] Step S2: pre-processing the submap point cloud to remove interference information;

[0065] Step S3: Use the trained neural network model to extract features from the preprocessed submap point cloud;

[0066] Step S4: performing sequence matching between the corresponding features of the submap point cloud and the prior position information;

[0067] Step S5: According to the sequence matching results, a reasonable closed loop is finally found, that is, the scenes that the robot has reached are detected.

[0068] The details are described below.

[0069] Further, if Figure 2 As shown, step S2 includes:

[0070] Step S2.1: downsampling the submap point cloud;

[0071] Step S2.2: Use the CSF ground point filtering algorithm to remove the ground information in the submap point cloud after downsampling;

[0072] Step S2.3: Determine whether the submap point cloud after removing the ground information satisfies the requirement of reducing the number of points to 4096;

[0073] If the requirements are met, the submap point cloud is normalized;

[0074] Otherwise, the insufficient points are randomly supplemented from the submap point cloud after removing the ground information, and then normalized.

[0075] Step S2 preprocesses the submap point cloud, which has the following two main benefits.

[0076] 1) Preprocessing can effectively remove redundant information in the submap point cloud. This part of redundant information (such as ground information and unnecessary repeated information in the point cloud) cannot be used as useful information of the point cloud feature. The remaining recognizable information can be used as a feature to measure the similarity between point clouds, preventing redundant information from affecting the accuracy of loop detection results. The submap point clouds before and after removal are shown in Figure 1. Figure 4 (b) and Figure 4 (a) shown.

[0077] 2) Preprocessing can make the submap point cloud meet the input requirements of the neural network model, downsample and normalize it, and reduce the calculation time of the neural network model.

[0078] Furthermore, the neural network model of step S3 uses the pointnetvlad model.

[0079] Step S3 uses the trained neural network model to extract features from the preprocessed submap point cloud. The neural network model uses the pointnetvlad model. The input of the model is the preprocessed submap point cloud, and the output is the features corresponding to the point cloud.

[0080] Further, if Figure 3As shown, step S4 includes:

[0081] Step S4.1: store the corresponding features of n submap point clouds as a frame sequence;

[0082] Step S4.2: Based on the prior position information, the frame sequence is calculated using the distance equation to obtain a distance matrix, where the element of each position in the distance matrix is ​​the difference value between the corresponding features of the two submap point clouds;

[0083] Step S4.3: Based on the distance matrix, find the best result matching the corresponding features of a single submap point cloud to obtain the local matching result;

[0084] Step S4.4: Based on the local matching results, find continuous matching strings in the distance matrix according to the vehicle speed and direction, and output the sequence matching results. If there are n submap point clouds, then after the neural network model, n corresponding features are obtained, and the frame sequence is a 1x n or nx 1 matrix. The distance matrix is ​​an nxn matrix, and the element at each position in the matrix corresponds to the difference value between the features corresponding to the row and column of this position. The prior position information can be used as part of the submap feature descriptor to participate in the similarity calculation, and then the distance matrix is ​​obtained.

[0085] Local matching is to find the object with the best matching feature of a submap separately.

[0086] Sequence matching is based on local matching, enumerating speed and direction, and finding continuous matching strings in the distance matrix. The principle is as follows: if a car is driving continuously on the road and a closed loop is found at a certain position, then the next section of the trajectory will also find a closed loop accordingly. The use of sequence matching can significantly reduce the probability of false matching.

[0087] The loop detection results without sequence matching are as follows: Figure 5 (a) is shown; the loop detection result after sequence matching is shown in Figure 5 (b) As shown in the figure, the effect of closed loop obtained by sequence matching is very ideal. More and more effective closed loops can provide more constraints, which can ensure the global consistency of the map based on SLAM technology and provide better positioning accuracy for autonomous driving.

[0088] The feature descriptor method and deep learning model method mentioned in the background technology both experience a significant drop in performance when the environment changes.

[0089] The present invention pre-processes the submap point cloud to retain the scene with texture information that is not easily changed. At the same time, sequence matching is adopted. It does not matter if a certain frame or several frames cannot be matched individually. As long as a series of sequence matches meet the threshold, it can also detect the closed loop, which effectively solves the problems in the background technology.

[0090] In some embodiments of a device for slam loop detection, the device includes:

[0091] Point cloud acquisition module, used to collect submap point cloud, which is composed of several point clouds;

[0092] Point cloud preprocessing module, used to preprocess the submap point cloud to remove interference information;

[0093] Point cloud feature extraction module, used to extract features from the preprocessed submap point cloud using the trained neural network model;

[0094] The sequence matching module is used to perform sequence matching between the corresponding features of the submap point cloud and the prior position information;

[0095] The closed loop determination module is used to find a reasonable closed loop based on the sequence matching results.

[0096] Furthermore, the point cloud preprocessing module includes:

[0097] Downsampling unit, used to downsample the submap point cloud;

[0098] The elimination unit is used to eliminate redundant information in the submap point cloud after downsampling by using a filtering algorithm;

[0099] A judgment unit is used to judge whether the submap point cloud after removing redundant information meets the data dimension requirements;

[0100] If the requirements are met, the method of normalizing the unit is executed;

[0101] Otherwise, randomly add insufficient points from the submap point cloud after removing redundant information, and then execute the normalization unit method;

[0102] Normalization unit, used to normalize the submap point cloud.

[0103] Furthermore, the neural network model is a pointnetvlad model.

[0104] Furthermore, the sequence matching module includes:

[0105] A storage unit, used to store the corresponding features of n submap point clouds as a frame sequence;

[0106] A distance matrix calculation unit is used to calculate the frame sequence based on the prior position information using the distance equation to obtain a distance matrix, wherein the element of each position in the distance matrix is ​​the difference value between the corresponding features of the two submap point clouds;

[0107] The matching unit is used to find the best result matching the corresponding features of a single submap point cloud based on the distance matrix to obtain the local matching result;

[0108] The output unit is used to find continuous matching strings in the distance matrix based on the local matching results and output the sequence matching results.

[0109] It is worth noting that in this embodiment, the specific content of the SLAM loop detection device is similar to the content of the SLAM loop detection method disclosed in the above embodiment, and will not be repeated here.

[0110] In addition, another embodiment further discloses a storage medium, which stores a computer-readable program. When the computer-readable program is executed by one or more processors, the above-mentioned SLAM loop detection method is implemented.

[0111] Slam technology plays an important role in autonomous driving, providing mapping and positioning services for autonomous driving, and currently also participates in the truth processing of perception annotation, so globally consistent and accurate pose estimation is very necessary. Loop closure detection is a key technology to ensure global consistency, thereby effectively reducing the cumulative error in slam and obtaining a map with better accuracy.

[0112] The present invention discloses a method, device and storage medium for SLAM loop detection, which have the following beneficial effects:

[0113] First, the submap point cloud is preprocessed to remove redundant and invalid point information.

[0114] Second, potential closed loops are further screened through sequence matching to effectively reduce misidentification and improve the accuracy and recall rate of closed loops.

[0115] Third, the performance of laser SLAM loop detection is improved, which effectively reduces the cumulative error in SLAM and improves the accuracy of SLAM mapping, making the map and positioning in autonomous driving more accurate.

[0116] It should be understood that the various techniques described herein can be implemented in combination with hardware or software, or a combination thereof. Thus, the method and apparatus of the present invention, or some aspects or portions of the method and apparatus of the present invention, can take the form of program codes (i.e., instructions) embedded in a tangible medium, such as a floppy disk, a CD-ROM, a hard disk drive, or any other machine-readable storage medium, wherein when the program is loaded into a machine such as a computer and executed by the machine, the machine becomes a device for practicing the present invention.

[0117] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for illustrating the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which shall fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. The method of SLAM loop detection is characterized in that: include: Step S1: Collect submap point cloud, which is composed of several point clouds; Step S2: pre-processing the submap point cloud to remove interference information; Step S3: Use the trained neural network model to extract features from the preprocessed submap point cloud; Step S4: Sequentially match the corresponding features of the submap point cloud with the prior position information; Step S5: Find a reasonable closed loop based on the sequence matching results.

2. The method according to claim 1, characterized in that The step S2 comprises: Step S2.1: downsampling the submap point cloud; Step S2.2: Use a filtering algorithm to remove redundant information in the submap point cloud after downsampling; Step S2.3: Determine whether the submap point cloud after removing redundant information meets the data dimension requirements; If the requirements are met, the submap point cloud is normalized; Otherwise, the insufficient points are randomly supplemented from the submap point cloud after removing redundant information, and then normalized.

3. The method according to claim 1, characterized in that The neural network model is a pointnetvlad model.

4. The method according to claim 1, characterized in that: The step S4 comprises: Step S4.1: store the corresponding features of n submap point clouds as a frame sequence; Step S4.2: Based on the prior position information, the frame sequence is calculated using the distance equation to obtain a distance matrix, where the element of each position in the distance matrix is ​​the difference value between the corresponding features of the two submap point clouds; Step S4.3: Based on the distance matrix, find the best result matching the corresponding features of a single submap point cloud to obtain the local matching result; Step S4.4: Based on the local matching results, find continuous matching strings in the distance matrix and output the sequence matching results.

5. The device for slam loop detection is characterized by: include: Point cloud acquisition module, used to collect submap point cloud, which is composed of several point clouds; Point cloud preprocessing module, used to preprocess the submap point cloud to remove interference information; Point cloud feature extraction module, used to extract features from the preprocessed submap point cloud using the trained neural network model; The sequence matching module is used to perform sequence matching between the corresponding features of the submap point cloud and the prior position information; The closed loop determination module is used to find a reasonable closed loop based on the sequence matching results.

6. The device according to claim 5, characterized in that The point cloud preprocessing module includes: Downsampling unit, used to downsample the submap point cloud; The elimination unit is used to eliminate redundant information in the submap point cloud after downsampling by using a filtering algorithm; A judgment unit is used to judge whether the submap point cloud after removing redundant information meets the data dimension requirements; If the requirements are met, the method of normalizing the unit is executed; Otherwise, randomly add insufficient points from the submap point cloud after removing redundant information, and then execute the normalization unit method; Normalization unit, used to normalize the submap point cloud.

7. The device according to claim 5, characterized in that The neural network model is a pointnetvlad model.

8. The device according to claim 5, characterized in that The sequence matching module comprises: A storage unit, used to store the corresponding features of n submap point clouds as a frame sequence; A distance matrix calculation unit is used to calculate the frame sequence based on the prior position information using the distance equation to obtain a distance matrix, wherein the element of each position in the distance matrix is ​​the difference value between the corresponding features of the two submap point clouds; The matching unit is used to find the best result matching the corresponding features of a single submap point cloud based on the distance matrix to obtain the local matching result; The output unit is used to find continuous matching strings in the distance matrix based on the local matching results and output the sequence matching results.

9. A storage medium, characterized in that The storage medium stores a computer-readable program, and when the computer-readable program is executed by one or more processors, the method for SLAM loop detection according to any one of claims 1 to 4 is implemented.

Images