Loop detection method, system, medium and device based on point cloud geometric information

Abstract

The present invention belongs to the field of autonomous navigation technology of mobile robots. A loop detection method, system, medium and device based on point cloud geometric information are provided. For each scan, the laser radar point cloud obtained is aggregated into a local map according to the posture estimated by the front-end odometer, and the local map point cloud is represented in the form of NDT; the geometric values ​​of voxel units at different heights are weighted and summed to convert them into a two-dimensional image representation; ORB descriptors are extracted on the two-dimensional image, and feature matching and storage are performed in an existing database to obtain N loop candidate local maps; geometric verification is performed to determine the best loop, point cloud registration is performed, and the rigid body transformation matrix between the obtained local maps is added as a loop factor to the factor graph for optimization to obtain a global consistency map; the present invention is applicable to a variety of indoor or outdoor sensor scanning modes, and improves the consistency and accuracy of the point cloud map.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous navigation of mobile robots, and in particular to a loop detection method, system, medium and equipment based on point cloud geometric information. Background Art

[0002] The statements in this section merely provide background art related to the present invention and do not necessarily constitute prior art.

[0003] With the continuous improvement of artificial intelligence, mobile robots with autonomous navigation capabilities are increasingly widely used in various fields such as industrial logistics, indoor services, and campus inspections. Laser SLAM (Simultaneous Localization and Mapping) technology has become one of the basic technologies in mobile robot applications due to its advantages such as high measurement accuracy, strong environmental adaptability, and good robustness. Loop detection is used to correct the drift error generated by the front-end odometer, and plays a pivotal role in the entire SLAM system, helping to build globally consistent trajectories and maps.

[0004] The current loop detection methods mainly include: (1) methods based on odometer position: when the cumulative pose error is large, this type of method may fail to detect some key frames that are far away using the nearest neighbor search, resulting in invalid loop closure; (2) methods assisted by external sensors: using external sensors such as GPS to improve positioning accuracy not only increases equipment costs, but also GPS is often unavailable in indoor scenes; (3) methods based on descriptors: some descriptors mainly use information such as the maximum height, point cloud density, and intensity of the point cloud. The method of extracting the maximum height information of the point cloud often fails in indoor environments. The point cloud density will vary at different sensor distances, and the intensity value of the point cloud will be affected by environmental conditions. In addition, the point cloud intensity values ​​collected by different lidar sensors may also vary, thus affecting accurate location recognition.

[0005] Existing loop detection methods have the following limitations: (1) Information loss: Most existing loop detection methods are mainly designed for mechanical lidar. Solid-state lidar may miss key landmark information due to its limited field of view, resulting in missed detection and false alarms of loop detection; (2) Scene limitation: Most existing descriptors are designed for outdoor scenes and may not be well suited for some indoor scenes, such as underground parking lots. At the same time, some existing loop detection methods have poor robustness in dynamic scenes; (3) Dependence on dense point clouds: Most of the current loop detection methods rely on dense point cloud data to extract features, which may cause existing methods to perform poorly when processing lightweight maps and increase storage costs and data transmission pressure; (4) Sensitivity to viewpoint changes: Although some methods based on global descriptors solve the problem of rotational invariance, large lateral offsets may lead to loop omissions. Summary of the invention

[0006] In order to address the deficiencies in the prior art, the present invention provides a loop detection method, system, medium and device based on point cloud geometric information, which is suitable for a variety of indoor or outdoor sensor scanning modes and improves the consistency and accuracy of point cloud maps.

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

[0008] In a first aspect, the present invention provides a loop closure detection method based on point cloud geometric information.

[0009] A loop detection method based on point cloud geometric information includes the following processes:

[0010] For the LiDAR point cloud obtained by continuous accumulation scanning, a local map is generated according to the pose estimated by the robot's front-end odometer, and the point cloud of the local map is represented by normal distribution transformation;

[0011] According to the normal distribution transformation representation of the local map point cloud, the geometric value of the point cloud in each voxel unit is calculated, and the geometric values ​​of the voxel units with different heights are weighted summed to generate a two-dimensional image;

[0012] Extracting feature descriptors on the two-dimensional image, performing feature matching and storage in an existing database, obtaining multiple loop candidate local maps, and performing geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop;

[0013] The two-dimensional rigid body transformation matrix is ​​used as the initial estimate of the three-dimensional point cloud fine registration, and the six-degree-of-freedom rigid body transformation matrix obtained by the registration is added to the factor graph as a loop constraint factor for optimization to obtain a global consistency map.

[0014] In a second aspect, the present invention provides a loop detection system based on point cloud geometric information.

[0015] A loop detection system based on point cloud geometric information, comprising:

