A closed loop detection method and apparatus

Abstract

The application provides a closed loop detection method and device, wherein the method comprises: acquiring laser point cloud data of a current frame collected by a laser radar, the laser radar being installed on a mobile carrier; selecting edge points from a plurality of detection points according to the laser point cloud data of the current frame; constructing a polar coordinate projection graph of the current frame, the polar coordinate projection graph taking a position of the laser radar as a pole point, the selected edge points being distributed in the polar coordinate projection graph according to a positional relationship with the laser radar, and the polar coordinate projection graph being divided into a plurality of sectors; determining, for each sector, a corresponding intensity representative value and distance representative value of the sector; generating a fusion description graph of the current frame according to the intensity representative values and distance representative values corresponding to all the sectors; and detecting whether a moving track of the mobile carrier satisfies a closed loop detection condition according to the fusion description graph. Through the application, the accuracy of a detection result of whether a moving track of a mobile carrier satisfies a closed loop detection condition is improved.

Description

Technical Field

[0001] This application relates to the field of laser positioning technology, and more specifically, to a closed-loop detection method and apparatus. Background Technology

[0002] Currently, loop closure detection is of great importance in Automated Guided Vehicle (AGV) scenarios. AGVs play a key role in industrial automation and logistics transportation, and their core functions include accurate positioning and mapping. During the autonomous navigation of AGVs, the positioning and mapping algorithms need to process a large amount of sensor data, such as LiDAR, cameras, inertial measurement units (IMUs), and odometers. These sensors inevitably produce cumulative errors, leading to deviations in positioning and mapping, thereby affecting the navigation accuracy and reliability of AGVs.

[0003] Closed-loop detection can identify whether an AGV has returned to a previously visited location during operation. When the AGV detects a closed loop, it can correct accumulated errors and optimize the entire trajectory by matching the current environment with the previously constructed map. This not only helps to eliminate sensor errors but also ensures the consistency and accuracy of the map.

[0004] In loop closure detection, the construction of Link3D descriptors relies on the geometric features of point clouds, such as edge features. However, in extreme environments such as indoor spaces or corridors, there are relatively simple and repetitive geometric structures, such as vertical walls, floors, and ceilings. These structures lack rich feature points and cannot extract unique feature points for descriptor construction. Furthermore, it is difficult to find enough significant feature points in such environments, which leads to a scarcity of descriptors and thus affects the accuracy and reliability of loop closure detection. Summary of the Invention

[0005] In view of this, the purpose of this application is to provide a closed-loop detection method and apparatus to overcome at least one of the above-mentioned defects.

[0006] In a first aspect, embodiments of this application provide a loop closure detection method, the method comprising: acquiring laser point cloud data of the current frame collected by a lidar, the lidar being mounted on a mobile carrier, the laser point cloud data including point cloud data of multiple detection points detected by the lidar during the movement of the mobile carrier; selecting edge points from the multiple detection points based on the laser point cloud data of the current frame; constructing a polar coordinate projection map of the current frame, the polar coordinate projection map having the location of the lidar as the pole, the selected edge points being distributed in the polar coordinate projection map according to their positional relationship with the lidar, the polar coordinate projection map being divided into multiple sectors; determining the intensity representative value and distance representative value corresponding to each sector; generating a fusion description map of the current frame based on the intensity representative values ​​and distance representative values ​​corresponding to all sectors; and detecting whether the movement trajectory of the mobile carrier satisfies the loop closure detection conditions based on the fusion description map.

[0007] In one optional embodiment of this application, the polar coordinate projection map further includes multiple region blocks. The multiple region blocks are obtained by dividing the polar coordinate projection map along the polar radius direction with the pole as the starting point and according to a preset polar radius coordinate unit. Each sector in the polar coordinate projection map includes an equal number of region blocks.

[0008] In one optional embodiment of this application, the intensity representative value corresponding to each sector is determined in the following way: based on the position of each edge point, the edge point located in each region block is determined; for each sector, the intensity value corresponding to the edge point in each region block under that sector is determined, and the maximum intensity value is selected as the intensity representative value corresponding to that region block.

[0009] In one optional embodiment of this application, the distance representative value corresponding to each sector is determined in the following way: for each sector, the edge points of the sector are determined, and all edge points in the sector are clustered to form multiple feature clusters; for each sector, the feature cluster closest to the pole in the sector is selected, and the distance value between the centroid of the feature cluster and the pole is determined as the distance representative value corresponding to the region block.

[0010] In one optional embodiment of this application, the step of selecting the feature cluster closest to the pole in each sector and determining the distance between the centroid of the feature cluster and the pole as the distance representative value corresponding to the region block includes: for all sectors, starting from the sector containing the feature cluster closest to the pole, determining in a preset order whether there is a feature cluster closest to the pole in each sector; for each sector, if there is a feature cluster closest to the pole in the sector, recording the distance between the centroid of the feature cluster and the pole as the distance representative value corresponding to the sector; for each sector, if there is no feature cluster closest to the pole in the sector, recording the distance representative value corresponding to the sector as a fixed constant.

