Robot laser slam loop detection method and device, electronic equipment and storage medium

Abstract

The application provides a robot laser SLAM loop detection method and device, electronic equipment and storage medium, and relates to the technical field of mobile robot loop detection. The robot laser SLAM loop detection method comprises the following steps: acquiring a current frame image and first point cloud data corresponding to the current frame image; obtaining grid data corresponding to the current frame image according to the first point cloud data; obtaining a plurality of descriptors corresponding to the current frame image according to the grid data and determining a loop similarity; sorting out a candidate key frame image according to the loop similarity and obtaining a registration similarity; and judging whether the current frame image constitutes a loop according to the registration similarity. The robot laser SLAM loop detection method of the application describes the surface shape of the point cloud data based on a plurality of descriptors, does not need to rely on an additional sensor, has low computational complexity, and can guarantee that the loop detection has high precision and robustness when applied to an outdoor scene.

Description

Technical Field

[0001] This invention relates to the field of loop closure detection technology for mobile robots, and more specifically, to a robot laser SLAM loop closure detection method, apparatus, electronic device, and storage medium. Background Technology

[0002] Loop closure detection (also known as loop shut-off detection) is a technique that enables mobile robots to build map loops by determining whether the current scene is a scene that has been visited before.

[0003] Loop closure detection is an important component of laser simultaneous localization and mapping (SLAM) systems for mobile robots. Good loop closure detection can effectively solve the motion drift problem caused by the accumulation of laser odometry errors due to long time and space distances, thus helping robots to complete navigation tasks more accurately.

[0004] Existing loop closure detection methods include odometry-based verification and feature-based verification.

[0005] The methods for verifying odometer readings include:

[0006] [1] Inertial navigation odometry verification: This method is only applicable to indoor scenarios where the robot's range of motion is small, and cannot be applied to outdoor scenarios where the range of motion is large.

[0007] [2] GPS-assisted verification. This method requires the robot to be equipped with a GPS device, and the positioning accuracy is affected by the GPS signal.

[0008] Feature-based verification methods include:

[0009] [1] Manual feature verification: This method requires scene similarity judgment through manually designed environmental features. When faced with scenes with single features such as histogram or local laser point cloud height and reflection intensity, it lacks encoding description of abstract features such as surface shape, and its reliability is not high.

[0010] [2] Neural network verification: In this method, the accuracy of loop closure detection depends on the quality and quantity of training data, which increases the robot's computational requirements and leads to an increase in the overall cost of the robot.

[0011] The existing methods have the aforementioned limitations, which means that robots still face certain obstacles and defects in the process of loop closure detection.

[0012] There is currently no effective technical solution to the above problems. Summary of the Invention

[0013] The purpose of this invention is to provide a robotic laser SLAM loop closure detection method, device, electronic device, and storage medium. It describes the surface shape of point cloud data based on multiple descriptors, without relying on additional sensors and with low computational complexity. It can also ensure high accuracy and robustness of loop closure detection when applied to outdoor scenarios.

[0014] In a first aspect, the present invention provides a robotic laser SLAM loop closure detection method, comprising the following steps:

[0015] S1. Obtain the current frame image and the first point cloud data corresponding to the current frame image;

[0016] S2. When the current frame image is a keyframe image, the first point cloud data is gridded to obtain the grid data corresponding to the current frame image;

[0017] S3. Obtain the fast matching descriptor, surface feature descriptor and frequency feature descriptor corresponding to the current frame image based on the grid data, in order to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each of the suspected keyframe images and the current frame image;

[0018] S4. Based on the loop closure similarity, candidate keyframe images are selected from the multiple suspected keyframe images for registration with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image.

[0019] S5. Based on the registration similarity, determine whether the current frame image constitutes a loop closure.

[0020] The robotic laser SLAM loop closure detection method of the present invention accurately describes abstract features such as the surface shape of point cloud data using multiple descriptors, thereby enabling the search for scenes that form a closed loop with the current scene from historical scenes in a more efficient, stable, and accurate manner, thus achieving the purpose of loop closure detection.

[0021] Furthermore, the specific steps in step S1 include:

[0022] S11. Obtain the initial point cloud data corresponding to the current frame image;

[0023] S12. Perform ground removal processing on the initial point cloud data to obtain the second point cloud data;

[0024] S13. Perform voxel filtering on the second point cloud data to obtain the first point cloud data.

[0025] Preprocessing the current frame image to remove ground data and reduce noise helps to quickly match descriptors for fast search, and surface feature descriptors and frequency feature descriptors accurately describe the surface shape of point cloud data.

[0026] Furthermore, the specific steps in step S2 include:

[0027] S21. Project the first point cloud data onto a cylindrical coordinate system; the cylindrical coordinate system is composed of a polar coordinate system and a z-axis perpendicular to the polar coordinate system;

[0028] S22. Divide the polar coordinate system into multiple rings along the r-axis direction and divide each ring into multiple sectors;

[0029] S23. Determine the sector to which each point belongs based on the cylindrical coordinates of each point in the first point cloud data;

[0030] S24. Divide the Cartesian coordinate system into multiple grids along the x-axis and y-axis directions respectively; the origin of the Cartesian coordinate system is the pole of the polar coordinate system, the x-axis of the Cartesian coordinate system is the polar axis of the polar coordinate system, and the counterclockwise direction of the Cartesian coordinate system is the positive direction of the polar coordinate system.

[0031] S25. Determine the grid to which each point belongs based on the Cartesian coordinates of each point in the first point cloud data;

[0032] S26. The sector and the corresponding grid of each point in the first point cloud data are taken as the grid data.

[0033] Furthermore, the specific steps in step S3 include:

[0034] S31. Obtain the fast matching descriptor according to the following steps:

[0035] S311. Obtain the first grid value of each sector and combine all the first grid values ​​into a group of size . The first matrix; the first grid value is the z-axis coordinate of the point with the largest z-axis coordinate among all points contained in the sector;

[0036] S312. Use the first matrix as the fast matching descriptor;

[0037] S32. Obtain the surface feature descriptor according to the following steps:

[0038] S321. Calculate the mean polar coordinates of each sector according to the following formula:

[0039] ;

[0040] in, The polar coordinate mean of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. The total number of points contained in the h-th sector of the w-th ring in the grid data corresponding to the current frame image. The i-th point in the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let w be the cylindrical coordinate vector of the i-th point in the h-th sector of the w-th annulus in the grid data corresponding to the current frame image, w be the w-th annulus in the grid data corresponding to the current frame image, h be the h-th sector in each annulus in the grid data corresponding to the current frame image, W be the total number of annulus in the grid data corresponding to the current frame image, and H be the total number of sectors in each annulus in the grid data corresponding to the current frame image.