[0016] The local map generation unit is configured to: generate a local map for the laser radar point cloud obtained by accumulating continuous scanning according to the pose estimated by the robot front end odometer, and use normal distribution transformation to represent the point cloud of the local map;

[0017] The two-dimensional image generation unit is configured to: calculate the geometric value of the point cloud in each voxel unit according to the normal distribution transformation representation of the local map point cloud, perform weighted summation on the geometric values ​​of voxel units with different heights, and generate a two-dimensional image;

[0018] The best loop generation unit is configured to: extract feature descriptors on the two-dimensional image, perform feature matching and storage in an existing database, obtain multiple loop candidate local maps, and perform geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop;

[0019] The consistency map generation unit is configured to: use the two-dimensional rigid body transformation matrix as an initial estimate of the three-dimensional point cloud fine alignment, add the six-degree-of-freedom rigid body transformation matrix obtained by alignment as a loop constraint factor to the factor graph for optimization, and obtain a global consistency map.

[0020] In a third aspect, the present invention provides a computer device, comprising: a processor and a computer-readable storage medium;

[0021] a processor adapted to execute a computer program;

[0022] A computer-readable storage medium, wherein a computer program is stored in the computer-readable storage medium, and when the computer program is executed by the processor, the loop detection method based on point cloud geometric information as described in the first aspect of the present invention is implemented.

[0023] In a fourth aspect, the present invention provides a computer-readable storage medium storing a computer program, wherein the computer program is suitable for being loaded by a processor and executing the loop detection method based on point cloud geometric information as described in the first aspect of the present invention.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] 1. The present invention is not directly based on a single LiDAR scan, but uses the geometric information of the local map generated by the local consistent odometer estimation to detect loops. The present invention can overcome the instability problem that may exist in a single LiDAR scan data, and can better capture environmental information and make up for the limitation of the small field of view angle of the solid-state LiDAR.

[0026] 2. The present invention not only focuses on the structure of the highest point, but also captures the environmental information more comprehensively by considering the geometric information of the point cloud in voxels at different heights, thereby providing a more stable description of the environment. This makes the method not only suitable for outdoor scenes, but also for indoor scenes such as underground parking lots. At the same time, the present invention places more emphasis on the feature information of static structures, such as walls and columns, which helps the system to identify stable features in the presence of dynamic obstacles and improve the accuracy of loop detection in dynamic environments.

[0027] 3. The present invention generates descriptors directly from NDT (normal distribution variation) maps without dense point clouds, making the present invention suitable for processing lightweight maps, while reducing the demand for onboard storage and the pressure of data transmission, and reducing maintenance costs, making it more efficient when processing large-scale scenes.

[0028] 4. The present invention performs ORB feature detection on a two-dimensional image generated by point cloud geometry information. These ORB features are rotational and translational invariant, so they can still be correctly matched under different viewpoints, thereby effectively detecting loops.

[0029] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0031] Figure 1 A schematic diagram of the principle of a loop closure detection method based on point cloud geometric information provided in Example 1 of the present invention;

[0032] Figure 2 A schematic diagram of the field of view angle of the mechanical laser radar provided in Example 1 of the present invention;

[0033] Figure 3 A schematic diagram of the field of view angle of the solid-state laser radar provided in Example 1 of the present invention;

[0034] Figure 4 A schematic diagram of the HBST data structure provided in Example 1 of the present invention;

[0035] Figure 5 A specific flow chart of the local map loop detection and verification process provided in Example 1 of the present invention;

[0036] Figure 6A schematic diagram of a loop detection system based on point cloud geometric information provided by Embodiment 2 of the present invention;

[0037] Figure 7 A schematic diagram of a computer device provided in Example 3 of the present invention. DETAILED DESCRIPTION

[0038] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0039] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0040] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0041] Embodiment 1:

[0042] This implementation proposes a loop detection method based on point cloud geometric information, such as Figure 1 As shown in the figure, for each scan, the original point cloud is accumulated by the lidar point cloud obtained, and the pose estimated by the front-end odometer is combined into a local map, and the local map point cloud is represented in the form of NDT (normal distribution change) and converted into a two-dimensional form; then, the geometric value of the point cloud in each voxel is calculated, and the geometric values ​​of voxel units at different heights are weighted and summed to generate a two-dimensional image; the ORB descriptor is extracted on the two-dimensional image, and feature retrieval and addition are performed in the existing database to determine whether a match is found. If so, N candidate local maps are obtained, otherwise the process ends; on this basis, geometric verification is performed to detect the validity of the loop (that is, determine whether it is a loop, if so, continue to determine the best loop, if not, the process ends); finally, point cloud registration is performed, and the rigid body transformation matrix between the local maps is added to the factor graph as a loop factor for optimization to obtain a global consistency map (that is, a 6DoF pose map).