[0011] In one optional embodiment of this application, detecting whether the movement trajectory of the mobile carrier satisfies the closed-loop detection condition based on the fused description map includes: binarizing the fused description map of the current frame to obtain a binary matrix of the current frame, the binary matrix including a distance binary matrix and an intensity binary matrix, each column of the binary matrix corresponding to each sector, the row data in the distance binary matrix representing the distance representative value corresponding to each sector, and each row data in the intensity binary matrix representing the intensity representative value corresponding to each region block; determining the total detection score based on the binary matrix of the current frame and the historical binary matrix; determining whether the total detection score is greater than a detection score threshold; if the total detection score is greater than the detection score threshold, determining that the movement trajectory of the mobile carrier satisfies the closed-loop detection condition; if the total detection score is not greater than the detection score threshold, determining that the movement trajectory of the mobile carrier does not satisfy the closed-loop detection condition.

[0012] In one optional embodiment of this application, the historical binary matrix includes a historical distance binary matrix and a historical intensity binary matrix. The total detection score is determined as follows: The historical distance binary matrix in the historical fusion description map is obtained; a distance similarity score is determined based on the dot product of the historical distance binary matrix and the distance binary matrix of the current frame, the sum of all historical distance representative values ​​in the historical distance binary matrix, and the sum of all distance representative values ​​in the distance binary matrix; the historical intensity binary matrix in the historical fusion description map is obtained; an XOR operation is performed on the historical intensity binary matrix and the target intensity binary matrix to determine the XOR data; a geometric similarity score is determined based on the XOR data and the total number of elements in the target intensity binary matrix; an intensity similarity score is determined based on the sum of the dot products of each column in the historical intensity binary matrix and the target intensity binary matrix, the sum of the historical intensity representative values ​​corresponding to each column in the historical intensity binary matrix, and the sum of the intensity representative values ​​corresponding to each column in the target intensity binary matrix; and the total detection score is determined based on the weight values ​​corresponding to the distance similarity score, the geometric similarity score, and the intensity similarity score.

[0013] In one optional embodiment of this application, the target intensity binary matrix is ​​determined as follows: based on the offset angle of the lidar during the movement of the mobile carrier, the viewing angle change value between the intensity binary matrix and the historical intensity binary matrix is ​​determined, wherein the viewing angle change value represents that each column element in the intensity binary matrix is ​​moved to the right / left by a preset number of columns to correspond to the elements in the historical intensity binary matrix; based on the viewing angle change value, each column in the intensity binary matrix is ​​moved by a preset number of columns in a preset direction, and the moved intensity binary matrix is ​​determined as the target intensity binary matrix.

[0014] Secondly, embodiments of this application also provide a closed-loop detection device, the device comprising: an acquisition module, configured to acquire laser point cloud data of the current frame collected by a lidar, the lidar being mounted on a mobile carrier, the laser point cloud data including point cloud data of multiple detection points detected by the lidar during the movement of the mobile carrier; a selection module, configured to select edge points from the multiple detection points based on the laser point cloud data of the current frame; a construction module, configured to construct a polar coordinate projection map of the current frame, the polar coordinate projection map having the location of the lidar as the pole, the selected edge points being distributed in the polar coordinate projection map according to their positional relationship with the lidar, the polar coordinate projection map being divided into multiple sectors; a determination module, configured to determine the intensity representative value and distance representative value corresponding to each sector; a generation module, configured to generate a fused description map of the current frame based on the intensity representative values ​​and distance representative values ​​corresponding to all sectors; and a detection module, configured to detect whether the movement trajectory of the mobile carrier satisfies the closed-loop detection conditions based on the fused description map.

[0015] Thirdly, embodiments of this application also provide an electronic device, including: a processor, a memory, and a bus, wherein the memory stores machine-readable instructions executable by the processor, and when the electronic device is running, the processor communicates with the memory via the bus, and when the machine-readable instructions are executed by the processor, the steps of the method described above are performed.

[0016] The closed-loop detection method and apparatus provided in this application acquire laser point cloud data of the current frame collected by a lidar, select edge points from multiple detection points based on the laser point cloud data of the current frame, construct a polar coordinate projection map of the current frame with the lidar location as the pole, and distribute the selected edge points according to their positional relationship with the lidar in the polar coordinate projection map, which is divided into multiple sectors; for each sector, determine the corresponding intensity representative value and distance representative value; generate a fusion description map of the current frame based on the intensity representative values ​​and distance representative values ​​corresponding to all sectors; and detect whether the movement trajectory of the moving vehicle meets the closed-loop detection conditions based on the fusion description map. This application improves the accuracy of the detection results regarding whether the movement trajectory of the moving vehicle meets the closed-loop detection conditions.

[0017] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0018] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 This is a flowchart of the closed-loop detection method provided in the embodiments of this application;

[0020] Figure 2 One of the schematic diagrams of polar coordinate projection provided in the embodiments of this application;

[0021] Figure 3 This is a schematic diagram of a one-dimensional distance matrix provided in an embodiment of this application;

[0022] Figure 4 This is a second schematic diagram of a polar coordinate projection provided in an embodiment of this application;

[0023] Figure 5 This is a schematic diagram of the closed-loop detection device provided in the embodiments of this application;

[0024] Figure 6 The present application provides a schematic diagram of the structure of an electronic device. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application 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 this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. Based on the embodiments of this application, every other embodiment obtained by those skilled in the art without inventive effort falls within the scope of protection of this application.

[0026] First, the applicable application scenarios of this application will be introduced. This application can be applied to the field of laser positioning technology.

[0027] Research has found that in loop closure detection, a descriptor is a feature vector or set of features used to represent and compare features between point clouds or other data. Descriptors typically contain unique features that help distinguish different locations and can be effectively used to match and identify scenes that have already been seen. Loop closure detection relies heavily on robust descriptor construction.

