Closed-loop detection method and device
By using lidar in AGV to obtain laser point cloud data, construct polar coordinate projection maps and generate fusion description maps, the scarcity of closed-loop detection feature points in AGV is solved and the detection accuracy is improved.
Patent Information
- Application Number
- CN202411116183.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-14
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2044-08-14
AI Technical Summary
In automatic guided vehicles (AGV), closed-loop detection has a scarcity of characteristic points in environments such as indoors or corridors, which affects the accuracy and reliability of closed-loop detection.
By obtaining the current frame laser point cloud data collected by the lidar, selecting edge points and constructing polar coordinate projection maps, dividing them into sectors, determining the intensity and distance representative values of each sector, and generating a fusion description map to detect moving tracks Whether the closed-loop detection conditions are met.
The accuracy of the detection results of whether the mobile carrier's operating trajectory meets the closed-loop detection conditions is improved, and the closed-loop detection capability in a scarce environment of characteristic points is enhanced.
Smart Images

Figure CN119024349A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of laser positioning technology, and in particular to a closed-loop detection method and device. Background Art
[0002] At present, loop closure detection has an important background and significance in the automatic guided vehicle (AGV) scenario. AGV plays a key role in industrial automation and logistics transportation. Its core functions include accurate positioning and mapping. During the autonomous navigation of AGV, the positioning and mapping algorithms need to process a large amount of sensor data, such as lidar, camera, inertial measurement unit (IMU) and odometer. These sensors will inevitably produce cumulative errors, resulting in deviations in positioning and mapping, thus affecting the navigation accuracy and reliability of AGV.
[0003] Closed-loop detection can identify whether the AGV has returned to a location it has visited before during operation. When the AGV recognizes a closed loop, it can correct the accumulated errors and optimize the entire trajectory by matching the current environment with the previously built map. This not only helps eliminate sensor errors, but also ensures the consistency and accuracy of the map.
[0004] In closed-loop detection, the construction of Link3D descriptors relies on the geometric features of point clouds, such as edge features. However, in extreme environments such as indoors 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. In addition, it is difficult to find enough significant feature points in such environments, which will lead to a scarcity of descriptors, thereby affecting the accuracy and reliability of closed-loop detection. Summary of the invention
[0005] In view of this, an object of the present application is to provide a closed-loop detection method and device to overcome at least one of the above-mentioned defects.
[0006] In a first aspect, an embodiment of the present application provides a closed-loop detection method, the method comprising: obtaining laser point cloud data of a current frame collected by a laser radar, the laser radar being installed on a mobile carrier, the laser point cloud data comprising point cloud data of multiple detection points detected by the laser radar 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 taking the position of the laser radar as the pole, the selected edge points being distributed in the polar coordinate projection map according to their positional relationship with the laser radar, and the polar coordinate projection map being divided into multiple sectors; for each sector, determining an intensity representative value and a distance representative value corresponding to the 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 moving trajectory of the mobile carrier meets the closed-loop detection conditions based on the fusion description map.
[0007] In an optional embodiment of the present application, the polar coordinate projection diagram also includes multiple area blocks, which are obtained by dividing the polar coordinate projection diagram along the polar direction according to preset polar coordinate units with the pole as the starting point, and each sector in the polar coordinate projection diagram includes an equal number of area blocks.
[0008] In an optional embodiment of the present application, the intensity representative value corresponding to each sector is determined in the following manner: based on the position of each edge point, the edge points located in each area block are determined; for each sector, the intensity values corresponding to the edge points in each area block under the sector are determined, and the maximum intensity value is selected as the intensity representative value corresponding to the area block.
[0009] In an optional embodiment of the present application, the distance representative value corresponding to each sector is determined in the following manner: for each sector, the edge points in 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 area block.
[0010] In an optional embodiment of the present application, for each sector, a 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 area block, including: for all sectors, taking the sector where the feature cluster closest to the pole is located as the starting position, determining in sequence 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 value 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 an optional embodiment of the present application, detecting whether the moving trajectory of the mobile carrier meets the closed-loop detection condition according to the fusion description graph includes: binarizing the fusion description graph 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 corresponds to each sector, the row data in the distance binary matrix represents the distance representative value corresponding to each sector, and each row data in the intensity binary matrix represents the intensity representative value corresponding to each area block; determining a total detection score according to the binary matrix of the current frame and the historical binary matrix; judging 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 detection result is that the moving trajectory of the mobile carrier meets the closed-loop detection condition; if the total detection score is not greater than the detection score threshold, determining that the detection result is that the moving trajectory of the mobile carrier does not meet the closed-loop detection condition.
[0012] In an optional embodiment of the present application, the historical binary matrix includes a historical distance binary matrix and a historical intensity binary matrix, and the total detection score is determined in the following manner: the historical distance binary matrix in the historical fusion description graph is obtained, and the distance similarity score is determined according to 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 graph is obtained, and the historical intensity binary matrix and the target intensity binary matrix are XOR-processed to determine the XOR data; the geometric similarity score is determined according to the XOR data and the total number of elements in the target intensity binary matrix; the intensity similarity score is determined according to 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 according to the weight values corresponding to the distance similarity score, the geometric similarity score, and the intensity similarity score, respectively.
[0013] In an optional embodiment of the present application, the target intensity binary matrix is determined in the following manner: based on the offset angle of the laser radar 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, and 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 intensity binary matrix after the move is determined as the target intensity binary matrix.
[0014] In the second aspect, an embodiment of the present application also provides a closed-loop detection device, which includes: an acquisition module, which is used to acquire laser point cloud data of a current frame collected by a laser radar, wherein the laser radar is installed on a mobile carrier, and the laser point cloud data includes point cloud data of multiple detection points detected by the laser radar during the movement of the mobile carrier; a selection module, which is used to select edge points from the multiple detection points according to the laser point cloud data of the current frame; a construction module, which is used to construct a polar coordinate projection map of the current frame, wherein the polar coordinate projection map takes the position of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to the positional relationship between the laser radar and the laser radar, and the polar coordinate projection map is divided into multiple sectors; a determination module, which is used to determine the intensity representative value and the distance representative value corresponding to each sector; a generation module, which is used to generate a fusion description map of the current frame according to the intensity representative values and distance representative values corresponding to all sectors; a detection module, which is used to detect whether the moving trajectory of the mobile carrier meets the closed-loop detection conditions according to the fusion description map.
[0015] In a third aspect, an embodiment of the present application further provides an electronic device, comprising: 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 and the memory communicate 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 device provided by the embodiment of the present application obtains the laser point cloud data of the current frame collected by the laser radar, selects edge points from multiple detection points according to the laser point cloud data of the current frame; constructs a polar coordinate projection diagram of the current frame, the polar coordinate projection diagram takes the position of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection diagram according to the positional relationship between the laser radar and the laser radar, and the polar coordinate projection diagram is divided into multiple sectors; for each sector, the intensity representative value and the distance representative value corresponding to the sector are determined; according to the intensity representative values and the distance representative values corresponding to all sectors, a fusion description diagram of the current frame is generated; according to the fusion description diagram, whether the moving trajectory of the mobile carrier meets the closed-loop detection conditions is detected. Through this application, the accuracy of the detection result of whether the running trajectory of the mobile carrier meets the closed-loop detection conditions is improved.
[0017] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, preferred embodiments are specifically cited below and described in detail with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS
[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.
[0019] Figure 1 A flow chart of a closed-loop detection method provided in an embodiment of the present application;
[0020] Figure 2 One of the schematic diagrams of the polar coordinate projection diagram provided in the embodiment of the present application;
[0021] Figure 3 A schematic diagram of a one-dimensional matrix for determining distances provided in an embodiment of the present application;
[0022] Figure 4 The second schematic diagram of the polar coordinate projection diagram provided in the embodiment of the present application;
[0023] Figure 5 A schematic diagram of the structure of a closed-loop detection device provided in an embodiment of the present application;
[0024] Figure 6 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION
[0025] To make the purpose, technical scheme and advantages of the embodiments of the present application clearer, the technical scheme in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all of the embodiments. The components of the embodiments of the present application usually described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, each other embodiment obtained by those skilled in the art without making creative work belongs to the scope of protection of the present application.
[0026] First, the application scenarios to which the present application is applicable are introduced. The present application can be applied in the field of laser positioning technology.
[0027] Research has found that in closed-loop detection, a descriptor is a feature vector or feature set used to represent and compare features between point clouds or other data. A descriptor usually contains unique features that help to distinguish different locations and can be effectively used to match and identify scenes that have been seen. Closed-loop detection is highly dependent on the construction of robust descriptors.
[0028] The construction of Link3D descriptors relies on the geometric features of point clouds, such as edge features. However, in extreme environments such as indoors 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. It is difficult to find enough significant feature points in such environments, which will lead to a scarcity of descriptors and affect the accuracy and reliability of matching.
[0029] Based on this, the embodiment of the present application provides a closed-loop detection method and device, which obtains the laser point cloud data of the current frame collected by the laser radar, selects edge points from multiple detection points according to the laser point cloud data of the current frame; constructs a polar coordinate projection diagram of the current frame, the polar coordinate projection diagram takes the position of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection diagram according to the positional relationship between the laser radar and the laser radar, and the polar coordinate projection diagram is divided into multiple sectors; for each sector, the intensity representative value and the distance representative value corresponding to the sector are determined; according to the intensity representative values and the distance representative values corresponding to all sectors, a fusion description diagram of the current frame is generated; according to the fusion description diagram, whether the moving trajectory of the mobile carrier meets the closed-loop detection conditions is detected. Through this application, the accuracy of the detection result of whether the running trajectory of the mobile carrier meets the closed-loop detection conditions is improved.
[0030] See also Figure 1 , Figure 1 Flow chart of the closed-loop detection method provided in the embodiment of the present application. Figure 1 As shown in , the closed-loop detection method provided by the embodiment of the present application includes:
[0031] S101, obtaining laser point cloud data of the current frame collected by the laser radar.
[0032] The laser radar is installed on a mobile carrier, and the laser point cloud data includes point cloud data of multiple detection points detected by the laser radar during the movement of the mobile carrier.
[0033] By way of example, the mobile carrier may be an automated guided vehicle (AGV), which is used in the fields of industrial automation and logistics transportation. The laser point cloud data acquisition frame is determined according to preset conditions. By way of example, it may be after a preset time has passed or the mobile carrier has moved to a preset distance or a preset angle, etc. It may be determined according to actual needs and is not specifically limited here.
[0034] The point cloud data of the multiple detection points includes the coordinates of the multiple detection points and the intensity value detected at the position of each detection point.
[0035] S102: Select edge points from multiple detection points according to 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 divided into two types: edge points and plane 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 not a preset number of detection points around the detection point, it is considered that the detection point is a discrete point, which will generate noise and filter the detection point.
[0038] Then, for each detection point after filtering, the normal vector is calculated, and the neighborhood detection points of each detection point are found. The covariance matrix is calculated through these neighborhood detection points, and eigenvalue decomposition is performed. The result of the eigenvalue decomposition is used to calculate the curvature value of each detection point.
[0039] According to the size of the curvature value of each detection point, the detection points with curvature values greater than the preset curvature value are marked as edge points. These points are usually located on the edge or contour of the object, and the geometric changes are large compared with the mobile carrier before the movement. The detection points with curvature values less than the preset curvature value are marked as plane points. These points are usually located on the flat surface of the object, and the geometric changes are small compared with the mobile carrier before the movement. By accurately identifying and distinguishing edge points and plane points in point cloud data, the accuracy and efficiency of point cloud processing and matching are improved.
[0040] S103: construct a polar coordinate projection image of the current frame.
[0041] The polar coordinate projection map takes the location of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to their positional relationship with the laser radar. The polar coordinate projection map is divided into multiple sectors.
[0042] By distributing the selected edge points to the polar coordinate plane, the influence caused by the uneven distribution of edge points along the Z axis can be eliminated. The edge points in the Cartesian coordinate system are converted to the polar coordinate system for distribution. i (x i ,y i , z i , η i ) is converted to p′ i =(ρ i ,θ i , 0, η i ), where x, y, and z represent coordinates, and η i represents the intensity value corresponding to the i-th edge point, ρ i and θ i P i ′ is the angle between the distance from the origin and the polar axis.
[0043] S104. For each sector, determine the intensity representative value and the distance representative value corresponding to the sector.
[0044] The distance representative value corresponding to each sector is determined by:
[0045] For each sector, determine the edge points in the sector, and cluster all the edge points in the sector to form multiple feature clusters;
[0046] For each sector, a 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.
[0047] Clustering is performed for each edge point in each sector. Initially, each edge point is regarded as a separate cluster. For each edge point, its distance to other edge points is calculated, and edge points with close distances are divided into the same cluster according to the rules of the clustering algorithm.
[0048] For all sectors, starting from the sector where the feature cluster closest to the pole is located, determine in turn 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 closest to the pole in the sector, the distance between the centroid of the feature cluster and the pole is recorded as the representative distance value corresponding to the sector.
[0050] For each sector, if there is no feature cluster closest to the pole in the sector, the distance representative value corresponding to the sector is recorded as a fixed constant.
[0051] See also Figure 2 , Figure 2 This is one of the schematic diagrams of the polar coordinate projection diagram provided in the embodiment of the present application.
[0052] like Figure 2 As shown in , each sector in the polar coordinate projection diagram is evenly distributed on the polar coordinate projection diagram. For example, the polar coordinate projection diagram is a circle. The polar coordinate projection diagram is divided into 180-dimensional sectors according to the polar angle. K0 is used as the starting point. The regional intensity of each sector is a distance representative value. The direction vector of the centroid of the feature cluster closest to the pole is used as the main direction. In a counterclockwise order, it is determined whether there is a feature cluster closest to the pole in each sector. The blue point in each sector is the centroid of the feature cluster formed at the edge point of the sector, and the yellow point in each sector 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 in 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, Figure 2For example, k0 is the pole, k1 is the centroid of the feature cluster closest to k0, and the search is performed 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 representative value is 0. In the final 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 abnormal nearest feature cluster centroids on the distance one-dimensional matrix, we search for the three nearest feature cluster centroids, see Figure 3 , Figure 3 This is a schematic diagram of a one-dimensional matrix for determining distances provided in an embodiment of the present application. Figure 3 As shown in , Des1 is the distance one-dimensional matrix corresponding to the nearest feature cluster centroid, and Des2 is the distance one-dimensional matrix corresponding to the second nearest feature cluster centroid. Des1 has the highest priority because it is closest to the pole, and Des3 has the lowest priority. The distance representative value of each sector in the final distance one-dimensional matrix corresponds to the non-zero value with the highest priority. If there is no value, it is 0, such as Figure 3 As shown in the dotted box, in the first dotted box, Des1, Des2 and Des3 all have values, but because Des1 is the one-dimensional distance matrix from the centroid of the feature cluster to the pole, Des2 is the one-dimensional distance matrix from the centroid of the feature cluster to the pole, and Des3 is the one-dimensional distance matrix from the centroid of the feature cluster to the pole, Des1 has the highest priority, and the first element of Final Des is the element in Des1.
[0056] See also Figure 4 , Figure 4 This is a second schematic diagram of a polar coordinate projection diagram provided in an embodiment of the present application. Figure 4 As shown in , the polar coordinate projection diagram also includes multiple area blocks, and the multiple area blocks are obtained by dividing the polar coordinate projection diagram along the polar direction according to preset polar coordinate units with the pole as the starting point. Each sector in the polar coordinate projection diagram includes an equal number of area blocks. For example, the polar coordinate projection diagram is divided into Ns sectors and Nr area blocks.
[0057] like Figure 4 As shown, the blue point in each sector is the centroid of the feature cluster formed by the edge points of the sector, the green point in each sector is the centroid of the feature cluster formed by the edge points closest to the pole, and the yellow point is the centroid of the feature cluster closest to the pole in all sectors.
[0058] The intensity representative value corresponding to each sector is determined by:
[0059] According to the position of each edge point, the edge point located in each area block is determined.
[0060] For each sector, the intensity values corresponding to the edge points in each area block under the sector are determined, and the maximum intensity value is selected as the intensity representative value corresponding to the area block.
[0061] The edge points in the area block are screened out, and according to the intensity value corresponding to each edge point in each area block, a maximum intensity value is selected as the intensity representative value of the area block.
[0062] Each area block is very small relative to the entire point cloud area. Combined with the object reflectivity, the echo intensity is strongly correlated with the object surface material. Therefore, it is believed that the echo intensity values in each area block are not much different. The maximum intensity value in the area block is taken as the echo intensity of the entire area block, thereby obtaining the intensity representative value. The generated intensity two-dimensional matrix reflects the intensity distribution of the environment.
[0063] S105. Generate a fusion description graph of the current frame according to the intensity representative values and distance representative values corresponding to all sectors.
[0064] Specifically, the intensity representative value can be composed of L according to each sector max ×180-dimensional intensity two-dimensional matrix, the rows of which correspond to each area block, the columns of which correspond to each sector, and each element in the intensity two-dimensional matrix corresponds to the intensity representative value of the preset area block in the preset sector. For example, the preset polar coordinate unit of each area block is 1, and there are L in each sector. max area blocks, L max is the maximum detection distance of the laser radar, which is also the radius of the polar coordinate projection image.
[0065] Specifically, the distance representative value can be formed into a 1×180-dimensional distance one-dimensional matrix according to each sector, where the row of the distance one-dimensional matrix is a row, the column of the distance one-dimensional matrix corresponds 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 and the pole in the preset sector.
[0066] By combining the two-dimensional intensity matrix and the one-dimensional distance matrix, we get (L max +1)×180 dimension, i.e., the fusion description graph.
[0067] For example, the maximum detection distance of the lidar point cloud is 90 meters, the polar 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 through the intensity representative value of the area block, and a 1×180-dimensional distance one-dimensional matrix is created through the distance representative value of each sector. The two matrices are superimposed according to the sector to generate a 91×180 two-dimensional matrix.
[0068] Combining the distance representative value and the intensity representative value to construct a fused description graph can make full use of the complementarity of geometric and intensity features and enhance the recognition ability of the fused description graph. 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 closed-loop detection can be improved in environments with scarce features and high repeatability, the mismatch rate can be reduced, and the positioning accuracy of the system can be enhanced.
[0069] S106. Detect whether the moving trajectory of the mobile carrier meets the closed-loop detection condition according to the fusion description graph.
[0070] When the mobile carrier is moving, the point cloud data obtained at the current position will be used through edge point determination, intensity representative value determination, distance representative value determination and other operations to construct a fusion description map reflecting the current environment and store it in the historical database. At the same time, the previous fusion description map is retrieved from the historical database for similarity matching.
[0071] The fusion description map of the current frame is binarized to obtain a binary matrix of the current frame.
[0072] As the body continues to move, the fusion description graph is constantly created, the size of the historical database and the amount of similarity matching calculations will continue to increase. In order to reduce the computing cost, we adopted a three-stage hierarchical retrieval strategy and verification method to accelerate the process of fusion description graph similarity matching through fast binary operations.
[0073] Compared to operations involving floating-point multiplications such as histogram matching, binary operations (such as bitwise or logical operations) usually achieve faster computation speed. We introduce an efficient binary matching strategy. Given a (L max +1)×180 dimensional fusion description map M, located in front of the fusion description map L max The ×180 dimension stores the 0-255 integer intensity representative value. After the fusion description map, the 1×180 dimension stores the normalized distance value. The fusion description map M is binarized to obtain the binary matrix I using the following formula:
[0074]
[0075] Among them, x and y are row and column indices. If the element in the fusion description graph M is 0, the element in the corresponding binary matrix is set to false, otherwise it is true. Here, false is recorded as 0 and true is recorded 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 representative value corresponding to each sector. Each row data in the intensity binary matrix represents the intensity representative value corresponding to each area block.
[0077] The total detection score is determined based on the binary matrix of the current frame and the historical binary matrix.
[0078] Here, the historical binary matrix includes a historical distance binary matrix and a historical intensity binary matrix.
[0079] The total test score is determined as follows:
[0080] Obtain the historical distance binary matrix in the historical fusion description graph, and determine the distance similarity score 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.
[0081] Because compared to L max ×180-dimensional intensity binary matrix operations and 1×180-dimensional distance binary matrix operations are more efficient. In the distance similarity calculation, the shift of column vectors is used to explore different alignment methods.
[0082] The historical fusion description map corresponding to the current position is denoted as M q , the fusion description graph searched in the historical database is denoted as M c , M q and M c Decompose to obtain the distance one-dimensional matrix, 1×180 dimensional data, respectively recorded as the distance one-dimensional matrix D q and the historical distance one-dimensional matrix D c , then the distance similarity score can be obtained by 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 of the one-dimensional distance matrix, D q ×D c Represents the one-dimensional distance matrix D q and the historical distance one-dimensional matrix D c The dot product of ||D q || means D q The distance between all elements in represents the sum of the values, ||D c || means D cThe historical distances of all elements in represent the sum of the values.
[0085] Cosine similarity is used to measure whether the directions of two vectors in the vector space are consistent. Its value range is between [-1,1], 1 means the exact same direction (highest similarity), 0 means the two vectors are perpendicular to each other (no similarity), and -1 means the completely opposite directions (lowest similarity). For the 1×180 dimension distance representative value matrix, the similarity score between them can be directly calculated without column shift or other complex operations, so it has better efficiency.
[0086] The historical intensity binary matrix in the historical fusion description graph is obtained, and the historical intensity binary matrix and the target intensity binary matrix are subjected to XOR processing to determine the XOR data.
[0087] The fusion description map M corresponding to the current position q , and the historical fusion description diagram M searched in the historical database c Decompose to obtain the intensity two-dimensional matrix, L max ×180 dimensional data, respectively recorded as the intensity two-dimensional matrix A q and the historical intensity two-dimensional matrix A c , the geometric similarity is calculated by the following formula:
[0088]
[0089] Among them, δ g (A q ,A c ) represents the geometric similarity of the intensity two-dimensional matrix, XOR(A q , A c ) represents the intensity of the two-dimensional matrix A q and the historical intensity two-dimensional matrix A c XOR operation between ||A q || represents the intensity two-dimensional matrix A q The total number of elements in .
[0090] Based on the total number of elements in the XOR data and target intensity binary matrices, a geometric similarity score is determined.
[0091] Since each column in the fusion description image represents a specific azimuth, 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 10 degrees, it is equivalent to all the columns in the fusion description image moving a certain number of columns to the right or left. For position recognition, the change in perspective can be explained as the change in the first L columns in the matrix. maxColumn shift of rows,When the view angle changes, the columns in these rows will shift, so through the column shift operation, this rotation can be aligned to a unified reference frame for comparison.
[0092] The intensity two-dimensional matrix in the moved fusion description image is determined as the target intensity two-dimensional matrix, and the target intensity binary matrix is determined by the following method:
[0093] According to the offset angle of the laser radar of the mobile carrier during the movement, the perspective change value between the intensity binary matrix and the historical intensity binary matrix is determined.
[0094] The viewing angle change value represents that each column element in the intensity binary matrix moves to the right / left by a preset number of columns to correspond to the elements in the historical intensity binary matrix.
[0095] According to the viewing angle change value, each column in the intensity binary matrix is moved in a preset direction by a preset number of columns, and the moved intensity binary matrix is determined as a target intensity binary matrix.
[0096] The geometric similarity score is determined by the following formula:
[0097]
[0098] Among them, SCORE g (A q , A c ) represents the geometric similarity score of the target intensity binary matrix, A m q Represents the intensity two-dimensional matrix A q The target intensity binary matrix is obtained by moving each column in a 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] According to each column number, multiple target intensity binary matrices are determined by moving, and the geometric similarity corresponding to each target intensity binary matrix is recalculated, and the maximum geometric similarity is determined as A. m q The geometric similarity score is calculated, the value of m is recorded, and m is determined to be the number of columns that the mobile carrier needs to move to align the angle with the laser radar before moving during the movement.
[0100] 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.
[0101] In intensity structure matching, the historical fusion description graph M is mainly identified by column-by-column comparison.q and historical fusion description diagram M c The intensity similarity is set to V i q and V i c M q and M c The intensity similarity can be calculated by taking the cosine distance of the i-th column of , and the intensity similarity is determined in the following way:
[0102]
[0103] Among them, δ i (M q , M c ) represents the intensity similarity of the ith column of the two-dimensional matrix, V i q ×V i c represents the dot product of the i-th column in the intensity 2D matrix and the i-th column in the history intensity 2D matrix, ||V i q || means D q The distance representation value of all elements in the i-th column of ||D c || means D c The sum of the historical distances of all elements in the i-th column, 0≤i≤Ns-1, i=0 means M q The first column, i = Ns-1 represents M q The last column of .
[0104] The intensity similarity score is determined by:
[0105]
[0106] Among them, SCORE i (M q , M c ) represents the intensity similarity score of the fused description graph, M m q Represents a two-dimensional matrix after each column in the fusion description graph is moved m columns in the preset direction.
[0107] For example, M q The elements of a row in are [a, b, c, d], m is 1, and the default direction is right. After shifting 1 column to the right, it becomes M m q =[d, a, b, c].
[0108] The total detection score is determined according to the weight values corresponding to the distance similarity score, the geometric similarity score, and the 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 corresponding weight values, add the products of the three scores, and obtain the total detection score. Here, the weight value is set according to actual needs.
[0110] Specifically, it is determined whether the total detection score is greater than a detection score threshold. Here, the detection score threshold 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 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.
[0112] In this step, if it is detected that the moving trajectory of the mobile carrier does not meet the closed-loop detection conditions, a prompt will be given and the mobile carrier and the laser radar need to be optimized.
[0113] The similarity scores of three aspects are comprehensively considered to evaluate whether there is a closed loop. The distance similarity indicates the consistency of the distance between points in the trajectory space, the geometric similarity considers the consistency of the spatial structure and geometric shape of the point cloud, and the intensity similarity evaluates the similarity of the intensity features of the sensor captured data.
[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 the 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 forms a closed loop, thereby providing important real-time feedback and correction for the subsequent SLAM positioning and map construction process.
[0116] Based on the principle that the intensity readings of the same object are consistent, this application uses intensity representative values that can reflect the structural information of the surrounding environment, so that it can not only use robust feature information, but also use intensity representative values in environments where feature information is lacking, and perform double constraints to improve the accuracy of closed-loop recognition and matching. At the same time, the principle of higher efficiency of binary operations is used to accelerate the matching of fused description graphs.
[0117] The closed-loop detection method and device provided by the embodiment of the present application obtains the laser point cloud data of the current frame collected by the laser radar, and selects edge points from multiple detection points based on the laser point cloud data of the current frame, and then constructs a polar coordinate projection diagram of the current frame. The polar coordinate projection diagram takes the position of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection diagram according to the positional relationship between the laser radar and the laser radar. The polar coordinate projection diagram is divided into multiple sectors, and for each sector, the intensity representative value and distance representative value corresponding to the sector are determined; according to the intensity representative values and distance representative values corresponding to all sectors, a fusion description diagram of the current frame is generated, and according to the fusion description diagram, whether the moving trajectory of the mobile carrier meets the closed-loop detection conditions is detected. Through this application, the accuracy of the detection result of whether the running trajectory of the mobile carrier meets the closed-loop detection conditions is improved.
[0118] Based on the same inventive concept, a closed-loop detection device corresponding to the closed-loop detection method is also provided in the embodiment of the present application. Since the principle of solving the problem by the device in the embodiment of the present application is similar to the above-mentioned closed-loop detection method in the embodiment of the present application, the implementation of the device can refer to the implementation of the method, and the repeated parts will not be repeated.
[0119] See also Figure 5 , Figure 5 This is a schematic diagram of the structure of the closed-loop detection device provided in the embodiment of the present application. Figure 5 As shown in , the device 500 comprises:
[0120] An acquisition module 501 is used to acquire laser point cloud data of a current frame collected by a laser radar, wherein the laser radar is installed on a mobile carrier, and the laser point cloud data includes point cloud data of multiple detection points detected by the laser radar during the movement of the mobile carrier;
[0121] A selection module 502 is used to select edge points from the multiple detection points according to the laser point cloud data of the current frame;
[0122] A construction module 503 is used to construct a polar coordinate projection map of the current frame, wherein the polar coordinate projection map takes the position of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to the positional relationship between the edge points and the laser radar, and the polar coordinate projection map is divided into a plurality of sectors;
[0123] A determination module 504 is used to determine, for each sector, a strength representative value and a distance representative value corresponding to the sector;
[0124] A generating module 505, configured to generate a fusion description graph of the current frame according to intensity representative values and distance representative values corresponding to all sectors;
[0125] The detection module 506 is used to detect whether the moving trajectory of the mobile carrier meets the closed-loop detection condition according to the fusion description graph.
[0126] See also Figure 6 , Figure 6 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. Figure 6 As shown in , 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 communicates with the memory 620 via the bus 630. When the machine-readable instructions are executed by the processor 610, the above-mentioned Figure 1 The steps of the closed-loop detection method in the illustrated method embodiment and the specific implementation thereof can be found in the method embodiment, which will not be described in detail here.
[0128] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.
[0129] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. The device embodiments described above are merely schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some communication interfaces, and the indirect coupling or communication connection of devices or units can be electrical, mechanical or other forms.
[0130] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
[0131] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.
[0132] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium that is executable by a processor. Based on this understanding, the technical solution of the present application can essentially be embodied in the form of a software product, or in other words, the part that contributes to the prior art or the part of the technical solution. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.
[0133] Finally, it should be noted that the above-described embodiments are only specific implementation methods of the present application, which are used to illustrate the technical solutions of the present application, rather than to limit them. The protection scope of the present application is not limited thereto. Although the present application is described in detail with reference to the above-mentioned embodiments, ordinary technicians in the field should understand that any technician familiar with the technical field can still modify the technical solutions recorded in the above-mentioned embodiments within the technical scope disclosed in the present application, or can easily think of changes, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be included in the protection scope of the present application. Therefore, the protection scope of the present application shall be based on the protection scope of the claims.
Claims
1. A closed-loop detection method, characterized in that: include: Acquire laser point cloud data of a current frame collected by a laser radar, wherein the laser radar is installed on a mobile carrier, and the laser point cloud data includes point cloud data of a plurality of detection points detected by the laser radar during the movement of the mobile carrier; Selecting edge points from the plurality of detection points according to the laser point cloud data of the current frame; Construct a polar coordinate projection map of the current frame, wherein the polar coordinate projection map takes the position of the laser radar as the pole, the selected edge points are distributed in the polar coordinate projection map according to the positional relationship between the edge points and the laser radar, and the polar coordinate projection map is divided into a plurality of sectors; For each sector, determining the intensity representative value and the distance representative value corresponding to the sector; Generate a fusion description graph of the current frame according to the intensity representative values and distance representative values corresponding to all sectors; According to the fusion description graph, it is detected whether the moving trajectory of the mobile carrier meets the closed-loop detection condition.
2. The method according to claim 1, characterized in that The polar coordinate projection diagram also includes a plurality of area blocks, which are obtained by dividing the polar coordinate projection diagram along the polar direction according to preset polar coordinate units with the pole as the starting point, and each sector in the polar coordinate projection diagram includes an equal number of area blocks.
3. The method according to claim 2, characterized in that The intensity representative value corresponding to each sector is determined by: According to the position of each edge point, determine the edge point located in each area block; For each sector, the intensity values corresponding to the edge points in each area block under the sector are determined, and the maximum intensity value is selected as the intensity representative value corresponding to the area block.
4. The method according to claim 1, characterized in that The distance representative value corresponding to each sector is determined by: For each sector, determine the edge points in the sector, and cluster all the edge points in the sector to form multiple feature clusters; For each sector, a feature cluster in the sector that is closest to the pole is selected, and a distance value between the centroid of the feature cluster and the pole is determined as a distance representative value corresponding to the region block.
5. The method according to claim 4, characterized in that The step of selecting, for each sector, a feature cluster closest to the pole in the sector, and determining a distance value between the centroid of the feature cluster and the pole as a distance representative value corresponding to the region block includes: For all sectors, starting from the sector where the feature cluster closest to the pole is located, 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, the distance value between the centroid of the feature cluster and the pole is recorded 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, the distance representative value corresponding to the sector is recorded as a fixed constant.
6. The method according to claim 1, characterized in that The detecting, according to the fusion description graph, whether the moving trajectory of the mobile carrier satisfies a closed-loop detection condition includes: Binarizing the fusion description image of the current frame to obtain a binary matrix of the current frame, wherein the binary matrix includes a distance binary matrix and an intensity binary matrix, each column of the binary matrix corresponds to each sector, and the row data in the distance binary matrix represents the distance representative value corresponding to each sector, and each row data in the intensity binary matrix represents the intensity representative value corresponding to each area block; Determine a total detection score according to the binary matrix of the current frame and the historical binary matrix; Determine 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 detection result is that the moving trajectory of the mobile carrier meets the closed-loop detection condition; If the total detection score is not greater than the detection score threshold, it is determined that the detection result is that the moving trajectory of the mobile carrier does not meet the closed-loop detection condition.
7. The method according to claim 6, characterized in that The historical binary matrix includes a historical distance binary matrix and a historical intensity binary matrix, and the total detection score is determined in the following way: Obtain a historical distance binary matrix in the historical fusion description graph, and determine a distance similarity score according to 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; Obtain the historical intensity binary matrix in the historical fusion description graph, perform XOR processing on the historical intensity binary matrix and the target intensity binary matrix, and determine XOR data; Determining a geometric similarity score based on the total number of elements in the XOR data and the target intensity binary matrix; Determine the intensity similarity score according to 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 according to the weight values respectively corresponding to the distance similarity score, the geometric similarity score, and the intensity similarity score.
8. The method according to claim 7, characterized in that The target intensity binary matrix is determined by: According to the offset angle of the laser radar of the mobile carrier during the movement, the perspective change value between the intensity binary matrix and the historical intensity binary matrix is determined, and the perspective change value represents that each column element in the intensity binary matrix moves to the right / left by a preset number of columns to correspond to the elements in the historical intensity binary matrix; According to the viewing angle change value, each column in the intensity binary matrix is moved in a preset direction by a preset number of columns, and the moved intensity binary matrix is determined as a target intensity binary matrix.
9. A closed-loop detection device, characterized in that: include: An acquisition module, used to acquire laser point cloud data of a current frame collected by a laser radar, wherein the laser radar is installed on a mobile carrier, and the laser point cloud data includes point cloud data of a plurality of detection points detected by the laser radar during the movement of the mobile carrier; A selection module, used for selecting edge points from the multiple detection points according to the laser point cloud data of the current frame; A construction module is used to construct a polar coordinate projection map of the current frame, wherein the polar coordinate projection map takes the position of the laser radar as the pole, and the selected edge points are distributed in the polar coordinate projection map according to the positional relationship between the edge points and the laser radar, and the polar coordinate projection map is divided into a plurality of sectors; A determination module, used to determine, for each sector, a strength representative value and a distance representative value corresponding to the sector; A generation module, used to generate a fusion description graph of the current frame according to the intensity representative values and distance representative values corresponding to all sectors; The detection module is used to detect whether the moving trajectory of the mobile carrier meets the closed-loop detection condition according to the fusion description graph.
10. An electronic device, characterized in that: include: 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 the processor executes the machine-readable instructions to perform the steps of any method as claimed in claims 1 to 8.
Citation Information
Patent Citations
A point clouds registration system for autonomous vehicles
CN112055805A
GNSS / laser radar loopback detection method for mobile robot
CN113850864A
Loopback detection method and equipment based on point cloud intensity and height information
CN115047487A
Three-dimensional laser radar synchronous mapping and positioning method and system fused with point cloud intensity
CN116679314A
Small-view-angle laser radar rapid loopback detection method in unstructured environment
CN117148375A