[0043] More specifically, the process includes the following:

[0044] S1: Raw point cloud processing.

[0045] Many existing loop closure detection methods use a single LiDAR scan data, such as Figure 2 and Figure 3 It can be seen that the field of view of solid-state laser radar is much smaller than that of mechanical laser radar, resulting in limited environmental information. Therefore, the present invention uses the geometric information of the local map to detect loops to obtain richer and more stable environmental information.

[0046] On the one hand, we use the estimated pose obtained from the front-end odometer to build a local map by accumulating continuous scans. Since the relative pose estimate provided by the front-end odometer has local consistency, the generated local point cloud map will not have too much error. , in the radar coordinate system corresponding to each scan, by converting all point clouds to the radar coordinate system corresponding to the i-th scan, the converted point cloud is recorded as , ,in, , indicating the i The pose of the scan in the global coordinate system is estimated to generate a local point cloud map, where It is a special European group.

[0047] On the other hand, since the NDT (normal distribution variation) method has good robustness to noise and outliers in the point cloud, it is suitable for processing sparse point cloud data, and still maintains good performance and advantages in processing speed when processing large-scale scenes, the present invention represents the local map point cloud in the form of NDT (normal distribution variation), that is, the point cloud is divided into a number of voxels according to a certain resolution, and the Gaussian distribution of the point cloud in each voxel is calculated. t It is generated by converting the laser radar point cloud scanned multiple times in succession to the coordinate system corresponding to the first scan in the map, where t is the index of the local map, which is arranged in chronological order. While transforming the coordinates of each point, the voxel where the point is located is calculated based on its transformed three-dimensional coordinates, and then the mean and covariance matrix of each voxel are updated:

[0048] (1);

[0049] (2)

[0050] (3);

[0051] in, represents the three-dimensional coordinates of the i-th point, represents the mean value after the i-th point in the same voxel is added, and is a 3×3 matrix. Represents the same voxel in the front i The covariance matrix of each point is obtained by using a single-pass algorithm to update the mean and covariance matrix of each voxel online, which can improve computational efficiency, reduce memory usage, and is suitable for processing large-scale data sets and dynamically changing environments.

[0052] In addition, the local map M tThe size of is determined by the distance traveled by the mobile robot, rather than accumulating a fixed number of continuous scans. Exceeding a certain threshold As a judgment standard, continuous scans are accumulated to build a local map. Here n represents the number of scans contained in the local map. Since the local map is built based on the driving distance, the number of lidar scans contained in each local map is different, that is, n is not a fixed value. is the pose estimate of the i-th scan in the global coordinate system T i The translation component of It is The pose estimate of the scan in the global coordinate system T i This can avoid accumulating too much duplicate data when the robot moves a small distance. By setting the threshold reasonably, it can ensure that the local map contains enough information to support the loop detection task, and will not cause low processing efficiency due to excessive data volume.

[0053] S2: 2D image generation.

[0054] After obtaining the NDT representation of the entire local map point cloud, the geometric information of the point clouds in different voxels is distinguished by calculating the geometric value of each voxel. For each given voxel unit, the eigenvalue is calculated according to its covariance matrix Generally speaking, if , then the point cloud in the unit is considered to be a straight line; if , then the point cloud in the unit is considered to be a plane.