[0041] S322. Calculate the covariance matrix of each sector according to the following formula:

[0042] ;

[0043] in, The covariance matrix of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image is given. This indicates finding the transpose of the matrix;

[0044] S323. Combine the mean values ​​of all the polar coordinates into a group of size... The second matrix;

[0045] S324. Combine all the covariance matrices into a matrix of size [missing value]. The third matrix;

[0046] S325. The second matrix and the third matrix are used as the surface feature descriptors, and the polar coordinate mean and covariance matrix of the same sector conform to the following functional relationship used to describe the shape features of each sector:

[0047] ;

[0048] in, For encoding functions, The shape feature of the h-th sector in the w-th ring of the grid data corresponding to the current frame image;

[0049] S33. Obtain the frequency feature descriptor according to the following steps:

[0050] S331. Obtain the second grid value for each of the grids, wherein the second grid value is the z-axis coordinate value of the point with the largest z-axis coordinate value among all points contained in the grid;

[0051] S332. Combine all the second grid values ​​into a group of size... The fourth matrix;

[0052] S333. Perform a two-dimensional Fourier transform on the fourth matrix according to the following formula, and use the transformed result as the frequency feature descriptor:

[0053] ;

[0054] in, The transformation result is... As the first frequency variable, Let X be the number of rows divided along the x-axis in a Cartesian coordinate system, and Y be the number of columns divided along the y-axis in a Cartesian coordinate system. Let e ​​be the e-th row of the fourth matrix, and t be the t-th column of the fourth matrix. It is the imaginary unit.

[0055] By combining surface feature descriptors and frequency feature descriptors, the advantages of both can be combined to compensate for each other's shortcomings. This helps to ensure that both rotation and translation invariances are not lost during scene encoding. This ensures that when the robot passes through the same scene, even if it generates a different rotation angle and translation distance than historical data, it can still accurately describe the shape features of the current frame image and achieve high-precision loop closure detection.

[0056] Furthermore, the specific steps in step S3 include:

[0057] S34. Based on the fast search method, according to the fast matching descriptor, obtain multiple suspected keyframe images from historical keyframe images;

[0058] S35. Calculate the loop closure similarity between each of the suspected keyframe images and the current frame image based on the surface feature descriptor and the frequency feature descriptor.

[0059] Fast matching descriptors enable rapid searching, effectively improving search efficiency and shortening the loop closure detection cycle.

[0060] Furthermore, the specific steps in step S35 include:

[0061] S351. Calculate the surface feature similarity between each of the suspected keyframe images and the current frame image according to the following formula:

[0062] ;

[0063] ;

[0064] in, The surface feature similarity between the m-th suspected keyframe image and the current frame image is given. This is the surface feature descriptor corresponding to the m-th suspected keyframe image. The surface feature descriptor corresponding to the current frame image. Let H be the divergence between the h-th sector of the w-th ring in the grid data corresponding to the m-th suspected keyframe image and the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let H be the probability density of the h-th sector within the w-th ring of the grid data corresponding to the m-th suspected keyframe image. This represents the probability density of the h-th sector within the w-th ring of the grid data corresponding to the current frame image. Let h be the covariance matrix of the h-th sector within the w-th ring of the grid data corresponding to the m-th suspected keyframe image. This refers to finding the trace of a matrix. The mean polar coordinates of the h-th sector in the w-th ring of the grid data corresponding to the m-th suspected keyframe image;

[0065] S352. Calculate the frequency feature similarity between each of the suspected keyframe images and the current frame image according to the following formula:

[0066] ;

[0067] in, The frequency feature similarity between the m-th suspected keyframe image and the current frame image is given. This is the frequency feature descriptor corresponding to the m-th suspected keyframe image. The frequency feature descriptor corresponding to the current frame image. This indicates the search for the L1 norm.

[0068] S353. Based on the surface feature similarity and the frequency feature similarity, the loop closure similarity is calculated using the following formula:

[0069] ;

[0070] in, Let m be the loop closure similarity between the m-th suspected keyframe image and the current frame image. For the m-th suspected keyframe image, The current frame image, The first non-zero parameter is preset. This is the preset second non-zero parameter.

[0071] Furthermore, the specific steps in step S4 include:

[0072] S41. Based on the point cloud registration method, each of the candidate keyframe images is registered with the current frame image to obtain the registration similarity between each of the candidate keyframe images and the current frame image.

[0073] In a second aspect, the present invention provides a robotic laser SLAM loop closure detection device, comprising:

[0074] The first acquisition module is used to acquire the current frame image and the first point cloud data corresponding to the current frame image;

[0075] The gridding module is used to grid the first point cloud data when the current frame image is a keyframe image, so as to obtain the grid data corresponding to the current frame image;

[0076] The second acquisition module is used to acquire the fast matching descriptor, surface feature descriptor and frequency feature descriptor corresponding to the current frame image based on the grid data, so as to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each of the suspected keyframe images and the current frame image.

[0077] The sorting module is used to sort out candidate keyframe images from multiple suspected keyframe images according to the loop closure similarity, and to register them with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image.

[0078] The judgment module is used to determine whether the current frame image constitutes a loop closure based on the registration similarity.

[0079] The robot laser SLAM loop closure detection device provided by this invention uses loop closure similarity for discrimination, which is actually based on the surface shape of point cloud data. Since the surface shape of point cloud data can accurately reflect various features of the current scene, it can more accurately and effectively filter out dissimilar historical data, thereby achieving the effect of quickly and accurately determining whether the current frame image constitutes a loop closure.

[0080] Thirdly, the present invention provides an electronic device including a processor and a memory, the memory storing computer-readable instructions, which, when executed by the processor, perform the steps of the robotic laser SLAM loop closure detection method provided in the first aspect above.

[0081] Fourthly, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the robotic laser SLAM loop closure detection method provided in the first aspect above.

[0082] As can be seen from the above, the robot laser SLAM loop closure detection method provided by this invention can accurately describe abstract features such as the surface shape of point cloud data by meshing point cloud data and then using fast matching descriptors, surface feature descriptors, and frequency feature descriptors. Therefore, loop closure detection is achieved by searching for scenes in historical scenes that form closed loops with the current scene in real time. The advantages are:

[0083] Firstly, it can significantly improve the motion drift problem caused by cumulative positioning errors, which is conducive to achieving high-precision loop closure detection;