[0028] Link3D descriptors are built upon the geometric features of point clouds, such as edge features. However, in extreme environments such as indoor spaces or corridors, there are relatively simple and repetitive geometric structures, such as long and flat walls, floors, and ceilings. These structures lack rich feature points, and feature point-based descriptors rely on extracting unique feature points for matching. In such environments, it is difficult to find enough significant feature points, which leads to a scarcity of descriptors and affects the accuracy and reliability of matching.

[0029] Based on this, embodiments of this application provide a closed-loop detection method and apparatus. The method involves acquiring laser point cloud data of the current frame collected by a lidar, selecting edge points from multiple detection points based on the laser point cloud data of the current frame, constructing a polar coordinate projection map of the current frame with the lidar's location as the pole, distributing the selected edge points according to their positional relationship with the lidar, and dividing the polar coordinate projection map into multiple sectors. For each sector, the method determines the corresponding intensity representative value and distance representative value. Based on the intensity representative values ​​and distance representative values ​​of all sectors, a fused description map of the current frame is generated. Based on the fused description map, the method detects whether the movement trajectory of the mobile vehicle meets the closed-loop detection conditions. This application improves the accuracy of the detection results regarding whether the movement trajectory of the mobile vehicle meets the closed-loop detection conditions.

[0030] Please see Figure 1 , Figure 1 This is a flowchart illustrating the closed-loop detection method provided in an embodiment of this application. Figure 1 As shown in the embodiments of this application, the closed-loop detection method includes:

[0031] S101. Obtain the laser point cloud data of the current frame collected by the lidar.

[0032] The lidar is installed on a mobile vehicle, and the lidar point cloud data includes point cloud data of multiple detection points detected by the lidar during the movement of the mobile vehicle.

[0033] For example, the mobile carrier can be an Automated Guided Vehicle (AGV), which is used in industrial automation and logistics transportation. The laser point cloud data acquisition frame is determined according to preset conditions. For example, it can be after a preset time, or the mobile carrier moves to a preset distance or preset angle, etc. It can be determined according to actual needs and is not specifically limited here.

[0034] Point cloud data from multiple detection points includes the coordinates of the multiple detection points and the intensity value detected at each detection point location.

[0035] S102. Select edge points from multiple detection points based on the laser point cloud data of the current frame.

[0036] In this step, the local geometric features of the laser point cloud data are analyzed, and multiple detection points are classified into two types: edge points and planar points. This distinction is mainly based on the calculation and analysis of the curvature of the detection points.

[0037] First, the laser point cloud data is filtered and preprocessed to remove noise and reduce the amount of data. Here, for each detection point, the detection points around the detection point are searched. If there are a preset number of detection points around the detection point, it means that the detection point is meaningful. If there are no preset number of detection points around the detection point, the detection point is considered to be a discrete point that will generate noise, and the detection point is filtered out.

[0038] Then, for each filtered probe point, the normal vector is calculated, and the neighboring probe points of each probe point are found. The covariance matrix is ​​calculated through these neighboring probe points, and eigenvalue decomposition is performed. The result of eigenvalue decomposition is used to calculate the curvature value of each probe point.

[0039] Based on the curvature value of each detection point, points with curvature values ​​greater than a preset curvature value are marked as edge points. These points are typically located on the edges or contours of an object and exhibit significant geometric changes compared to before the moving vehicle moved. Points with curvature values ​​less than the preset curvature value are marked as planar points. These points are typically located on the flat surfaces of an object and exhibit less geometric change compared to before the moving vehicle moved. By accurately identifying and distinguishing edge points and planar points in point cloud data, the accuracy and efficiency of point cloud processing and matching are improved.

[0040] S103. Construct the polar coordinate projection map of the current frame.

[0041] The polar coordinate projection map uses the location of the lidar as the pole, and the selected edge points are distributed on the polar coordinate projection map according to their positional relationship with the lidar. The polar coordinate projection map is divided into multiple sectors.

[0042] By distributing the selected edge points to the polar coordinate plane, the effect of uneven distribution of edge points along the Z-axis can be eliminated. This transforms the edge points from the Cartesian coordinate system to the polar coordinate system, i.e., edge point p i (x i y i , z i η i Convert ) to p′ i =(ρ i θ i ,0,η i ), where x, y, z represent coordinates, η i ρ represents the intensity value corresponding to the i-th edge point. i and θ i P respectively i The angle between the distance from the origin to the polar axis and the polar axis.

[0043] S104. For each sector, determine the representative intensity value and the representative distance value corresponding to that sector.

[0044] The distance representation value corresponding to each sector is determined in the following way:

[0045] For each sector, the edge points within that sector are identified, and all edge points within the sector are clustered to form multiple feature clusters;

[0046] For each sector, select the feature cluster that is closest to the pole in that sector, and determine the distance between the centroid of the feature cluster and the pole as the representative distance value for that region block.

[0047] Clustering is performed for each edge point within each sector. Initially, each edge point is treated as a separate cluster. For each edge point, its distance to other edge points is calculated, and edge points that are close in distance are grouped into the same cluster according to the rules of the clustering algorithm.

[0048] For all sectors, starting from the sector containing the feature cluster closest to the pole, determine in a preset order whether there is a feature cluster closest to the pole in each sector.

[0049] For each sector, if there is a feature cluster that is closest to the pole within that sector, then the distance between the centroid of that feature cluster and the pole is recorded as the representative distance value for that sector.