[0055] In order to avoid the limitation of traditional methods that require setting two thresholds ( and The ratio of and The present invention proposes a one-dimensional NDT geometry classification index based on the eigenvalue:

[0056] (4);

[0057] in, is a threshold close to zero to handle Infinitely close to zero or equal to zero.

[0058] Therefore, by performing a direct threshold judgment on the indicator ratio, the geometric information of the point cloud in different voxels can be effectively distinguished. In order to clearly describe the geometric information of the point cloud, the ratio value is mapped to different geometric values ​​using formula (5), that is, the ratio value within the specified range is divided into segments of equal size:

[0059] (5);

[0060] in, represents a set of integers, represents the maximum ratio value selected, d represents the length of each selected segment, S represents the geometric value, which is an integer. In this way, the three-dimensional coordinate value (X, Y, Z) and the corresponding geometric value S of each voxel unit are obtained.

[0061] The calculation formula for the voxel coordinates (X, Y, Z) of each point in the local map is as follows:

[0062] (6);

[0063] (7);

[0064] (8);

[0065] in, is the 3D coordinate of the mth point in the local map, h is the height of the laser radar from the ground, and v represents the size of the voxel unit. is the floor function.

[0066] Then, the present invention converts the three-dimensional local map represented by NDT into a two-dimensional image for processing. For ground mobile robots, the XY plane of its local map coordinate system is almost parallel to the ground, so the geometric values ​​of the voxels with the same (X, Y) coordinates are directly weighted and summed. In the case of large changes in roll and pitch angles, the ground can be segmented first, the ground normal vector can be extracted, and the local map point cloud can be uniformly converted to a coordinate system parallel to the ground for processing. The three-dimensional point cloud data is converted into a two-dimensional form for processing, which retains the local two-dimensional geometric information. Since the processing dimension is reduced, the computational complexity of the algorithm is also reduced. In addition, the present invention does not consider voxel units that are far away to avoid the influence of noise and outliers, and only performs weighted summation on the geometric values ​​of voxels within a specified height range. By assigning different weights to the geometric values ​​according to the voxel height, the distinction of the geometric information of point clouds at different locations is increased:

[0067] (9);

[0068] in, represents the number of voxels in the Z direction, represents the geometric value of each voxel, Represents the weighted sum of the geometric values ​​of voxels with the same (X, Y) coordinate.

[0069] In this way, in the Cartesian coordinate system, the three-dimensional local map represented by NDT is converted into a two-dimensional image. Each two-dimensional grid stores the weighted sum of geometric values. t The grayscale value of a two-dimensional image is defined as follows:

[0070] (10);

[0071] (11);

[0072] (12);

[0073] in, is the dimension of the generated two-dimensional image, which depends on the size of the local point cloud map. max and min are the maximum and minimum functions respectively. represents the maximum value of geometric weighted value, Represents the minimum value of the geometric weight.

[0074] Will The image grayscale values ​​below 5% are set to zero, which helps to reduce the influence of noise and ground points in the local map. The image grayscale values ​​are then converted to integers ranging from 0 to 255 to facilitate feature detection on the two-dimensional image in the subsequent process.

[0075] S3: Feature detection and matching.

[0076] After obtaining the two-dimensional image of the local map, the key points in the image are detected and the descriptors of these key points are calculated to obtain the local map M. t A set of related ORB descriptors. In order to speed up the calculation, the scale invariance of the ORB descriptor is not considered here, because the two-dimensional image is an orthographic projection of the 3D world and there is no scale ambiguity; at the same time, the detection of feature points is not affected by changes in viewing angles, which makes feature matching more stable and reliable. For ORB descriptors, the Hamming distance is usually used to measure the difference between two descriptors for effective feature storage and matching.

[0077] The present invention uses a binary search tree (HBST) embedded with Hamming distance to store the ORB descriptor set D obtained from each two-dimensional image. t And the corresponding local map index t, which can search and insert descriptors in logarithmic time. The schematic diagram of the HBST data structure is as follows Figure 4As shown. Each leaf node stores a subset of all descriptors, and each intermediate node has two child nodes and stores an index , where dim(d) is the dimension of the descriptor, and the left child node stores the kth i The descriptor with bit 0, the right child node stores the kth i Descriptors with bit 1. The depth of HBST is bounded by the number of bits in the ORB descriptor (256 bits), which means that a query descriptor is compared with the intermediate nodes of the tree at most 256 times before terminating at a leaf node.

[0078] More specifically, Figure 4 In the picture k i ( k 1. k 2. k 3) Indicates the index bit selected by the non-leaf node, a , b , c is a specific bit of the ORB descriptor, for example, d[k1=a]=0 means a The descriptor with bit 0 is divided into the left subtree, and d[k1=a]=1 means the first a The descriptor with bit 1 is divided into the right subtree; d[k2=b]=0 means the b The descriptor with bit 0 is divided into the left subtree, and d[k2=b]=1 means the first b The descriptor with bit 1 is divided into the right subtree; d[k3=c]=0 means the c The descriptor with bit 0 is divided into the left subtree, and d[k3=c]=1 means the first c Descriptors with bit 1 are assigned to the right subtree; leaf nodes store a series of ORB descriptors and their corresponding local map index IDs (e.g., ID1ORB descriptor 1, ID1ORB descriptor 2, and ID1ORB descriptor 3).

[0079] In addition, each leaf node can store up to 100 descriptors. By limiting these two design parameters, the computation time of the feature matching process can be effectively limited, and the efficiency of the algorithm can be improved without affecting the overall performance of loop detection. When a leaf node reaches its maximum storage capacity, the leaf node is converted to an intermediate node, and all the descriptors it previously contained are divided into two new leaf nodes. In order to construct a balanced binary tree as much as possible to balance the search time and the completeness of the search results, the index k of the intermediate node is selected. i When , the bit whose average value μ of the index is closest to 0.5 is selected to achieve uniform division of the non-leaf node descriptors:

[0080] (13);

[0081] in, Indicates that the jth descriptor is in the kth i The value in the bit, N is the number of descriptors stored in the node.

[0082] In obtaining a set of ORB descriptors D from the 2D image of the query local map q After that, for each descriptor, it traverses the entire tree from the root node to the leaf node, scans all the descriptors stored in this leaf node in turn to find its nearest neighbor match, and inserts itself into this leaf node. The Hamming distance between the two matching descriptors must be less than the set threshold.

[0083] Since the database stores ORB descriptors and corresponding local map indexes, a voting mechanism can be designed for retrieval. For each matching descriptor, the system will vote for the local map index to which it belongs. After processing all query descriptors, the system will count the number of votes obtained by each local map index. According to the voting results, the top N local maps with the most votes are selected as loop candidate local maps for further loop verification. Through the above process, a series of mutually matching ORB descriptors between the query local map and each loop candidate local map are obtained, and geometric verification is performed on this basis.

[0084] S4: Perform geometric verification on the top N loop candidate local maps with the largest number of matching descriptors.

[0085] S4.1: Determine whether the matching local maps meet the time difference requirement to avoid misjudging temporally adjacent local maps as loop pairs. This can be determined by the difference between local map indexes. That is, subsequent verification can only be performed when the index difference is greater than a certain threshold. This requires that the local map indexes are arranged in order over time.

[0086] S4.2: After the time difference requirement is met, the matching descriptors and their number are obtained according to the index of the loop candidate local map, and then it is determined whether the number of matching descriptors is greater than the set threshold. If the number requirement is met, the subsequent verification step is performed;

[0087] S4.3: For the query local map and each loop candidate local map, the present invention uses a registration strategy based on the RANSAC algorithm to estimate a two-dimensional rigid body transformation, which can best align the matching features between the two-dimensional images and exclude some possible false matches, thereby verifying the validity of the matching; the specific implementation process is as follows:

[0088] (1) Randomly select two point pairs from the matched key point pairs as sample point pairs for one iteration, and use the Kabsch-Umeyama algorithm to solve the two-dimensional rigid body transformation matrix.

[0089] (2) Calculate the distance error between the remaining key point pairs under the action of the rigid body transformation matrix. If the distance error between the point pairs is less than the set threshold, the point pair is an internal point in the sample, that is:

[0090] (14);

[0091] in, and For two local Figure 2 The i-th pair of key points that match each other in the dimensional image, Candidate local area Figure 2 The key point coordinates in the dimensional image, To query local Figure 2 The coordinates of the key points in the dimensional image, T is the rigid body transformation matrix, d is the set distance threshold, and the number of internal points under the rigid body transformation is counted;

[0092] (3) Repeat the above steps until the upper limit of the number of iterations is reached, count the number of interior points under different rigid body transformations, and take the maximum number of interior points as the optimal solution;

[0093] (4) Using all the inlier point pairs under the rigid body transformation with the largest number of inliers, estimate the rigid body transformation again, and use this as the final two-dimensional rigid body transformation matrix, and return the number of inliers;

[0094] S4.4: After performing the RANSAC alignment between key points, a series of two-dimensional rigid body transformation matrices and the number of inliers between mutually matching local maps are obtained. The two-dimensional rigid body transformation with the largest number of inliers is selected as the initial estimate for the subsequent point cloud fine alignment. The corresponding loop candidate local map is the best loop. Of course, the number of inliers needs to exceed a certain threshold. When generating a two-dimensional image, due to the use of a two-dimensional grid, the original point cloud data is discretized to a certain extent. In order to eliminate the influence of this discretization on the translation amount, the calculated translation amount is scaled according to the grid size, so as to more accurately represent the actual translation amount.

[0095] The specific process of local map loop detection and verification is as follows Figure 5 As shown, the following detailed process is included:

[0096] Generate a two-dimensional image, detect key points and calculate ORB descriptors, perform matching and adding operations in the database, obtain the N candidate local maps with the largest number of matching descriptors, and determine whether the matching local map (i.e., candidate local map) meets the time difference requirement. If not, return the initial value (without any processing); if so, obtain the matching descriptor according to the candidate map index, and determine whether the number of matching descriptors is greater than the threshold. If not, return the initial value (without any processing). If so, obtain the matching key point pair, RANSAC calculates the two-dimensional rigid body transformation matrix, returns the candidate local map index, the two-dimensional rigid body transformation matrix and the number of inliers, and the local map matching item with the largest number of inliers is the best loop.

[0097] S5: Point cloud fine registration.

[0098] For ground mobile robots, the changes in the Z-axis direction and the changes in the roll and pitch angles are generally not too large, so the two-dimensional rigid body transformation matrix obtained in the above process can be used as a good initial estimate for the fine registration of 3D point clouds; using the ICP algorithm for fine point cloud registration, a good initial estimate can reduce the number of iterations and speed up the convergence speed, thereby improving the efficiency of registration. The six-degree-of-freedom rigid body transformation matrix obtained by fine point cloud registration is added to the factor graph as a loop constraint factor for optimization to generate a globally consistent trajectory and map.

[0099] Embodiment 2:

[0100] like Figure 6 As shown, this implementation provides a loop detection system based on point cloud geometric information, including:

[0101] The local map generation unit is configured to: generate a local map for the laser radar point cloud obtained by accumulating continuous scanning according to the pose estimated by the robot front end odometer, and use normal distribution transformation to represent the point cloud of the local map;

[0102] The two-dimensional image generation unit is configured to: calculate the geometric value of the point cloud in each voxel unit according to the normal distribution transformation representation of the local map point cloud, perform weighted summation on the geometric values ​​of voxel units with different heights, and generate a two-dimensional image;

[0103] The best loop generation unit is configured to: extract feature descriptors on the two-dimensional image, perform feature matching and storage in an existing database, obtain multiple loop candidate local maps, and perform geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop;

[0104] The consistency map generation unit is configured to: use the two-dimensional rigid body transformation matrix as an initial estimate of the three-dimensional point cloud fine alignment, add the six-degree-of-freedom rigid body transformation matrix obtained by alignment as a loop constraint factor to the factor graph for optimization, and obtain a global consistency map.

[0105] The specific working process of each unit is described in Example 1 and will not be repeated here.

[0106] It is understandable that the above-mentioned units can be separately or completely combined into one or several other units to constitute, or one (some) of the units can be further divided into multiple smaller units in function to constitute, which can achieve the same operation without affecting the realization of the technical effects of the embodiments of the present application. The above-mentioned units are divided based on logical functions. In practical applications, the functions of one unit can also be implemented by multiple units, or the functions of multiple units can be implemented by one unit. In other embodiments of the present application, the system may also include other units. In practical applications, these functions can also be implemented with the assistance of other units, and can be implemented by the collaboration of multiple units.

[0107] According to another embodiment of the present application, the system described in this embodiment can be constructed, and the method of Example 1 of the present application can be implemented by running a computer program (including program code) capable of executing the steps involved in the corresponding method described in Example 1 on a general-purpose computing device such as a computer, which includes processing elements and storage elements such as a central processing unit (CPU), random access memory (RAM), and read-only memory (ROM). The computer program can be recorded on, for example, a computer-readable recording medium, and loaded into the above-mentioned computing device through the computer-readable recording medium and run therein.

[0108] Embodiment 3:

[0109] like Figure 7 As shown, this implementation provides an electronic device, which includes a processor 1001, a communication interface 1002, and a computer-readable storage medium 1003. The processor 1001, the communication interface 1002, and the computer-readable storage medium 1003 may be connected via a bus or other means.

[0110] Among them, the communication interface 1002 is used to receive and send data, the computer-readable storage medium 1003 can be stored in the memory of the electronic device, the computer-readable storage medium 1003 is used to store a computer program, the computer program includes program instructions, and the processor 1001 is used to execute the program instructions stored in the computer-readable storage medium 1003.

[0111] The processor 1001 (or CPU (Central Processing Unit)) is the computing core and control core of the electronic device, which is suitable for implementing one or more instructions, and specifically suitable for loading and executing one or more instructions to implement corresponding method processes or corresponding functions.

[0112] The processor 1001 is configured to execute the following process:

[0113] For the LiDAR point cloud obtained by continuous accumulation scanning, a local map is generated according to the pose estimated by the robot's front-end odometer, and the point cloud of the local map is represented by normal distribution transformation;

[0114] According to the normal distribution transformation representation of the local map point cloud, the geometric value of the point cloud in each voxel unit is calculated, and the geometric values ​​of the voxel units with different heights are weighted summed to generate a two-dimensional image;

[0115] Extracting feature descriptors on the two-dimensional image, performing feature matching and storage in an existing database, obtaining multiple loop candidate local maps, and performing geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop;

[0116] The two-dimensional rigid body transformation matrix is ​​used as the initial estimate of the three-dimensional point cloud fine registration, and the six-degree-of-freedom rigid body transformation matrix obtained by the registration is added to the factor graph as a loop constraint factor for optimization to obtain a global consistency map.

[0117] The specific working process is described in Example 1 and will not be repeated here.

[0118] Embodiment 4:

[0119] This implementation provides a computer-readable storage medium (Memory), which is a memory device in an electronic device for storing programs and data. It is understandable that the computer-readable storage medium here can include both built-in storage media in the electronic device and, of course, extended storage media supported by the electronic device. The computer-readable storage medium provides a storage space that stores the processing system of the electronic device.

[0120] In addition, the storage space also stores one or more instructions suitable for being loaded and executed by the processor, and these instructions may be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here may be a high-speed RAM memory, or a non-volatile memory, such as at least one disk memory; optionally, it may also be at least one computer-readable storage medium located away from the aforementioned processor.

[0121] In one embodiment, the computer-readable storage medium stores one or more instructions; the processor loads and executes the one or more instructions stored in the computer-readable storage medium to implement the following process:

[0122] For the LiDAR point cloud obtained by continuous accumulation scanning, a local map is generated according to the pose estimated by the robot's front-end odometer, and the point cloud of the local map is represented by normal distribution transformation;

[0123] According to the normal distribution transformation representation of the local map point cloud, the geometric value of the point cloud in each voxel unit is calculated, and the geometric values ​​of the voxel units with different heights are weighted summed to generate a two-dimensional image;

[0124] Extracting feature descriptors on the two-dimensional image, performing feature matching and storage in an existing database, obtaining multiple loop candidate local maps, and performing geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop;

[0125] The two-dimensional rigid body transformation matrix is ​​used as the initial estimate of the three-dimensional point cloud fine registration, and the six-degree-of-freedom rigid body transformation matrix obtained by the registration is added to the factor graph as a loop constraint factor for optimization to obtain a global consistency map.

[0126] The specific working process is described in Example 1 and will not be repeated here.

[0127] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A loop detection method based on point cloud geometric information, characterized in that: The process includes: For the LiDAR point cloud obtained by continuous accumulation scanning, a local map is generated according to the pose estimated by the robot's front-end odometer, and the point cloud of the local map is represented by normal distribution transformation; According to the normal distribution transformation representation of the local map point cloud, the geometric value of the point cloud in each voxel unit is calculated, and the geometric values ​​of the voxel units with different heights are weighted summed to generate a two-dimensional image; Extracting feature descriptors on the two-dimensional image, performing feature matching and storage in an existing database, obtaining multiple loop candidate local maps, and performing geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop; The two-dimensional rigid body transformation matrix is ​​used as the initial estimate of the three-dimensional point cloud fine registration, and the six-degree-of-freedom rigid body transformation matrix obtained by the registration is added to the factor graph as a loop constraint factor for optimization to obtain a global consistency map.

2. The loop detection method based on point cloud geometric information as claimed in claim 1, characterized in that: When the distance moved by the robot exceeds the set threshold, it is regarded as a continuous scan, and the lidar point cloud is obtained based on the accumulated results of multiple continuous scans.

3. The loop detection method based on point cloud geometric information as claimed in claim 1, characterized in that: For each given voxel unit, calculating an eigenvalue according to the covariance matrix of the voxel unit; Calculate the geometric shape classification index ratio of the one-dimensional normal distribution transformation according to the eigenvalue; Map the geometric shape classification index ratio to different geometric values: , Indicates the maximum geometric shape classification index ratio value selected, represents the set of integers, d Indicates the length of each selected segment, S represents the geometry value, which is an integer. Express Perform a rounding operation.

4. The loop detection method based on point cloud geometric information as claimed in claim 3, characterized in that: The geometric values ​​of voxels within the set height range are weighted and summed, and different weights are assigned to the geometric values ​​according to the voxel height to increase the distinction of geometric information of point clouds at different locations. ,in, represents the number of voxels in the Z direction, represents the geometric value of each voxel, Represents the weighted sum of the geometric values ​​of voxels with the same (X, Y) coordinate; Transform the normal distribution into a three-dimensional local map Converted into a two-dimensional image, each two-dimensional grid of the two-dimensional image stores the weighted sum of the geometric values, and the grayscale value of the two-dimensional image is: , , , is the dimension of the generated two-dimensional image, represents the maximum value of geometric weighted value, Represents the minimum value of geometric weight; Will The image grayscale values ​​below 5% are set to zero, and then the image grayscale values ​​are converted to integers from 0 to 255 to obtain the final two-dimensional image.

5. The loop detection method based on point cloud geometric information as claimed in claim 1, characterized in that: By detecting key points in the two-dimensional image and calculating the descriptors of the key points, a set of ORB descriptors related to the local map is obtained, and a binary search tree embedded with Hamming distance is used to store the ORB descriptor set obtained from each two-dimensional image. D t and the corresponding local map index t; Each leaf node of the binary search tree stores a subset of all ORB descriptors, and each intermediate node has two child nodes and stores an index ,in is the dimension of the ORB descriptor, where the left child node stores the k i The descriptor with bit 0, the right child node stores the k i Descriptor with bit 1; The depth of the binary search tree is bounded by the number of bits of the ORB descriptor, 256. Each leaf node can store up to 100 descriptors. When a leaf node reaches its maximum storage capacity, it is converted to an intermediate node. All ORB descriptors previously contained in the leaf node are divided into two new leaf nodes. k i When , the bit whose average value of the index is closest to 0.5 is selected to evenly divide the non-leaf node descriptors; After obtaining a set of query descriptors from the two-dimensional image of the query local map, for each query descriptor, traverse the entire tree from the root node to the leaf node, scan all ORB descriptors stored in this leaf node in turn to find its nearest neighbor match, and insert itself into this leaf node. The Hamming distance between the two matching descriptors is less than the set threshold; The ORB descriptors and the corresponding local map indexes are stored in the database. A vote is performed for the local map index to which each matching ORB descriptor belongs. After all query descriptors are processed, the number of votes obtained by each local map index is counted. Based on the voting results, the top N local maps with the most votes are selected as loop candidate local maps.

6. The loop detection method based on point cloud geometric information as claimed in claim 1, characterized in that: Perform geometry validation, including: The matched local map indexes are arranged in order over time, and it is determined whether the time difference between the matched local map indexes is greater than the set threshold. If so, the time difference requirement is met and the next step is performed. Otherwise, the time difference requirement is not met and the verification ends. After the time difference requirement is met, the matching descriptors and their number are obtained according to the index of the loop candidate local map, and then it is determined whether the number of matching descriptors is greater than the set threshold. If the number requirement is met, the next step is entered, otherwise, the verification ends; For the query local map and each loop candidate local map, a registration strategy based on the RANSAC algorithm is used to estimate the two-dimensional rigid body transformation to align the matching features between the two-dimensional images, and obtain a series of two-dimensional rigid body transformation matrices and the number of internal points between the mutually matching local maps; The two-dimensional rigid body transformation with the largest number of internal points is selected as the final two-dimensional rigid body transformation matrix, and the loop candidate local map corresponding to the final two-dimensional rigid body transformation matrix is ​​the best loop.

7. The loop detection method based on point cloud geometric information as claimed in claim 6, characterized in that: The registration strategy based on the RANSAC algorithm estimates the two-dimensional rigid body transformation, including: Randomly select two point pairs from the matched key point pairs as sample point pairs for one iteration, and use the Kabsch-Umeyama algorithm to solve the two-dimensional rigid body transformation matrix; Calculate the distance error between the remaining key point pairs under the rigid body transformation matrix. If the distance error between the point pairs is less than the set threshold, the point pair is an inlier point in the sample, and count the number of inliers in the sample under the current rigid body transformation matrix. Repeat until the upper limit of the number of iterations is reached, count the number of interior points under different rigid body transformation matrices, and take the maximum number of interior points as the optimal solution; Using all the inlier point pairs under the rigid body transformation matrix with the largest number of inliers, the rigid body transformation is estimated again, which is used as the final two-dimensional rigid body transformation matrix, and the number of inliers is returned.

8. A loop detection system based on point cloud geometric information, characterized in that: include: The local map generation unit is configured to: generate a local map for the laser radar point cloud obtained by accumulating continuous scanning according to the pose estimated by the robot front end odometer, and use normal distribution transformation to represent the point cloud of the local map; The two-dimensional image generation unit is configured to: calculate the geometric value of the point cloud in each voxel unit according to the normal distribution transformation representation of the local map point cloud, perform weighted summation on the geometric values ​​of voxel units with different heights, and generate a two-dimensional image; The best loop generation unit is configured to: extract feature descriptors on the two-dimensional image, perform feature matching and storage in an existing database, obtain multiple loop candidate local maps, and perform geometric verification to obtain a two-dimensional rigid body transformation matrix corresponding to the best loop; The consistency map generation unit is configured to: use the two-dimensional rigid body transformation matrix as an initial estimate of the three-dimensional point cloud fine alignment, add the six-degree-of-freedom rigid body transformation matrix obtained by alignment as a loop constraint factor to the factor graph for optimization, and obtain a global consistency map.

9. A computer device, characterized in that: include: A processor and a computer readable storage medium; a processor adapted to execute a computer program; A computer-readable storage medium, wherein a computer program is stored in the computer-readable storage medium, and when the computer program is executed by the processor, the loop detection method based on point cloud geometric information as described in any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which is suitable for being loaded by a processor and executing the loop detection method based on point cloud geometric information as described in any one of claims 1 to 7.

Images