[0084] Secondly, low computational load can reduce the robot's computational requirements, which helps to reduce the overall cost of the robot.

[0085] Third, loop closure detection does not rely on installing sensors such as GPS on the robot;

[0086] Fourth, it is not limited by the robot's range of motion, can be applied to outdoor scenarios, and can ensure high detection accuracy and robustness.

[0087] Other features and advantages of the invention will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing embodiments of the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in the written description and the accompanying drawings. Attached Figure Description

[0088] Figure 1 This is a flowchart of a robotic laser SLAM loop closure detection method provided in an embodiment of the present invention.

[0089] Figure 2 This is a schematic diagram of a robotic laser SLAM loop closure detection device provided in an embodiment of the present invention.

[0090] Figure 3 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention.

[0091] Label Explanation:

[0092] 100. First acquisition module; 200. Mesh module; 300. Second acquisition module; 400. Sorting module; 500. Judgment module; 13. Electronic device; 1301. Processor; 1302. Memory; 1303. Communication bus. Detailed Implementation

[0093] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0094] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, in the description of this invention, terms such as "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0095] Please refer to Figure 1 , Figure 1 This is a flowchart of a robotic laser SLAM loop closure detection method. The robotic laser SLAM loop closure detection method includes the following steps:

[0096] S1. Obtain the current frame image and the first point cloud data corresponding to the current frame image;

[0097] S2. When the current frame image is a keyframe image, the first point cloud data is meshed to obtain the mesh data corresponding to the current frame image;

[0098] S3. Obtain the fast matching descriptor, surface feature descriptor and frequency feature descriptor corresponding to the current frame image based on the grid data, in order to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each suspected keyframe image and the current frame image;

[0099] S4. Based on the loop closure similarity, candidate keyframe images are selected from multiple suspected keyframe images and used to register with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image.

[0100] S5. Based on the registration similarity, determine whether the current frame image constitutes a loop closure.

[0101] In this embodiment, in practical applications, after converting the first point cloud data into grid data, the surface shape of the first point cloud data is described using surface feature descriptors and frequency feature descriptors. Then, a fast matching descriptor is used to quickly search for suspected keyframe images with surface shapes similar to the current frame image from historical data (i.e., historical keyframe images), and the corresponding loop closure similarity is calculated for each suspected keyframe image. Based on the loop closure similarity, candidate keyframe images with highly similar surface shapes to the current frame image are selected. Finally, the corresponding registration similarity is calculated for each candidate keyframe image. The registration similarity determines whether the current frame image constitutes a loop (i.e., whether the current scene is a historical scene previously visited by the robot). Compared to traditional methods that use a single feature for discrimination, this embodiment uses loop closure similarity for discrimination, which is essentially based on the surface shape of the point cloud data. The judgment is made at the level of shape. Since the surface shape of point cloud data can accurately reflect various features of the current scene, it can more accurately and effectively filter out dissimilar historical data, thereby achieving a fast and accurate determination of whether the current frame image constitutes a loop closure. This greatly improves the reliability of the robot's active mapping process, which is conducive to building a more accurate environmental map. In addition, the establishment of closed loop relationships can also significantly improve the motion drift problem caused by positioning errors, which helps the robot to complete navigation tasks more accurately. Furthermore, the process of obtaining point cloud data from the scene and describing its surface shape using surface feature descriptors and frequency feature descriptors is not limited by the robot's range of motion, does not rely on GPS positioning equipment, and does not require training complex neural network models. Its limitations are small, so the robot faces fewer obstacles and defects in the process of loop closure detection.

[0102] In some embodiments, the specific steps in step S1 include:

[0103] S11. Obtain the initial point cloud data corresponding to the current frame image;

[0104] S12. Perform ground removal processing on the initial point cloud data to obtain the second point cloud data;

[0105] S13. Perform voxel filtering on the second point cloud data to obtain the first point cloud data.

[0106] In this embodiment, in practical applications, the current frame image often contains many unnecessary features (such as ground data) and invalid data (such as noise). Ground data and noise will introduce more irrelevant features, which will affect the fast matching descriptor to achieve fast search, and affect the surface feature descriptor and frequency feature descriptor to accurately describe the surface shape of the point cloud data. Therefore, it is often necessary to preprocess the current frame image to remove ground data and reduce noise.

[0107] Specifically, the robot first uses a LiDAR to acquire the initial point cloud data of the current frame image. The initial point cloud data is a set of three-dimensional coordinates in the Cartesian coordinate system, including multiple points, each of which has corresponding three-dimensional coordinates in the Cartesian coordinate system.

[0108] Taking a Cartesian coordinate system with the LiDAR installation location as the origin and the z-axis as the positive direction perpendicular to the ground as the coordinate origin, the ground removal process includes setting a first threshold on the z-axis, removing all points whose z-axis coordinate values ​​are greater than the first threshold, and finally obtaining the second point cloud data with the ground data removed.

[0109] Subsequently, the voxel filtering process involves dividing the second point cloud data into multiple cubic units (i.e., voxels) of a specified size, then performing statistical analysis on the second point cloud data within each voxel to distinguish between voxels containing valid second point cloud data and noise, and finally recombining the voxels containing valid second point cloud data to obtain the first point cloud data.

[0110] In some embodiments, the specific steps in step S2 include:

[0111] S27. Use a laser odometry to determine whether the current frame image is a keyframe image.

[0112] Specifically, a second threshold is set based on the moving distance. When the robot carrying the laser odometer moves a distance exceeding the second threshold, the current frame image obtained at the current position is determined to be a keyframe image.

[0113] In some embodiments, the specific steps in step S2 include:

[0114] S21. Project the first point cloud data onto a cylindrical coordinate system; the cylindrical coordinate system consists of a polar coordinate system and a z-axis perpendicular to the polar coordinate system;

[0115] S22. Divide the polar coordinate system into multiple rings along the r-axis (i.e., the polar axis) and divide each ring into multiple sectors;

[0116] S23. Determine the sector to which each point belongs based on the cylindrical coordinates of each point in the first point cloud data;

[0117] S24. Divide the Cartesian coordinate system into multiple grids along the x-axis and y-axis respectively; the origin of the Cartesian coordinate system is the pole of the polar coordinate system, the x-axis of the Cartesian coordinate system is the polar axis of the polar coordinate system, and the counterclockwise direction of the Cartesian coordinate system is the positive direction of the polar coordinate system.

[0118] S25. Determine the grid to which each point belongs based on the Cartesian coordinates of each point in the first point cloud data;

[0119] S26. Take the sector and grid corresponding to each point in the first point cloud data as grid data.