[0050] For each sector, if there is no feature cluster closest to the pole within that sector, then the distance representative value corresponding to that sector is recorded as a fixed constant.

[0051] Please see Figure 2 , Figure 2 This is one of the schematic diagrams of polar coordinate projection provided in the embodiments of this application.

[0052] like Figure 2 As shown, each sector in the polar coordinate projection map is uniformly distributed on the polar coordinate projection map. For example, the polar coordinate projection map is circular and is divided into 180-dimensional sectors according to the polar angle. Starting from k0, each sector region intensity has a distance representative value. Taking the direction vector of the centroid of the feature cluster closest to the pole as the main direction, in a counterclockwise order, it is determined whether there is a feature cluster closest to the pole in each sector. In each sector, the blue point is the centroid of the feature cluster formed by the edge point of the sector, and the yellow point is the centroid of the feature cluster formed by the edge point closest to the pole.

[0053] Here, if there is no feature cluster closest to the pole within the sector, the distance representative value corresponding to the sector is recorded as a fixed constant. For example, the fixed constant is 0. The sector and distance representative value corresponding to the centroid of the feature cluster can be determined by calculating the polar coordinates of the centroid of the feature cluster.

[0054] For example, with Figure 2For example, where k0 is the pole and k1 is the centroid of the feature cluster closest to k0, the search proceeds counterclockwise with k0-k1 as the main direction. If there is no feature cluster that meets the conditions in the third sector, the corresponding distance value is 0. In the final generated one-dimensional matrix, D1 is the distance from k1 to k0, D2 is the distance from k2 to k0, and D3 is 0 because there is no feature cluster that meets the conditions in the third sector.

[0055] To reduce the impact of anomalously nearest feature cluster centroids on the distance one-dimensional matrix, we search for the three nearest feature cluster centroids. (See [link to relevant documentation]). Figure 3 , Figure 3 This is a schematic diagram of a one-dimensional distance matrix provided in an embodiment of this application. Figure 3 As shown, Des1 is the one-dimensional distance matrix corresponding to the centroid of the nearest feature cluster, and Des2 is the one-dimensional distance matrix corresponding to the centroid of the second nearest feature cluster. Des1 has the highest priority because it is closest to the extrema, and Des3 has the lowest priority. In the final one-dimensional distance matrix, the distance representation value of each sector corresponds to the non-zero value with the highest priority. If there are no values, it is 0. Figure 3 As shown in the dashed box, Des1, Des2, and Des3 all contain values ​​in the first dashed box. However, since Des1 is the one-dimensional distance matrix of the feature cluster centroid to the nearest pole, Des2 is the one-dimensional distance matrix of the feature cluster centroid to the second nearest pole, and Des3 is the one-dimensional distance matrix of the feature cluster centroid to the third nearest pole, Des1 has the highest priority. The first element of Final Des is an element from Des1.

[0056] Please see Figure 4 , Figure 4 This is a second schematic diagram of a polar coordinate projection provided in an embodiment of this application. For example... Figure 4 As shown, the polar coordinate projection map also includes multiple region blocks. These region blocks are obtained by dividing the polar coordinate projection map along the polar radius direction with the pole as the starting point, according to a preset polar radius coordinate unit. Each sector in the polar coordinate projection map includes an equal number of region blocks. For example, the polar coordinate projection map is divided into Ns sectors and Nr region blocks.

[0057] like Figure 4 As shown, the blue points in each sector are the centroids of the feature clusters formed by the edge points of that sector, the green points in each sector are the centroids of the feature clusters formed by the edge points closest to the pole, and the yellow points are the centroids of the feature clusters closest to the pole in all sectors.

[0058] The intensity representative value corresponding to each sector is determined in the following way:

[0059] Based on the location of each edge point, determine the edge points located within each region block.

[0060] For each sector, determine the intensity value corresponding to the edge point within each region block under that sector, and select the maximum intensity value as the representative intensity value corresponding to that region block.

[0061] The edge points located within the region block are filtered out, and the maximum intensity value is selected as the representative intensity value of the region block based on the intensity value corresponding to each edge point within each region block.

[0062] Each region is small relative to the entire point cloud area. Considering the strong correlation between object reflectivity, echo intensity, and object surface material, it is assumed that the echo intensity values ​​within each region are not significantly different. The maximum intensity value within a region is taken as the echo intensity of the entire region, thus obtaining a representative intensity value. The generated two-dimensional intensity matrix reflects the intensity distribution of the environment.

[0063] S105. Generate a fusion description map for the current frame based on the intensity representative value and distance representative value corresponding to all sectors.

[0064] Specifically, the intensity representative value can be determined based on the L value composed of each sector. max A 180-dimensional intensity 2D matrix is ​​used, where rows correspond to each region block and columns correspond to each sector. Each element in the intensity 2D matrix corresponds to the intensity value of a predefined region block within a predefined sector. For example, the predefined polar radius coordinate unit for each region block is 1, so each sector contains L... max A block of regions, L max This represents the maximum detection range of the lidar, which is also the radius of the polar coordinate projection map.

[0065] Specifically, the distance representative value can be formed into a 1×180-dimensional distance one-dimensional matrix based on each sector. The rows of the distance one-dimensional matrix are one row, the columns of the distance one-dimensional matrix correspond to each sector, and the elements in the distance one-dimensional matrix correspond to the distance representative value between the centroid of the feature cluster closest to the pole in the preset sector and the pole.