[0120] Specifically, in step S21, after the first point cloud data is projected onto the cylindrical coordinate system, the z-axis coordinate value of each point in the first point cloud data is preserved.

[0121] In step S22, the space is divided into W annexes, and the width of each annulus is: ,in, Let w be the width of the w-th ring. The third threshold is set as the preset threshold, and w is the w-th annulus; then each annulus is divided into H sectors, thus dividing the entire space into... Each sector.

[0122] In step S23, the sector to which each point belongs is determined based on its cylindrical coordinate values, and then... This represents the set of the first point cloud in the h-th sector of the w-th annulus. ).

[0123] In step S24, the space is divided into X rows along the x-axis, and the width of each row is: ,in, Let x be the width of the x-th row. The preset fourth threshold is used, x is the x-th row, and then the space is divided into Y columns along the y-axis, with the width of each column being: ,in, The width of column y is... The fifth preset threshold is used, and y is the y-th column, thus dividing the entire space into... Each grid.

[0124] In step S25, the grid to which each point belongs is determined based on its Cartesian coordinates, and then... This represents the set of the second point cloud in the x-th row and y-th column of the grid.

[0125] In step S26, the grid data records the position of each point in the first point cloud data (including the ring to which it belongs, the sector to which it belongs, the row number to which it belongs, and the column number to which it belongs).

[0126] In some embodiments, the specific steps in step S3 include:

[0127] S31. Obtain the fast matching descriptor according to the following steps:

[0128] S311. Obtain the first grid value of each sector and combine all the first grid values ​​into a group of size . The first matrix; the first grid value is the z-axis coordinate of the point with the largest z-axis coordinate among all points contained in the sector;

[0129] S312. Use the first matrix as a fast matching descriptor;

[0130] S32. Obtain the surface feature descriptor according to the following steps:

[0131] S321. Calculate the mean polar coordinates of each sector using the following formula:

[0132] ;

[0133] in, This represents the mean polar coordinates of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. This represents the total number of points contained in the h-th sector within the w-th annulus of the current frame image's grid data. This refers to the i-th point in the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let w be the cylindrical coordinate vector of the i-th point in the h-th sector of the w-th annulus in the grid data corresponding to the current frame image, w be the w-th annulus in the grid data corresponding to the current frame image, h be the h-th sector in each annulus in the grid data corresponding to the current frame image, W be the total number of annulus in the grid data corresponding to the current frame image, and H be the total number of sectors in each annulus in the grid data corresponding to the current frame image.

[0134] S322. Calculate the covariance matrix of each sector using the following formula:

[0135] ;

[0136] in, This is the covariance matrix of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. This indicates finding the transpose of the matrix;

[0137] S323. Combine the mean values ​​of all polar coordinates into a value of... The second matrix;

[0138] S324. Combine all covariance matrices into a matrix of size [missing value]. The third matrix;

[0139] S325. The second and third matrices are used as surface feature descriptors, and the polar coordinate mean and covariance matrix of the same sector conform to the following functional relationship used to describe the shape features of each sector:

[0140] ;

[0141] in, For encoding functions, The shape feature of the h-th sector in the w-th ring of the grid data corresponding to the current frame image;

[0142] S33. Obtain the frequency feature descriptor according to the following steps:

[0143] S331. Obtain the second grid value for each grid. The second grid value is the z-axis coordinate value of the point with the largest z-axis coordinate value among all points contained in the grid.

[0144] S332. Combine all second grid values ​​into a group of size... The fourth matrix;

[0145] S333. Perform a two-dimensional Fourier transform on the fourth matrix according to the following formula, and use the transformed result as a frequency feature descriptor:

[0146] ;

[0147] in, For the transformation result, As the first frequency variable, Let X be the number of rows divided along the x-axis in a Cartesian coordinate system, and Y be the number of columns divided along the y-axis in a Cartesian coordinate system. Let e ​​be the e-th row of the fourth matrix, and t be the t-th column of the fourth matrix. It is the imaginary unit.

[0148] It should be noted that the two-dimensional Fourier transform is common knowledge, and the explanations of the first frequency variable, the second frequency variable, and the input function will not be repeated here.

[0149] Specifically, in step S325, since the normal distribution can express different distribution shapes through polar coordinate mean and covariance matrix, the shape features of the point cloud in each sector are encoded in the form of a normal distribution using the encoding function. The shape features of each sector are described by the normal distribution shape (the normal distribution shape of all sectors describes the shape features of the current frame image). The shape features can more accurately reflect the various features of each sector and the entire current frame image. Therefore, by comparing the shape features with historical data, historical data can be filtered more accurately, thereby improving the accuracy of loop closure detection.

[0150] Meanwhile, the surface feature descriptor encodes the surface shape of the point cloud in the sector using a probability distribution, allowing surface shape characteristics that are difficult to calculate to be calculated based on the differences between the distributions. While the surface feature descriptor itself is rotation-invariant, it lacks translation invariance. Therefore, this embodiment further calculates the frequency feature descriptor using the two-dimensional Fourier transform principle, using the frequency feature descriptor to encode the variations in the height of the point cloud in each grid. The frequency feature descriptor itself is translation-invariant but lacks rotation invariance. Combining the surface feature descriptor and the frequency feature descriptor provides a comprehensive and accurate description of the point cloud's surface shape, achieving a complementary effect. This ensures that both rotational and translational invariances are not lost during scene encoding, thus ensuring that when the robot passes through the same scene, even if it experiences a different rotational angle and translational distance than historical data, the two invariants in the above encoding allow the robot to accurately identify that it has been to that location before, avoiding the robot's recognition and judgment of the scene being affected by changes in its own position and angle.

[0151] In some embodiments, even without obtaining fast matching descriptors, historical data can be screened using only surface feature descriptors and frequency feature descriptors. However, this method is actually a brute-force search (i.e., exhaustive search, which is existing technology and will not be elaborated here), and its efficiency is low.

[0152] In some preferred embodiments, the specific steps in step S3 include:

[0153] S34. Based on a fast search method, multiple suspected keyframe images are obtained from historical keyframe images according to fast matching descriptors;

[0154] S35. Based on the surface feature descriptor and frequency feature descriptor, calculate the loop closure similarity between each suspected keyframe image and the current frame image.

[0155] Specifically, the specific steps in step S34 include:

[0156] S341. Calculate the mean of each row in the fast matching descriptor to obtain the mean vector;

[0157] S342. Construct a tree-like data structure using the mean vector as a feature;