[0066] By merging the two-dimensional intensity matrix and the one-dimensional distance matrix, we obtain (L max A two-dimensional matrix with dimensions of +1)×180, i.e., a fusion description graph.

[0067] For example, if the maximum detection range of a lidar point cloud is 90 meters and the polar radius index is set to 90, and the 360-degree point cloud is divided into 180 dimensions according to the polar angle, then a 90×180-dimensional intensity two-dimensional matrix is ​​created using the intensity representative value of the region block, and a 1×180-dimensional distance one-dimensional matrix is ​​created using the distance representative value of each sector. The two matrices are superimposed according to the sectors to generate a 91×180 two-dimensional matrix.

[0068] By combining distance and intensity representative values ​​to construct a fusion description map, the complementarity of geometric and intensity features can be fully utilized to enhance the discriminative power of the fusion description map. The distance representative value provides the geometric structure of the scene, while the intensity representative value captures the surface reflection characteristics, thereby improving feature richness and anti-interference ability. Through this combination, the accuracy and robustness of loop closure detection can be improved in environments with scarce features and high repetition, reducing the false matching rate and enhancing the positioning accuracy of the system.

[0069] S106. Based on the fusion description diagram, detect whether the movement trajectory of the mobile carrier meets the closed-loop detection conditions.

[0070] As the mobile vehicle moves, it will construct a fusion description map reflecting the current environment by performing operations such as edge point determination, intensity representative value determination, and distance representative value determination on the point cloud data acquired at the current location, and store it in the historical database. At the same time, it will retrieve previous fusion description maps from the historical database and perform similarity matching.

[0071] The fused description map of the current frame is binarized to obtain the binary matrix of the current frame.

[0072] As the organism continues to move forward, it continuously creates fusion description maps. The size of the historical database and the computational load for similarity matching will continue to increase. In order to reduce computational costs, we adopted a three-stage hierarchical retrieval strategy and verification method, which accelerates the process of fusion description map similarity matching through fast binary operations.

[0073] Compared to operations involving floating-point multiplication, such as histogram matching, binary operations (e.g., bitwise operations or logical operations) typically achieve faster computation speeds. We introduce an efficient binary matching strategy, given a (L... max A fusion description graph M with dimensions +1)×180, located before the fusion description graph L. max The ×180 dimension stores the intensity representation values ​​as integers from 0 to 255. The 1×180 dimension after fusion description map stores the normalized distance values. The fused description map M is binarized to obtain the binary matrix I using the following formula:

[0074]

[0075] Where x and y are row and column indices. If an element in the fused description graph M is 0, the corresponding element in the binary matrix is ​​set to false; otherwise, it is set to true. Here, false is denoted as 0 and true is denoted as 1.

[0076] The binary matrix includes a distance binary matrix and an intensity binary matrix. Each column of the binary matrix corresponds to each sector. The row data in the distance binary matrix represents the distance value corresponding to each sector, and the row data in the intensity binary matrix represents the intensity value corresponding to each region block.

[0077] The total detection score is determined based on the binary matrix of the current frame and the historical binary matrices.

[0078] Here, the historical binary matrix includes the historical distance binary matrix and the historical intensity binary matrix.

[0079] The total test score is determined in the following manner:

[0080] Obtain the historical distance binary matrix from the historical fusion description map. Determine the distance similarity score based on the dot product of the historical distance binary matrix and the current frame's distance binary matrix, the sum of all historical distance representative values ​​in the historical distance binary matrix, and the sum of all distance representative values ​​in the distance binary matrix.

[0081] Because compared to L max Operations on a binary strength matrix of ×180 dimensions are more efficient than operations on a binary distance matrix of 1×180 dimensions. In distance similarity calculations, the shifting of column vectors is used to explore different alignment methods.

[0082] Let M be the historical fusion description map corresponding to the current location. q The fusion description graph searched from the historical database is denoted as M. c M q and M c The data is decomposed to obtain a one-dimensional distance matrix, with 1×180 dimensions, denoted as the one-dimensional distance matrix D. q One-dimensional matrix D of distance from history c The distance similarity score can be obtained through cosine similarity, and the distance similarity score is determined by the following formula:

[0083]

[0084] Among them, SCORE d (D q D c ) represents the distance similarity score from the one-dimensional distance matrix, D q ×D c D represents the distance matrix. q One-dimensional matrix D of distance from history c The dot product, ||D q || represents D q The distance between all elements in ||D represents the sum of values. c || represents D cThe historical distance representation of all elements in the table is the sum of their values.

[0085] Cosine similarity measures whether two vectors have the same direction in a vector space. Its value ranges from -1 to 1, where 1 represents completely identical directions (highest similarity), 0 represents perpendicular directions (no similarity), and -1 represents completely opposite directions (lowest similarity). For a 1×180 dimension distance matrix, the similarity score can be directly calculated without column shifting or other complex operations, thus offering good efficiency.

[0086] Obtain the historical intensity binary matrix from the historical fusion description map, and perform XOR processing on the historical intensity binary matrix and the target intensity binary matrix to determine the XOR data.

[0087] The fusion description map M corresponding to the current position q The historical fusion description graph M, which is searched in the historical database. c Decomposition yields a two-dimensional intensity matrix, L max The ×180 dimension data is denoted as the intensity two-dimensional matrix A. q and historical intensity two-dimensional matrix A c Geometric similarity is calculated using the following formula:

[0088]

[0089] Where, δ g (A q A c XOR(A) represents the geometric similarity of the two-dimensional intensity matrices. q A c The XOR symbol in ) represents the intensity two-dimensional matrix A q and historical intensity two-dimensional matrix A c The XOR operation between them, ||A q || represents the two-dimensional intensity matrix A q The total number of elements corresponding to it.

[0090] The geometric similarity score is determined based on the XOR data and the total number of elements in the target intensity binary matrix.

[0091] Since each column in the fusion description graph represents a specific azimuth angle, when the lidar rotates, the direction of the laser beam changes, which is equivalent to a change in the order of the columns in the matrix. For example, if the lidar rotates by 10 degrees, it is equivalent to all columns in the fusion description graph shifting to the right or left by a certain number of columns. For position recognition, the change in viewing angle can be interpreted as the first L columns in the matrix shifting to the left or right. maxColumn shifting of rows means that the columns in these rows will shift when the viewpoint changes. Therefore, by using column shifting operations, this rotation can be aligned to a uniform reference frame for comparison.

[0092] The intensity two-dimensional matrix in the moved fusion description map is determined as the target intensity two-dimensional matrix, and the target intensity binary matrix is ​​determined in the following way:

[0093] Based on the offset angle of the lidar during the movement of the mobile carrier, the viewpoint change value between the intensity binary matrix and the historical intensity binary matrix is ​​determined.

[0094] The perspective change value represents the preset number of columns that each column element in the intensity binary matrix moves to the right / left, corresponding to the elements in the historical intensity binary matrix.

[0095] Based on the change in viewing angle, each column of the intensity binary matrix is ​​moved a preset number of columns in a preset direction, and the moved intensity binary matrix is ​​determined as the target intensity binary matrix.

[0096] The geometric similarity score is determined using the following formula:

[0097]