[0158] S343. Use a tree data structure to obtain multiple suspected keyframe images from historical keyframe images.

[0159] In this embodiment, by calculating the mean of each row in the fast matching descriptor, a mean vector can be obtained. Then, this mean vector is used as a feature to construct a kd-tree (tree data structure). Based on the constructed kd-tree, fast search can be achieved, effectively improving search efficiency and shortening the loop closure detection cycle.

[0160] In some embodiments, the specific steps in step S35 include:

[0161] S351. Calculate the surface feature similarity between each suspected keyframe image and the current frame image according to the following formula:

[0162] ;

[0163] ;

[0164] in, Let m be the surface feature similarity between the m-th suspected keyframe image and the current frame image. This is the surface feature descriptor corresponding to the m-th suspected keyframe image. The surface feature descriptor corresponding to the current frame image. Let be the divergence between the h-th sector of the w-th ring in the grid data corresponding to the m-th suspected keyframe image and the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let H be the probability density of the h-th sector within the w-th ring in the grid data corresponding to the m-th suspected keyframe image. This represents the probability density of the h-th sector within the w-th ring of the grid data corresponding to the current frame image. Let h be the covariance matrix of the h-th sector within the w-th annulus in the grid data corresponding to the m-th suspected keyframe image. This refers to finding the trace of a matrix. The mean polar coordinates of the h-th sector in the w-th ring of the grid data corresponding to the m-th suspected keyframe image;

[0165] S352. Calculate the frequency feature similarity between each suspected keyframe image and the current frame image using the following formula:

[0166] ;

[0167] in, Let m be the frequency feature similarity between the m-th suspected keyframe image and the current frame image. This is the frequency feature descriptor corresponding to the m-th suspected keyframe image. This is the frequency feature descriptor corresponding to the current frame image. This indicates the search for the L1 norm.

[0168] S353. Based on surface feature similarity and frequency feature similarity, calculate the loop closure similarity using the following formula:

[0169] ;

[0170] in, Let be the loop closure similarity between the m-th suspected keyframe image and the current frame image. For the m-th suspected keyframe image, For the current frame image, The first non-zero parameter is preset. This is the preset second non-zero parameter.

[0171] In this embodiment, when applying it in practice, we first consider calculating the surface feature similarity, because the two matrices (the second matrix and the third matrix) of the surface feature descriptor actually record the probability density fitting in each sector. Therefore, we can calculate the KL divergence of each sector corresponding to the suspected keyframe image and the current frame image (see Formula 1 below) to compare the similarity of each sector corresponding to the two.

[0172] Formula 1;

[0173] in, It indicates a desire for the expected value.

[0174] Furthermore, since the surface feature descriptor is actually fitted using a normal distribution, the relationship between the KL divergence of each sector and the mean and covariance matrix of the actual recorded polar coordinates can be derived:

[0175] ;

[0176] Finally, by calculating the KL divergence of all sectors, the surface feature similarity between the suspected keyframe image and the current frame image can be obtained:

[0177] ;

[0178] Secondly, frequency feature similarity is considered. Frequency feature similarity is obtained by calculating the L1 norm of the frequency feature descriptors of the suspected keyframe image and the current frame image.

[0179] Finally, the sum of surface feature similarity and frequency feature similarity is calculated as the loop closure similarity between the suspected keyframe image and the current frame image (the higher the loop closure similarity, the more similar the suspected keyframe image and the current frame image are, and vice versa).

[0180] In some embodiments, the following step is included before step S353:

[0181] S3531. Modify the surface feature descriptor and the frequency feature descriptor.

[0182] Specifically, the steps in step S3531 include:

[0183] S35311. Calculate the heading angle deviation based on the surface feature descriptor;

[0184] S35312. Based on the heading angle deviation, adjust the mean polar coordinates in the surface feature descriptor corresponding to the current frame image from... Revised to ;

[0185] in, This represents the mean r-axis coordinate of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. This represents the mean angular coordinates of the h-th sector within the w-th ring of the grid data corresponding to the current frame image. This refers to the deviation in heading angle;

[0186] S35313. Based on the heading angle deviation, the frequency feature descriptor corresponding to the current frame image is transferred from... Revised to ;

[0187] ;

[0188] In this embodiment, since the heading angle deviation only affects the mean of the angular coordinates in the surface feature descriptor, a brute-force search can be used to traverse all angular coordinates to obtain the heading angle deviation.

[0189] The heading angle deviation affects both the surface feature descriptor and the frequency feature descriptor. Therefore, after obtaining the heading angle deviation, it is necessary to correct both the surface feature descriptor and the frequency feature descriptor. For the surface feature descriptor, only the second matrix needs to be corrected, that is, the heading angle deviation is added to the mean of the angular coordinates corresponding to the mean of the polar coordinates of each item in the second matrix, as shown in step S35312. For the frequency feature descriptor, the heading angle deviation needs to be substituted into the calculation, as shown in step S35313.

[0190] In some embodiments, the specific steps in step S4 include:

[0191] S41. Based on the point cloud registration method, each candidate keyframe image is registered with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image.

[0192] In this embodiment, after obtaining candidate keyframe images based on loop closure similarity, it is necessary to further input the current frame image and candidate keyframe images into the point cloud matching algorithm to obtain registration similarity, and determine whether a loop closure is formed through registration similarity.

[0193] It should be noted that point cloud matching algorithms are existing technologies and will not be elaborated upon here.

[0194] Furthermore, in step S5, if the registration similarity is greater than the sixth threshold, it indicates that the loop closure detection is successful (the current frame image constitutes a loop closure). If there are multiple successful results, the result with the highest registration similarity is selected.

[0195] In some embodiments, step S5 is followed by:

[0196] S6. Add the current frame image to the historical data as the historical keyframe image for the next loop closure detection.

[0197] In this embodiment, the current frame image that successfully forms a loop closure is added to the historical data to enrich the historical data and provide more data foundation for the next loop closure detection, which helps the robot to better complete the mapping and navigation work.

[0198] Please refer to Figure 2 , Figure 2 This invention provides a robotic laser SLAM loop closure detection device in some embodiments, which is integrated into a back-end control device in the form of a computer program, comprising:

[0199] The first acquisition module 100 is used to acquire the current frame image and the first point cloud data corresponding to the current frame image;

[0200] The meshing module 200 is used to mesh the first point cloud data when the current frame image is a key frame image, so as to obtain the mesh data corresponding to the current frame image;

[0201] The second acquisition module 300 is used to acquire the fast matching descriptor, surface feature descriptor and frequency feature descriptor corresponding to the current frame image based on the grid data, so as to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each suspected keyframe image and the current frame image.

[0202] The sorting module 400 is used to sort out candidate keyframe images from multiple suspected keyframe images based on loop closure similarity, and then register them with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image.

[0203] The judgment module 500 is used to determine whether the current frame image constitutes a loop based on the registration similarity.

[0204] In some embodiments, the first acquisition module 100 performs the following when acquiring the current frame image and the first point cloud data corresponding to the current frame image:

[0205] S11. Obtain the initial point cloud data corresponding to the current frame image;

[0206] S12. Perform ground removal processing on the initial point cloud data to obtain the second point cloud data;

[0207] S13. Perform voxel filtering on the second point cloud data to obtain the first point cloud data.

[0208] In some embodiments, the meshing module 200 is executed when meshing the first point cloud data to obtain the mesh data corresponding to the current frame image:

[0209] S27. Use a laser odometry to determine whether the current frame image is a keyframe image.

[0210] In some embodiments, the meshing module 200 is executed when meshing the first point cloud data to obtain the mesh data corresponding to the current frame image:

[0211] S21. Project the first point cloud data onto a cylindrical coordinate system; the cylindrical coordinate system consists of a polar coordinate system and a z-axis perpendicular to the polar coordinate system;

[0212] S22. Divide the polar coordinate system into multiple rings along the r-axis and divide each ring into multiple sectors;

[0213] S23. Determine the sector to which each point belongs based on the cylindrical coordinates of each point in the first point cloud data;

[0214] S24. Divide the Cartesian coordinate system into multiple grids along the x-axis and y-axis respectively; the origin of the Cartesian coordinate system is the pole of the polar coordinate system, the x-axis of the Cartesian coordinate system is the polar axis of the polar coordinate system, and the counterclockwise direction of the Cartesian coordinate system is the positive direction of the polar coordinate system.

[0215] S25. Determine the grid to which each point belongs based on the Cartesian coordinates of each point in the first point cloud data;

[0216] S26. Take the sector and grid corresponding to each point in the first point cloud data as grid data.

[0217] In some embodiments, the second acquisition module 300 is executed when acquiring the fast matching descriptor, surface feature descriptor, and frequency feature descriptor corresponding to the current frame image based on the grid data, in order to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each suspected keyframe image and the current frame image:

[0218] S31. Obtain the fast matching descriptor according to the following steps:

[0219] S311. Obtain the first grid value of each sector and combine all the first grid values ​​into a group of size . The first matrix; the first grid value is the z-axis coordinate of the point with the largest z-axis coordinate among all points contained in the sector;

[0220] S312. Use the first matrix as a fast matching descriptor;

[0221] S32. Obtain the surface feature descriptor according to the following steps:

[0222] S321. Calculate the mean polar coordinates of each sector using the following formula:

[0223] ;

[0224] in, This represents the mean polar coordinates of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. This represents the total number of points contained in the h-th sector within the w-th annulus of the current frame image's grid data. This refers to the i-th point in the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let w be the cylindrical coordinate vector of the i-th point in the h-th sector of the w-th annulus in the grid data corresponding to the current frame image, w be the w-th annulus in the grid data corresponding to the current frame image, h be the h-th sector in each annulus in the grid data corresponding to the current frame image, W be the total number of annulus in the grid data corresponding to the current frame image, and H be the total number of sectors in each annulus in the grid data corresponding to the current frame image.

[0225] S322. Calculate the covariance matrix of each sector using the following formula:

[0226] ;

[0227] in, This is the covariance matrix of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. This indicates finding the transpose of the matrix;

[0228] S323. Combine the mean values ​​of all polar coordinates into a value of... The second matrix;

[0229] S324. Combine all covariance matrices into a matrix of size [missing value]. The third matrix;

[0230] S325. The second and third matrices are used as surface feature descriptors, and the polar coordinate mean and covariance matrix of the same sector conform to the following functional relationship used to describe the shape features of each sector:

[0231] ;

[0232] in, For encoding functions, The shape feature of the h-th sector in the w-th ring of the grid data corresponding to the current frame image;

[0233] S33. Obtain the frequency feature descriptor according to the following steps:

[0234] S331. Obtain the second grid value for each grid. The second grid value is the z-axis coordinate value of the point with the largest z-axis coordinate value among all points contained in the grid.

[0235] S332. Combine all second grid values ​​into a group of size... The fourth matrix;

[0236] S333. Perform a two-dimensional Fourier transform on the fourth matrix according to the following formula, and use the transformed result as a frequency feature descriptor:

[0237] ;

[0238] in, For the transformation result, As the first frequency variable, Let X be the number of rows divided along the x-axis in a Cartesian coordinate system, and Y be the number of columns divided along the y-axis in a Cartesian coordinate system. Let e ​​be the e-th row of the fourth matrix, and t be the t-th column of the fourth matrix. It is the imaginary unit.

[0239] In some embodiments, the second acquisition module 300 is executed when acquiring the fast matching descriptor, surface feature descriptor, and frequency feature descriptor corresponding to the current frame image based on the grid data, in order to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each suspected keyframe image and the current frame image:

[0240] S34. Based on a fast search method, multiple suspected keyframe images are obtained from historical keyframe images according to fast matching descriptors;

[0241] S35. Based on the surface feature descriptor and frequency feature descriptor, calculate the loop closure similarity between each suspected keyframe image and the current frame image.

[0242] In some embodiments, the second acquisition module 300 is executed when obtaining multiple suspected keyframe images from historical keyframe images based on a fast search method and fast matching descriptors:

[0243] S341. Calculate the mean of each row in the fast matching descriptor to obtain the mean vector;

[0244] S342. Construct a tree-like data structure using the mean vector as a feature;

[0245] S343. Use a tree data structure to obtain multiple suspected keyframe images from historical keyframe images.

[0246] In some embodiments, the second acquisition module 300 performs the following when calculating the loop closure similarity between each suspected keyframe image and the current frame image based on surface feature descriptors and frequency feature descriptors:

[0247] S351. Calculate the surface feature similarity between each suspected keyframe image and the current frame image according to the following formula:

[0248] ;

[0249] ;