[0098] Among them, SCORE g (A q A c A represents the geometric similarity score of the binary matrix of target intensity. m q Represents the intensity two-dimensional matrix A q The target intensity binary matrix is ​​obtained by moving each column in the preset direction by a preset number of columns, where m represents the preset number of columns moved, 1≤m≤Ns, and Ns represents the maximum number of columns in the intensity binary matrix.

[0099] Multiple target intensity binary matrices are determined by moving the matrix according to the column number. For each target intensity binary matrix, the geometric similarity is recalculated, and the maximum geometric similarity is determined as A. m q The geometric similarity score is recorded, and the value of m is determined. m is the number of columns that the mobile carrier needs to move to align with the LiDAR before the movement during the movement.

[0100] The intensity similarity score is determined by summing the dot products of each column in the historical intensity binary matrix and the target intensity binary matrix, summing the historical intensity representative values ​​corresponding to each column in the historical intensity binary matrix, and summing the intensity representative values ​​corresponding to each column in the target intensity binary matrix.

[0101] In strength structure matching, the historical fusion description map M is mainly identified through column-by-column comparison.q Description of historical integration M c The intensity similarity is set to V. i q and V i c M respectively q and M c For the i-th column, the intensity similarity can be calculated by taking the cosine distance, and the intensity similarity is determined in the following way:

[0102]

[0103] Where, δ i (M q M c V represents the intensity similarity of the i-th column of the two-dimensional intensity matrix. i q ×V i c V represents the dot product of the i-th column of the intensity 2D matrix and the i-th column of the historical intensity 2D matrix. i q || represents D q The distance between all elements in the i-th column represents the sum of values, ||D c || represents D c The historical distance representation of all elements in the i-th column is the sum of their historical distance values, where 0 ≤ i ≤ Ns-1, and i = 0 represents M. q In the first column, i = Ns-1 represents M q The last column.

[0104] The intensity similarity score is determined using the following method:

[0105]

[0106] Among them, SCORE i (M q M c ) represents the intensity similarity score of the fused description map, M m q This represents a two-dimensional matrix after each column in the fusion description diagram has been shifted m columns in a preset direction.

[0107] Example, M q A row in the array contains [a, b, c, d], with m equal to 1. The default direction is right. Shifting one column to the right will result in the value M. m q = [d, a, b, c].

[0108] The total detection score is determined based on the weight values ​​corresponding to the distance similarity score, geometric similarity score, and intensity similarity score.

[0109] Obtain the weight values ​​corresponding to the distance similarity score, geometric similarity score, and intensity similarity score respectively. Multiply the distance similarity score, geometric similarity score, and intensity similarity score by their respective weight values, and sum the products of the three scores to obtain the total detection score. Here, the weight values ​​are set according to actual needs.

[0110] Specifically, it determines whether the total detection score is greater than the detection score threshold, which can be set according to actual needs.

[0111] If the total detection score is greater than the detection score threshold, the detection result is determined to be that the movement trajectory of the mobile vehicle meets the closed-loop detection condition. If the total detection score is not greater than the detection score threshold, the detection result is determined to be that the movement trajectory of the mobile vehicle does not meet the closed-loop detection condition.

[0112] In this step, if the movement trajectory of the mobile vehicle does not meet the closed-loop detection conditions, a prompt will be issued, indicating that the mobile vehicle and the lidar need to be optimized.

[0113] The similarity scores from three aspects are considered to assess whether a closed loop exists. Distance similarity indicates the consistency of distance between points in the trajectory space, geometric similarity considers the consistency of the spatial structure and geometry of the point cloud, and intensity similarity assesses the similarity of intensity features in the data captured by the sensor.

[0114] By analyzing these three scores, it can be determined whether the current point cloud data forms a closed loop on the trajectory. Specifically, the distance similarity score indicates that there is a consistent distance pattern between points on the trajectory, the geometric similarity score indicates the consistency of the spatial structure, and the intensity similarity score reflects the consistency characteristics of the sensor data.

[0115] In summary, by comprehensively considering the scores of these three aspects, we can effectively determine whether the current point cloud data has formed a closed loop, thus providing important real-time feedback and correction for the subsequent SLAM localization and map building process.

[0116] Based on the principle that intensity readings of the same object are consistent, this application uses intensity representative values ​​that reflect the structural information of the surrounding environment. This allows it to not only utilize robust feature information but also use intensity representative values ​​in environments lacking feature information, thus providing dual constraints to improve the accuracy of loop closure recognition and matching. Simultaneously, it leverages the principle that binary operations are more efficient to accelerate the matching of the fused description map.

[0117] The closed-loop detection method and apparatus provided in this application acquire laser point cloud data of the current frame collected by a lidar, and select edge points from multiple detection points based on the laser point cloud data of the current frame. Then, a polar coordinate projection map of the current frame is constructed, with the lidar location as the pole. The selected edge points are distributed in the polar coordinate projection map according to their positional relationship with the lidar. The polar coordinate projection map is divided into multiple sectors. For each sector, a representative intensity value and a representative distance value are determined. Based on the representative intensity values ​​and distance values ​​corresponding to all sectors, a fused description map of the current frame is generated. Based on the fused description map, the movement trajectory of the moving vehicle is detected to determine whether it meets the closed-loop detection conditions. This application improves the accuracy of the detection results regarding whether the movement trajectory of the moving vehicle meets the closed-loop detection conditions.

[0118] Based on the same inventive concept, this application also provides a closed-loop detection device corresponding to the closed-loop detection method. Since the principle of the device in this application is similar to the closed-loop detection method described above in this application, the implementation of the device can refer to the implementation of the method, and the repeated parts will not be described again.

[0119] Please see Figure 5 , Figure 5 This is a schematic diagram of the closed-loop detection device provided in an embodiment of this application. Figure 5 As shown, the device 500 includes:

[0120] The acquisition module 501 is used to acquire the laser point cloud data of the current frame collected by the lidar. The lidar is installed on a mobile carrier, and the laser point cloud data includes point cloud data of multiple detection points detected by the lidar during the movement of the mobile carrier.

[0121] The selection module 502 is used to select edge points from the plurality of detection points based on the laser point cloud data of the current frame;

[0122] The construction module 503 is used to construct the polar coordinate projection map of the current frame. The polar coordinate projection map takes the location of the lidar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to their positional relationship with the lidar. The polar coordinate projection map is divided into multiple sectors.

[0123] The determination module 504 is used to determine the intensity representative value and distance representative value corresponding to each sector;

[0124] The generation module 505 is used to generate a fusion description map of the current frame based on the intensity representative value and distance representative value corresponding to all sectors.

[0125] The detection module 506 is used to detect whether the movement trajectory of the mobile carrier meets the closed-loop detection conditions based on the fused description map.

[0126] Please see Figure 6 , Figure 6 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Figure 6 As shown, the electronic device 600 includes a processor 610, a memory 620, and a bus 630.

[0127] The memory 620 stores machine-readable instructions executable by the processor 610. When the electronic device 600 is running, the processor 610 and the memory 620 communicate via the bus 630. When the machine-readable instructions are executed by the processor 610, they can perform the operations described above. Figure 1 The steps of the closed-loop detection method in the illustrated method embodiment can be found in the method embodiment for specific implementation, and will not be repeated here.

[0128] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0129] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods 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 shown 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.

[0130] 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.

[0131] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0132] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0133] Finally, it should be noted that the above-described embodiments are merely specific implementations of this application, used to illustrate the technical solutions of this application, and not to limit them. The scope of protection of this application is not limited thereto. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments, or make equivalent substitutions for some of the technical features, within the scope of the technology disclosed in this application. Such modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be covered within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A closed-loop detection method, characterized in that, include: Acquire laser point cloud data of the current frame collected by a lidar, wherein the lidar is mounted on a mobile carrier, and the laser point cloud data includes point cloud data of multiple detection points detected by the lidar during the movement of the mobile carrier. Based on the laser point cloud data of the current frame, edge points are selected from the plurality of detection points; Construct a polar coordinate projection map of the current frame. The polar coordinate projection map takes the location of the lidar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to their positional relationship with the lidar. The polar coordinate projection map is divided into multiple sectors. For each sector, determine the representative intensity value and the representative distance value corresponding to that sector; Based on the intensity and distance representative values ​​corresponding to all sectors, generate the fusion description map of the current frame; The fusion description map of the current frame is binarized to obtain a binary matrix of the current frame. The binary matrix includes a distance binary matrix and an intensity binary matrix. Each column of the binary matrix corresponds to each sector. The row data in the distance binary matrix represents the distance representative value corresponding to each sector. The row data in the intensity binary matrix represents the intensity representative value corresponding to each region block. The total detection score is determined based on the binary matrix of the current frame and the historical binary matrix. Determine whether the total detection score is greater than the detection score threshold; If the total detection score is greater than the detection score threshold, the detection result is determined to be that the movement trajectory of the mobile carrier meets the closed-loop detection condition. If the total detection score is not greater than the detection score threshold, the detection result is determined to be that the movement trajectory of the mobile carrier does not meet the closed-loop detection condition.