[0250] in, Let m be the surface feature similarity between the m-th suspected keyframe image and the current frame image. This is the surface feature descriptor corresponding to the m-th suspected keyframe image. The surface feature descriptor corresponding to the current frame image. Let be the divergence between the h-th sector of the w-th ring in the grid data corresponding to the m-th suspected keyframe image and the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let H be the probability density of the h-th sector within the w-th ring in the grid data corresponding to the m-th suspected keyframe image. This represents the probability density of the h-th sector within the w-th ring of the grid data corresponding to the current frame image. Let h be the covariance matrix of the h-th sector within the w-th annulus in the grid data corresponding to the m-th suspected keyframe image. This refers to finding the trace of a matrix. The mean polar coordinates of the h-th sector in the w-th ring of the grid data corresponding to the m-th suspected keyframe image;

[0251] S352. Calculate the frequency feature similarity between each suspected keyframe image and the current frame image using the following formula:

[0252] ;

[0253] in, Let m be the frequency feature similarity between the m-th suspected keyframe image and the current frame image. This is the frequency feature descriptor corresponding to the m-th suspected keyframe image. This is the frequency feature descriptor corresponding to the current frame image. This indicates the search for the L1 norm.

[0254] S353. Based on surface feature similarity and frequency feature similarity, calculate the loop closure similarity using the following formula:

[0255] ;

[0256] in, Let be the loop closure similarity between the m-th suspected keyframe image and the current frame image. For the m-th suspected keyframe image, For the current frame image, The first non-zero parameter is preset. This is the preset second non-zero parameter.

[0257] In some embodiments, the second acquisition module 300 performs the following when calculating the loop closure similarity using a formula based on surface feature similarity and frequency feature similarity:

[0258] S3531. Modify the surface feature descriptor and the frequency feature descriptor.

[0259] In some embodiments, the second acquisition module 300 is executed when modifying the surface feature descriptor and the frequency feature descriptor:

[0260] S35311. Calculate the heading angle deviation based on the surface feature descriptor;

[0261] S35312. Based on the heading angle deviation, adjust the mean polar coordinates in the surface feature descriptor corresponding to the current frame image from... Revised to ;

[0262] in, This represents the mean r-axis coordinate of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. This represents the mean angular coordinates of the h-th sector within the w-th ring of the grid data corresponding to the current frame image. This refers to the deviation in heading angle;

[0263] S35313. Based on the heading angle deviation, the frequency feature descriptor corresponding to the current frame image is transferred from... Revised to ;

[0264] .

[0265] In some embodiments, the sorting module 400 performs the following steps when it is used to sort candidate keyframe images from multiple suspected keyframe images based on loop closure similarity, for registration with the current frame image, and to obtain the registration similarity between each candidate keyframe image and the current frame image:

[0266] S41. Based on the point cloud registration method, each candidate keyframe image is registered with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image.

[0267] In some embodiments, the robotic laser SLAM loop closure detection device further includes an adding module for adding the current frame image to historical data as a historical keyframe image for the next loop closure detection.

[0268] Please refer to Figure 3 , Figure 3 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention. The present invention provides an electronic device 13, including: a processor 1301 and a memory 1302. The processor 1301 and the memory 1302 are interconnected and communicate with each other via a communication bus 1303 and / or other forms of connection mechanism (not shown). The memory 1302 stores computer-readable instructions executable by the processor 1301. When the electronic device is running, the processor 1301 executes these computer-readable instructions to perform the robot laser SLAM loop closure detection method in any optional implementation of the above embodiments, to achieve the following functions: acquiring the current frame image and the current frame image pair. The first point cloud data is obtained; when the current frame image is a keyframe image, the first point cloud data is gridded to obtain the grid data corresponding to the current frame image; based on the grid data, the fast matching descriptor, surface feature descriptor, and frequency feature descriptor corresponding to the current frame image are obtained, which are used to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each suspected keyframe image and the current frame image; based on the loop closure similarity, candidate keyframe images are selected from multiple suspected keyframe images and used to register with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image; based on the registration similarity, it is determined whether the current frame image constitutes a loop closure.

[0269] This invention provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it performs the robotic laser SLAM loop closure detection method in any optional implementation of the above embodiments to achieve the following functions: acquiring a current frame image and corresponding first point cloud data; when the current frame image is a keyframe image, meshing the first point cloud data to obtain mesh data corresponding to the current frame image; acquiring a fast matching descriptor, surface feature descriptor, and frequency feature descriptor corresponding to the current frame image based on the mesh data, for extracting multiple suspected keyframe images from historical keyframe images and determining the loop closure similarity between each suspected keyframe image and the current frame image; selecting candidate keyframe images from the multiple suspected keyframe images based on the loop closure similarity, for registration with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image; and determining whether the current frame image constitutes a loop closure based on the registration similarity.

[0270] The computer-readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read Only Memory (EPROM), Programmable Red-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0271] In the embodiments provided by this invention, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Additionally, the displayed or discussed mutual couplings, direct couplings, or communication connections may be through some communication interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

[0272] Furthermore, the units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0273] Furthermore, the functional modules in the various embodiments of the present invention can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0274] In this document, relational terms such as first and second are used only to distinguish one entity or operation from another entity or operation, without necessarily requiring or implying any such actual relationship or order between these entities or operations.

[0275] The above description is merely an embodiment of the present invention and is not intended to limit the scope of protection of the present invention. For those skilled in the art, the present invention can have various modifications and variations. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A robotic laser SLAM loop closure detection method, characterized in that, Includes the following steps: S1. Obtain the current frame image and the first point cloud data corresponding to the current frame image; S2. When the current frame image is a keyframe image, the first point cloud data is gridded to obtain the grid data corresponding to the current frame image; S3. Obtain the fast matching descriptor, surface feature descriptor and frequency feature descriptor corresponding to the current frame image based on the grid data, in order to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each of the suspected keyframe images and the current frame image; S4. Based on the loop closure similarity, candidate keyframe images are selected from the multiple suspected keyframe images for registration with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image. S5. Based on the registration similarity, determine whether the current frame image constitutes a loop closure; The specific steps in step S2 include: S21. Project the first point cloud data onto a cylindrical coordinate system; the cylindrical coordinate system is composed of a polar coordinate system and a z-axis perpendicular to the polar coordinate system; S22. Divide the polar coordinate system into multiple rings along the r-axis direction and divide each ring into multiple sectors; S23. Determine the sector to which each point belongs based on the cylindrical coordinates of each point in the first point cloud data; S24. Divide the Cartesian coordinate system into multiple grids along the x-axis and y-axis directions respectively; the origin of the Cartesian coordinate system is the pole of the polar coordinate system, the x-axis of the Cartesian coordinate system is the polar axis of the polar coordinate system, and the counterclockwise direction of the Cartesian coordinate system is the positive direction of the polar coordinate system. S25. Determine the grid to which each point belongs based on the Cartesian coordinates of each point in the first point cloud data; S26. The sector and the corresponding grid of each point in the first point cloud data are taken as the grid data; The specific steps in step S3 include: S31. Obtain the fast matching descriptor according to the following steps: S311. Obtain the first grid value of each sector and combine all the first grid values ​​into a group of size . The first matrix; the first grid value is the z-axis coordinate of the point with the largest z-axis coordinate among all points contained in the sector; S312. Use the first matrix as the fast matching descriptor; S32. Obtain the surface feature descriptor according to the following steps: S321. Calculate the mean polar coordinates of each sector according to the following formula: ； in, The polar coordinate mean of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image. The total number of points contained in the h-th sector of the w-th ring in the grid data corresponding to the current frame image. The i-th point in the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let w be the cylindrical coordinate vector of the i-th point in the h-th sector of the w-th annulus in the grid data corresponding to the current frame image, w be the w-th annulus in the grid data corresponding to the current frame image, h be the h-th sector in each annulus in the grid data corresponding to the current frame image, W be the total number of annulus in the grid data corresponding to the current frame image, and H be the total number of sectors in each annulus in the grid data corresponding to the current frame image. S322. Calculate the covariance matrix of each sector according to the following formula: ； in, The covariance matrix of the h-th sector within the w-th annulus in the grid data corresponding to the current frame image is given. This indicates finding the transpose of the matrix; S323. Combine the mean values ​​of all the polar coordinates into a group of size... The second matrix; S324. Combine all the covariance matrices into a matrix of size [missing value]. The third matrix; S325. The second matrix and the third matrix are used as the surface feature descriptors, and the polar coordinate mean and covariance matrix of the same sector conform to the following functional relationship used to describe the shape features of each sector: ； in, For encoding functions, The shape feature of the h-th sector in the w-th ring of the grid data corresponding to the current frame image; S33. Obtain the frequency feature descriptor according to the following steps: S331. Obtain the second grid value for each of the grids, wherein the second grid value is the z-axis coordinate value of the point with the largest z-axis coordinate value among all points contained in the grid; S332. Combine all the second grid values ​​into a group of size... The fourth matrix; S333. Perform a two-dimensional Fourier transform on the fourth matrix according to the following formula, and use the transformed result as the frequency feature descriptor: ； in, The transformation result is... As the first frequency variable, Let X be the number of rows divided along the x-axis in a Cartesian coordinate system, and Y be the number of columns divided along the y-axis in a Cartesian coordinate system. Let e ​​be the e-th row of the fourth matrix, and t be the t-th column of the fourth matrix. It is the imaginary unit.

2. The robotic laser SLAM loop closure detection method according to claim 1, characterized in that, The specific steps in step S1 include: S11. Obtain the initial point cloud data corresponding to the current frame image; S12. Perform ground removal processing on the initial point cloud data to obtain the second point cloud data; S13. Perform voxel filtering on the second point cloud data to obtain the first point cloud data.

3. The robotic laser SLAM loop closure detection method according to claim 1, characterized in that, The specific steps in step S3 include: S34. Based on the fast search method, according to the fast matching descriptor, obtain multiple suspected keyframe images from historical keyframe images; S35. Calculate the loop closure similarity between each of the suspected keyframe images and the current frame image based on the surface feature descriptor and the frequency feature descriptor.

4. The robotic laser SLAM loop closure detection method according to claim 3, characterized in that, The specific steps in step S35 include: S351. Calculate the surface feature similarity between each of the suspected keyframe images and the current frame image according to the following formula: ； ； in, The surface feature similarity between the m-th suspected keyframe image and the current frame image is given. This is the surface feature descriptor corresponding to the m-th suspected keyframe image. The surface feature descriptor corresponding to the current frame image. Let H be the divergence between the h-th sector of the w-th ring in the grid data corresponding to the m-th suspected keyframe image and the h-th sector of the w-th ring in the grid data corresponding to the current frame image. Let h be the probability density of the h-th sector within the w-th ring of the grid data corresponding to the m-th suspected keyframe image. This represents the probability density of the h-th sector within the w-th ring of the grid data corresponding to the current frame image. Let h be the covariance matrix of the h-th sector within the w-th ring of the grid data corresponding to the m-th suspected keyframe image. This refers to finding the trace of a matrix. The mean polar coordinates of the h-th sector in the w-th ring of the grid data corresponding to the m-th suspected keyframe image; S352. Calculate the frequency feature similarity between each of the suspected keyframe images and the current frame image according to the following formula: ； in, The frequency feature similarity between the m-th suspected keyframe image and the current frame image is given. This is the frequency feature descriptor corresponding to the m-th suspected keyframe image. The frequency feature descriptor corresponding to the current frame image. This indicates the search for the L1 norm; S353. Based on the surface feature similarity and the frequency feature similarity, the loop closure similarity is calculated using the following formula: ； in, Let m be the loop closure similarity between the m-th suspected keyframe image and the current frame image. For the m-th suspected keyframe image, The current frame image, The first non-zero parameter is preset. This is the preset second non-zero parameter.

5. The robotic laser SLAM loop closure detection method according to claim 1, characterized in that, The specific steps in step S4 include: S41. Based on the point cloud registration method, each of the candidate keyframe images is registered with the current frame image to obtain the registration similarity between each of the candidate keyframe images and the current frame image.

6. A robotic laser SLAM loop closure detection device employing the robotic laser SLAM loop closure detection method as described in any one of claims 1-5, characterized in that, include: The first acquisition module is used to acquire the current frame image and the first point cloud data corresponding to the current frame image; The gridding module is used to grid the first point cloud data when the current frame image is a keyframe image, so as to obtain the grid data corresponding to the current frame image; The second acquisition module is used to acquire the fast matching descriptor, surface feature descriptor and frequency feature descriptor corresponding to the current frame image based on the grid data, so as to extract multiple suspected keyframe images from historical keyframe images and determine the loop closure similarity between each of the suspected keyframe images and the current frame image. The sorting module is used to sort out candidate keyframe images from multiple suspected keyframe images based on the loop closure similarity, and to register them with the current frame image to obtain the registration similarity between each candidate keyframe image and the current frame image. The judgment module is used to determine whether the current frame image constitutes a loop closure based on the registration similarity.

7. An electronic device, characterized in that, It includes a processor and a memory, the memory storing computer-readable instructions, which, when executed by the processor, perform the steps of the robot laser SLAM loop closure detection method as described in any one of claims 1-5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it performs the steps in the robotic laser SLAM loop closure detection method as described in any one of claims 1-5.

Images