2. The method according to claim 1, characterized in that, The polar coordinate projection map also includes multiple region blocks. These region blocks are obtained by dividing the polar coordinate projection map along the polar radius direction with the pole as the starting point and according to a preset polar radius coordinate unit. Each sector in the polar coordinate projection map includes the same number of region blocks.

3. The method according to claim 2, characterized in that, The intensity representative value corresponding to each sector is determined in the following way: Based on the location of each edge point, determine the edge points located within each region block; For each sector, determine the intensity value corresponding to the edge point within each region block under that sector, and select the maximum intensity value as the representative intensity value corresponding to that region block.

4. The method according to claim 3, characterized in that, The distance representation value corresponding to each sector is determined in the following way: For each sector, the edge points within that sector are identified, and all edge points within the sector are clustered to form multiple feature clusters; For each sector, select the feature cluster that is closest to the pole in that sector, and determine the distance between the centroid of the feature cluster and the pole as the distance representative value corresponding to that region block.

5. The method according to claim 4, characterized in that, For each sector, selecting the feature cluster closest to the pole within that sector and determining the distance between the centroid of that feature cluster and the pole as the representative distance value for that region block includes: For all sectors, starting from the sector containing the feature cluster closest to the pole, determine in a preset order whether there is a feature cluster closest to the pole in each sector; For each sector, if there is a feature cluster that is closest to the pole within that sector, then the distance between the centroid of that feature cluster and the pole is recorded as the representative distance value for that sector. For each sector, if there is no feature cluster closest to the pole in that sector, then the distance representative value corresponding to that sector is recorded as a fixed constant.

6. The method according to claim 1, characterized in that, The historical binary matrix includes a historical distance binary matrix and a historical intensity binary matrix. The total detection score is determined in the following way: Obtain the historical distance binary matrix from the historical fusion description map. Determine the distance similarity score based on the dot product of the historical distance binary matrix and the current frame's distance binary matrix, the sum of all historical distance representative values ​​in the historical distance binary matrix, and the sum of all distance representative values ​​in the distance binary matrix. Obtain the historical intensity binary matrix from the historical fusion description map, and perform XOR processing on the historical intensity binary matrix and the target intensity binary matrix to determine the XOR data; The geometric similarity score is determined based on the XOR data and the total number of elements in the target intensity binary matrix; The intensity similarity score is determined based on the sum of the dot products of each column in the historical intensity binary matrix and the target intensity binary matrix, the sum of the historical intensity representative values ​​corresponding to each column in the historical intensity binary matrix, and the sum of the intensity representative values ​​corresponding to each column in the target intensity binary matrix. The total detection score is determined based on the weight values ​​corresponding to the distance similarity score, the geometric similarity score, and the intensity similarity score.

7. The method according to claim 6, characterized in that, The target intensity binary matrix is ​​determined in the following manner: Based on the offset angle of the lidar during the movement of the mobile carrier, the viewing angle change value between the intensity binary matrix and the historical intensity binary matrix is ​​determined. The viewing angle change value represents the number of columns of each element in the intensity binary matrix that are moved to the right or left to correspond to the elements in the historical intensity binary matrix. Based on the change in viewing angle, each column of the intensity binary matrix is ​​moved a preset number of columns in a preset direction, and the moved intensity binary matrix is ​​determined as the target intensity binary matrix.

8. A closed-loop detection device, characterized in that, include: The acquisition module is used to acquire the laser point cloud data of the current frame collected by the lidar. The lidar is installed on a mobile carrier, and the laser point cloud data includes the point cloud data of multiple detection points detected by the lidar during the movement of the mobile carrier. The selection module is used to select edge points from the plurality of detection points based on the laser point cloud data of the current frame; The construction module is used to construct the polar coordinate projection map of the current frame. The polar coordinate projection map takes the location of the lidar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to their positional relationship with the lidar. The polar coordinate projection map is divided into multiple sectors. The determination module is used to determine the representative intensity value and the representative distance value for each sector. The generation module is used to generate a fusion description map of the current frame based on the intensity representative value and distance representative value corresponding to all sectors; The detection module is used to binarize the fused description map of the current frame to obtain a binary matrix of the current frame. The binary matrix includes a distance binary matrix and an intensity binary matrix. Each column of the binary matrix corresponds to each sector. The row data in the distance binary matrix represents the distance representative value corresponding to each sector, and the row data in the intensity binary matrix represents the intensity representative value corresponding to each region block. Based on the binary matrix of the current frame and the historical binary matrices, the total detection score is determined. Determine whether the total detection score is greater than the detection score threshold; If the total detection score is greater than the detection score threshold, the detection result is determined to be that the movement trajectory of the mobile vehicle meets the closed-loop detection condition; if the total detection score is not greater than the detection score threshold, the detection result is determined to be that the movement trajectory of the mobile vehicle does not meet the closed-loop detection condition.

9. An electronic device, characterized in that, include: The device includes a processor, a memory, and a bus, wherein the memory stores machine-readable instructions executable by the processor, and when the electronic device is in operation, the processor communicates with the memory via the bus, and the processor executes the machine-readable instructions to perform the steps of the method as described in any one of claims 1 to 7.

Images