Loopback detection method and related device

Abstract

The invention belongs to a positioning detection method, and provides a loopback detection method and a related device for solving the technical problems that synchronous positioning and map creation in a substation scene are high in error accumulation and inconsistent in global map, and a robot cannot accurately return to a starting point or a key position in a similar area. The method comprises the following steps: determining a candidate area according to a current position corresponding to a current frame point cloud, obtaining a candidate frame point cloud located in the candidate area, obtaining a descriptor and a polar vector of the candidate frame point cloud, calculating a three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud, and obtaining a three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud according to the descriptor of the current frame point cloud and the descriptor of the candidate frame point cloud. And calculating a similarity score between the current frame point cloud and the candidate frame point cloud, and when the similarity score meets a preset requirement, registering the current frame point cloud and the candidate frame point cloud by taking the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud as an initial value to obtain a registration score and an optimized three-degree-of-freedom relative pose.

Description

Technical Field

[0001] The present application relates to a positioning detection method, and specifically to a loop detection method and related devices. Background Art

[0002] Simultaneous Localization and Mapping (SLAM), with its excellent real-time environmental perception and precise positioning capabilities, has become a key force in promoting the efficient and safe unmanned inspection of substations.

[0003] Laser SLAM is remarkably robust in terms of ambient lighting, enabling stable feature registration even in dark environments or those subject to significant lighting variations. However, the complex and highly similar equipment structures in substations mean that long-term robot operation or extensive mapping in large-scale substation scenarios can lead to cumulative odometry and sensor errors. This accumulated error can cause maps to overlap, become misaligned, or break, resulting in global map inconsistencies. Furthermore, robots can easily become confused in similar areas (such as repeated equipment arrangements), leading to positioning drift and the inability to accurately return to their starting point or key locations. Summary of the Invention

[0004] This application provides a loop detection method and related devices to address the technical problems of high error accumulation in synchronous positioning and map creation in substation scenarios, inconsistency in global maps, and the inability of robots to accurately return to the starting point or key positions in similar areas.

[0005] In order to achieve the above objectives, this application adopts the following technical solutions: In a first aspect, the present application proposes a loop detection method, comprising: Get the descriptor and pole vector of the current frame point cloud; Determine the candidate region according to the current position corresponding to the current frame point cloud, and obtain the candidate frame point cloud located in the candidate region; Get the descriptors and pole vectors of the candidate frame point cloud; Calculate the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud by combining the pole vector of the current frame point cloud and the pole vector of the candidate frame point cloud; the three-degree-of-freedom relative pose includes the yaw angle, and the horizontal coordinate translation and the vertical coordinate translation on the xy plane; Based on the descriptors of the current frame point cloud and the candidate frame point cloud, the similarity score between the current frame point cloud and the candidate frame point cloud is calculated; if the similarity score meets the preset requirements, the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud is used as the initial value, and the current frame point cloud and the candidate frame point cloud are aligned through the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and the loop is confirmed based on the alignment score.

[0006] Furthermore, the method for calculating the yaw angle includes: For a given point cloud pair, part of the point cloud object is extracted through filtering based on semantic information to obtain a filtered point cloud; Convert the filtered point cloud to the polar coordinates of the xy plane to obtain the converted point cloud; The converted point cloud of each frame is divided into multiple sectors according to the yaw angle, and only the point with the smallest polar radius is retained in each sector, thus obtaining two frames of point clouds composed of multiple elements; Sort the points in the point cloud composed of multiple elements in the two frames according to the azimuth angle and save the corresponding polar vectors; The yaw angle is expressed as a column vector translation based on the number of sectors and the pole vector.

[0007] Furthermore, the method for calculating the horizontal coordinate translation amount and the vertical coordinate translation amount on the xy plane includes:

[0008] in, Indicates the horizontal coordinate translation on the xy plane, Indicates the vertical coordinate translation on the xy plane, represents the loss function, Representing point clouds Semantic tags, Representing point clouds Semantic tags, Indicates The closest point, Indicates that a point cloud Rotate to match another point cloud The same direction, the rotated point cloud obtained, express Middle The coordinates of a point.

[0009] Furthermore, the method of obtaining corresponding descriptors according to the divided blocks includes:

[0010] in, is a descriptor, Is an encoding function used to When the features are encoded , =0, is a function used to evaluate the representation ability of each semantic object, , , is the number of divisions along the azimuth, is the number of divisions along the radial direction, It is along the radial direction , along the azimuth blocks.

[0011] Furthermore, the method for calculating the similarity score includes:

[0012] in, is the similarity score, is a descriptor corresponding to a point cloud, is the descriptor corresponding to another point cloud, and I is the indicator function.

[0013] Furthermore, the method for registering the current frame point cloud and the candidate frame point cloud using the LOAM algorithm includes: Align the current frame point cloud and the candidate frame point cloud according to their three-degree-of-freedom relative poses; Perform nearest neighbor search on the aligned current frame point cloud and candidate frame point cloud, and for each point in the current frame point cloud Find corresponding points in candidate frames ; Set point and point The distance threshold between the junction points and point The distance between them is used to calculate the registration score between the current frame point cloud and the candidate frame point cloud; The registration is performed based on the registration score.

[0014] Furthermore, the method for calculating the registration score between the current frame point cloud and the candidate frame point cloud includes:

[0015] in, is the registration score, To satisfy The number of points, is the total number of points in the current frame point cloud, is the set distance threshold.

[0016] In a second aspect, the present application proposes a loop detection system, comprising: The first calculation module is used to obtain the descriptor and polar vector of the current frame point cloud; The candidate module is used to determine the candidate area according to the current position corresponding to the current frame point cloud, and obtain the candidate frame point cloud located in the candidate area; The second computing module is used to obtain the descriptors and polar vectors of the candidate frame point cloud; A pose module is used to calculate the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud by combining the pole vector of the current frame point cloud and the pole vector of the candidate frame point cloud; the three-degree-of-freedom relative pose includes the yaw angle, and the horizontal coordinate translation and the vertical coordinate translation on the xy plane; The loop detection module is used to calculate the similarity score between the current frame point cloud and the candidate frame point cloud based on the descriptors of the current frame point cloud and the candidate frame point cloud; if the similarity score meets the preset requirements, the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud is used as the initial value, and the current frame point cloud and the candidate frame point cloud are aligned through the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and confirm whether the loop is established based on the alignment score.

[0017] In a third aspect, the present application proposes an electronic device comprising: a memory and one or more processors; the memory is coupled to the processor; wherein the memory stores computer program code, the computer program code includes computer instructions, and when the computer instructions are executed by the processor, the electronic device performs the steps of the above-mentioned loop detection method.

[0018] In a fourth aspect, the present application proposes a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the above-mentioned loop detection method are implemented.

[0019] Compared with the prior art, this application has the following beneficial effects: The present application proposes a loop detection method, which determines a candidate area according to the current position corresponding to the current frame point cloud, obtains the candidate frame point cloud located in the candidate area, obtains the descriptor and polar vector of the candidate frame point cloud, and then calculates the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud. According to the descriptor of the current frame point cloud and the descriptor of the candidate frame point cloud, the similarity score between the current frame point cloud and the candidate frame point cloud is calculated. When the similarity score meets the preset requirements, the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud is used as the initial value, and the current frame point cloud and the candidate frame point cloud are aligned through the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and confirms whether the loop is established based on the alignment score. This application targets a laser SLAM loop detection method with dense spatial distribution, high similarity and easy confusion characteristics. It combines similarity verification, geometric verification and posture optimization to effectively reduce the probability of false loops. Through fine alignment and global optimization of candidate frame point clouds, it further ensures the accuracy of loop detection, eliminates cumulative errors, and makes up for the shortcomings of existing technologies in mapping accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments. It should be understood that the following drawings only illustrate certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without paying any creative work.

[0021] Figure 1 A flowchart of the loop detection method of this application; Figure 2 A schematic diagram of another embodiment of the loop detection method of the present application; Figure 3 This is a schematic diagram of a semantic scanning context descriptor in an embodiment of the present application; Figure 4 Schematic diagram of loop detection in an embodiment of the present application; Figure 5 A schematic diagram of the loop detection system of this application. DETAILED DESCRIPTION

[0022] To make the objectives, technical solutions, and advantages of the embodiments of the present application more clear, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Generally, the components of the embodiments of the present application described and shown in the drawings herein can be arranged and designed in various different configurations.

[0023] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for protection, but merely represents selected embodiments of the present application. All other embodiments obtained by persons of ordinary skill in the art based on the embodiments in the present application without creative work are within the scope of protection of the present application.

[0024] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, it does not need to be further defined or explained in subsequent drawings.

[0025] In the description of the embodiments of the present application, it should be noted that if the terms "upper", "lower", "horizontal", "inner", etc. appear, the orientation or position relationship indicated is based on the orientation or position relationship shown in the accompanying drawings, or the orientation or position relationship in which the product of the invention is usually placed when in use. This is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, it should not be understood as a limitation on the present application. In addition, the terms "first", "second", etc. are only used to distinguish the description and should not be understood as indicating or implying relative importance.

[0026] In addition, if the term "horizontal" appears, it does not mean that the component must be absolutely horizontal, but can be slightly tilted. For example, "horizontal" only means that its direction is more horizontal than "vertical", and does not mean that the structure must be completely horizontal, but can be slightly tilted.

[0027] In the description of the embodiments of this application, it should also be noted that, unless otherwise expressly specified or limited, the terms "disposed," "installed," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections or electrical connections; they can refer to direct connections or indirect connections through an intermediate medium; and they can refer to internal connections between two components. Those skilled in the art can understand the specific meanings of the above terms in this application based on specific circumstances.

[0028] SLAM technology plays a key role in unmanned substation inspections, but it also faces numerous challenges. In these scenarios, robots must autonomously navigate complex environments, perceive surrounding equipment layout and environmental conditions in real time, and accurately determine their own position. SLAM technology is the key to solving this problem. It can construct a real-time map of the environment and simultaneously determine the robot's position within it. This allows the robot to autonomously plan inspection routes, monitor equipment status, and troubleshoot problems without having to acquire environmental information in advance. This significantly improves inspection efficiency, reduces the safety risks and labor costs of manual inspections, and provides strong support for intelligent substation operation and maintenance. Laser SLAM primarily relies on LiDAR (LiDAR) to transmit a laser beam and receive the reflected signal, calculating the round-trip time of the laser to obtain three-dimensional information about the environment. This operating principle makes it insensitive to ambient lighting conditions. Whether in dimly lit indoor equipment rooms or in transition zones between indoor and outdoor areas with drastic changes in light intensity, LiDAR can reliably scan the environment and obtain distance information about object surfaces. Compared to visual SLAM, which relies on visual sensors, laser SLAM avoids image blur and feature extraction difficulties caused by insufficient illumination, nor does it suffer from overexposure caused by strong light. Therefore, in complex and variable lighting environments like substations, it can stably perform feature registration, providing reliable data for subsequent positioning and mapping. Furthermore, substations contain a large number of structurally similar devices, such as neatly arranged switchgear and similar-looking transformers. The highly similar geometric features of these similar devices in the point cloud data captured by lidar make it difficult for the SLAM algorithm to accurately distinguish between different devices during feature matching. This makes it easy to mismatch features of different devices, resulting in positioning errors. This error accumulates as the robot continues to operate and the mapping area expands. The layout of equipment within a substation often exhibits a certain regularity, with numerous similar areas, such as multiple compartments with essentially identical equipment arrangement and structure. When a robot enters these similar areas, the SLAM algorithm relies solely on environmental features for positioning, which can easily lead to confusion. Lacking sufficient unique features to accurately distinguish between different locations, the robot may mistakenly believe it is in a similar location it has previously passed, resulting in positioning drift. This positioning drift will cause the robot to deviate from the predetermined inspection route and be unable to accurately return to the starting point or the location of key equipment for inspection, seriously affecting the integrity and accuracy of the inspection, and may cause some equipment to be missed and potential equipment failures to be discovered in a timely manner.

[0029] Based on the above situation, the present application proposes a loop detection method and related devices, which are described in detail below with reference to embodiments and drawings.

[0030] It's important to note that loop closure detection is a key technology in SLAM systems. Its core goal is to correct for accumulated drift caused by sensor errors and environmental interference during long-term motion by identifying whether a robot or autonomous vehicle has returned to a past position. For example, when a robot returns to its starting point after exploring an environment, loop closure detection can pull its position estimate back to the true coordinates, avoiding map distortion or trajectory deviation. As the "memory module" of SLAM systems, loop closure detection optimizes positioning and mapping accuracy by identifying past positions. The technology is evolving from single sensors to multimodal fusion, from hand-crafted features to deep learning-driven approaches, and from offline training to online learning. As demand for autonomous driving, service robots, and other scenarios grows, the robustness and real-time performance of loop closure detection will become core technological competitiveness.

[0031] like Figure 1 FIG. 1 is a flow chart of a loop detection method of the present application, which may include: S101, obtaining the descriptor and pole vector of the point cloud of the current frame.

[0032] A descriptor is a set of numerical values used to describe the features of a point cloud. It can capture information such as the local geometric structure and texture of the point cloud. Different descriptors have different calculation methods and characteristics. By calculating descriptors, different point clouds can be better identified and compared. The polar vector is a vector representation associated with the point cloud. It can provide additional information about the direction and position of the point cloud in loop closure detection. The descriptors and polar vectors of the current frame point cloud are obtained in order to compare and match it with other frame point clouds in subsequent steps. For example, when calculating the similarity score, the descriptor will play a key role.

[0033] S102, determining a candidate region according to a current position corresponding to the current frame point cloud, and obtaining a candidate frame point cloud located within the candidate region.

[0034] The current position refers to the location of the current frame point cloud within the entire scene. By determining candidate regions based on the current position, the scope of subsequent processing can be narrowed, improving the efficiency of loop detection. Candidate regions can be determined based on certain rules (such as setting a certain radius or range with the current position as the center). Point clouds within the candidate regions are called candidate frame point clouds, which are subsequently compared and matched with the current frame point cloud. For example, if the current frame point cloud is located in a specific area of the scene, the candidate region can be set to a certain range near that area to reduce unnecessary calculations and comparisons.

[0035] S103: Obtain the descriptor and pole vector of the candidate frame point cloud.

[0036] Similar to obtaining the descriptors and pole vectors for the current frame's point cloud, descriptors and pole vectors are generated for the candidate frame's point cloud, allowing for subsequent analysis in conjunction with the corresponding information from the current frame's point cloud. These descriptors and pole vectors are used to calculate the relative pose and similarity score between the current frame's point cloud and the candidate frame's point cloud. For example, the descriptors of the candidate frame's point cloud can be compared with the descriptors of the current frame's point cloud to determine their similarity.

[0037] S104: Calculate the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud by combining the pole vector of the current frame point cloud and the pole vector of the candidate frame point cloud. The three-degree-of-freedom relative pose includes the yaw angle, and the horizontal and vertical coordinate translations on the xy plane.

[0038] It should be noted that the yaw angle represents the rotation angle about an axis perpendicular to the xy plane, typically the z-axis, and is used to describe the rotation of an object in the horizontal plane. The horizontal and vertical translations are used to determine the object's position in the xy plane. By calculating the relative pose between the current frame point cloud and the historical frame point cloud, basic pose relationship information is provided for subsequent loop closure detection. In practical applications, algorithms such as feature matching or ICP (Iterative Closest Point) can be used to calculate this relative pose. For example, the ICP algorithm iterates to find the best match between two point clouds to obtain the relative pose.

[0039] The pole vector contains the orientation information of the point cloud. By combining the pole vectors of the current frame point cloud and the candidate frame point cloud, the relative pose between them can be more accurately calculated. When calculating the three-degree-of-freedom relative pose, the orientation information provided by the pole vector is combined with the previously calculated yaw angle and coordinate information on the xy plane, making the relative pose calculation more precise. For example, the pole vector can help determine the relative orientation on the xy plane, thereby more accurately determining the positional relationship between the current frame point cloud and the candidate frame point cloud.

[0040] S105, based on the descriptors of the current frame point cloud and the descriptors of the candidate frame point cloud, calculate the similarity score between the current frame point cloud and the candidate frame point cloud; if the similarity score meets the preset requirements, use the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud as the initial value, and align the current frame point cloud and the candidate frame point cloud through the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and confirm whether the loop is established based on the alignment score.

[0041] Descriptors are quantified representations of point cloud features. By comparing the descriptors of the current frame's point cloud with those of the candidate frame's point cloud, a similarity score can be calculated between them. This similarity score can be calculated using various distance metrics, such as Euclidean distance and cosine similarity. Specifically, for example, the Euclidean distance between descriptors is calculated. A smaller distance indicates more similar descriptors between the two point clouds, meaning higher similarity between the point clouds. If the similarity score meets preset requirements, such as exceeding a certain threshold, a loop is considered to exist between the current frame's point cloud and the candidate frame's point cloud. The LOAM algorithm is used for point cloud registration and mapping. When the similarity score meets preset requirements, the LOAM algorithm uses the previously calculated three-degree-of-freedom relative pose as the initial value to perform a more precise registration of the current frame's point cloud with the candidate frame's point cloud. During the registration process, a registration score is generated, which reflects the degree of match between the two point clouds. If the registration score exceeds a certain threshold, the current frame's point cloud and the candidate frame's point cloud are considered well matched, and a loop closure has been established. Otherwise, a loop closure has not been established. The optimized three-degree-of-freedom relative pose provides more accurate relative pose information between the current frame point cloud and the candidate frame point cloud, which can be used for subsequent tasks such as map construction and pose estimation.

[0042] This application can quickly determine the approximate position of the current frame in the historical frame and narrow the search range by calculating the three-degree-of-freedom relative pose of the current frame point cloud and the historical frame point cloud. At the same time, the candidate area is determined according to the current position corresponding to the current frame point cloud, and only the candidate frame point cloud within the candidate area is further processed, avoiding the traversal of all historical frame point clouds, greatly improving the efficiency of loop detection. The descriptors and polar vectors of the current frame point cloud and the candidate frame point cloud are obtained, and the three-degree-of-freedom relative pose is calculated in combination with the polar vector, which can more accurately describe the features and positional relationship of the point cloud. The descriptor can capture information such as the local geometric structure and texture of the point cloud, and the polar vector provides the direction information of the point cloud. The combination of the two makes the calculation of the relative pose more accurate, thereby improving the accuracy of loop detection. The similarity score between the current frame point cloud and the candidate frame point cloud is calculated based on the descriptor, and when the similarity score meets the preset requirements, the LOAM algorithm is used for alignment to further confirm whether the loop is established. The LOAM algorithm can accurately align the current frame point cloud and the candidate frame point cloud to obtain the alignment score and the optimized three-degree-of-freedom relative pose. By comprehensively considering the similarity score and the registration score, it is possible to more reliably determine whether the loop is established and reduce the cases of misjudgment and missed judgment. By using feature information such as point cloud descriptors and polar vectors, it is possible to adapt to noise, occlusion and deformation of point cloud data. At the same time, the application of the LOAM algorithm also enhances the adaptability and robustness of the solution to different point cloud data, so that loop detection can run stably and reliably in complex actual environments. In addition, due to the use of efficient calculation methods and optimization strategies, the loop detection method of the present application can run in scenarios with high real-time requirements. Fast relative pose calculation and candidate area screening, as well as the efficient LOAM registration algorithm, enable the entire loop detection process to be completed in a shorter time, meet real-time requirements, and is suitable for application scenarios with strict real-time requirements such as robot navigation and autonomous driving.

[0043] like Figure 2 FIG. 1 is a schematic diagram of another embodiment of the loop detection method of the present application, which may include: S201, global semantic ICP (Iterative Closest Point).

[0044] The general ICP algorithm based on local iterative optimization is prone to local minima, which results in the inability to obtain valid initial values in the loop detection module, resulting in invalid loops. To solve this problem, this application proposes a two-step global semantic ICP algorithm consisting of yaw angle calculation and semantic ICP. By combining semantic information, accurate matching can be performed with any initial value. Specifically, the following methods can be used: (1) Yaw angle calculation.

[0045] Given a point cloud pair ( , ), extract some representative point cloud objects (such as poles, bushings, transformers) through filtering based on semantic information, and convert the filtered point cloud into polar coordinates in the xy plane:

[0046]

[0047]

[0048] in, is the first Points, is the first The polar radius of a point, is the first The polar angle of a point, is the first The x-axis coordinate of the point, is the first The y-axis coordinate of the point, is the first Semantic labels for points.

[0049] Then the converted point cloud of each frame is divided into sectors, and only the point with the smallest diameter is retained in each sector. Finally, two frames are obtained. Point cloud composed of elements and According to the yaw angle and Sort the points in and save their corresponding pole vectors and , the yaw angle can be expressed using a column vector translation:

[0050]

[0051] in, It is Elements After moving, is the yaw angle, is the translation of the column vector, and the subsequent ICP shift Same meaning, yes and The Manhattan distance is defined as:

[0052] Compared to scan context and depth scan context, this application only needs to compare one-dimensional vectors, making it more efficient. Furthermore, this application does not rely on maximizing similarity scores to estimate angles, effectively identifying non-closed point cloud pairs and avoiding mismatches caused by over-reliance on scores.

[0053] (2) Semantic ICP.

[0054] In the Scan Context-based approach, the translation of the point cloud affects the row and column structure of the feature descriptors. Simply relying on the displacement of the column descriptors is not sufficient to achieve optimal results. Therefore, this application proposes a semantic ICP algorithm to correct for translation differences between point clouds.

[0055] To find the relative translation we first transform the point cloud Rotate to In the same direction, let the rotated point cloud be , the rotated point cloud The coordinates are calculated as follows:

[0056] in, and Respectively and The The coordinates of a point.

[0057] The ICP problem is ultimately defined as:

[0058] in, Indicates the horizontal coordinate translation on the xy plane, Indicates the vertical coordinate translation on the xy plane, express and The integration parameter, namely the loss function, is used to measure the translation of the current frame point cloud The matching error between the posterior and target point clouds is Representing point clouds Semantic tags, Representing point clouds Semantic tags, express The corresponding point of The closest It should be noted that and That is, the displacement in the x-axis direction and the displacement in the y-axis direction respectively.

[0059] if equal ,but The output is 1 if yes, 0 otherwise.

[0060] Among them, based on the sorting characteristics of the point cloud, feature point search can be performed near the position corresponding to the target yaw angle. The search interval for a target point is defined as:

[0061] in, represents the yaw angle obtained by the above calculation, Indicates the length of the search interval.

[0062] In this search interval, the above operations are performed, and the relative translation between point clouds is finally calculated through the iterative optimization process. Among them, the iteration is to iterate the ICP problem and finally get the optimal translation. When finding corresponding points during the iteration process, the nearest neighbor point will be found from the search interval.

[0063] It should be noted that the search interval formula is a key constraint in the semantic ICP algorithm definition formula. It is used to limit the search range of corresponding points and avoid global search, thereby improving iteration efficiency and accuracy. S202, semantic scanning context.

[0064] Traditional Scan Context-based methods use point height and reflection intensity as features, essentially exploiting the different characteristics of different objects in the scene. However, height and reflection intensity are only low-level features of objects and are not sufficiently representative. This application uses high-level semantic features to represent the scene, thus proposing semantic scan context. Specifically, the following methods can be used: (1) Descriptor definition.

[0065] For a given point cloud frame P , first transform it to polar coordinates to achieve alignment. Figure 3 The figure shows a schematic diagram of the semantic scanning context descriptor, which represents the point cloud frame through the distribution of semantic features in the polar coordinate system. and Represent the polar radius and polar angle respectively, as the basis for partitioning blocks, each sector area corresponds to a column vector of the descriptor, and each annular area corresponds to a row vector of the descriptor. Then it is evenly divided into blocks. Figure 3 The construction process of the Semantic Scan Context (SSC) descriptor is shown, which represents the point cloud frame through the distribution of semantic features in the polar coordinate system. Indicates the radial distance from a point in the point cloud to the center of the lidar. The figure is divided into There are circular areas corresponding to the rows of descriptors. The table shows the azimuth of a point in the point cloud. The figure is divided into Sector-shaped areas, corresponding to the columns of descriptors. i and fan-shaped areas j The grid cells formed by the intersection are defined as:

[0066] in, is the maximum effective measurement distance of the laser radar, is the number of divisions along the azimuth, is the number of divisions along the radial direction, is the first k The polar angle of a point, , .

[0067] The descriptor is defined as follows:

[0068] in, is a descriptor, Is an encoding function used to When , =0, function Used to evaluate the representation ability of each semantic object.

[0069] Similar to the global ICP mentioned above, this application uses five language objects (transformer, insulator, bushing, pole, wire) to construct the descriptor. E The priority of different semantic categories in the scene is used to reflect their representativeness differences. Among them, objects that appear less frequently in the scene (such as transformers) are given higher representative weights, while common objects (such as wires) are relatively less representative.

[0070] (2) Similarity score.

[0071] First, the aligned point cloud is calculated based on the descriptor formula and Descriptor and , and then evaluate the degree of match by calculating the similarity score between the two descriptors , the calculation formula is as follows:

[0072] Where I is the indicator function, defined as follows:

[0073] in, is a Boolean expression.

[0074] S203, loop detection.

[0075] Although loop detection helps to eliminate the cumulative error of the SLAM system, the wrong loop will cause serious damage to the reliability of SLAM. In order to ensure the accuracy of loop detection, after obtaining the candidate frame through the semantic descriptor, this application further verifies the possible loop to avoid wrong loop. Figure 4 , which is a schematic diagram of loop detection in this embodiment.

[0076] The specific steps can be followed: (1) Loopback candidate frame generation.

[0077] The candidate frame generation module extracts potential loop candidate frames based on the odometry data. First, the LOAM (Lidar Odometry and Mapping) algorithm is used to calculate the odometry and calculate the relative pose (3-DOF relative pose) of the current frame point cloud and the historical frame point cloud. According to the process described in the previous two sections, the descriptor of the current frame point cloud is obtained. Sc With the pole vector Rc To improve efficiency and avoid repeated operations, the descriptor Sc With the pole vector Rc Stored in the database.

[0078] This application uses odometry information to perform preliminary coarse screening of candidate frames to accelerate the processing flow. Specifically: Set a circular search area centered on the current position of the point cloud of the current frame, with a radius of is determined as follows:

[0079] Among them, D is the cumulative distance between the current frame and the historical frame, is a constant coefficient representing the odometry drift rate. Point cloud scans within the circular search area are considered candidates, but those closest to the current position or current time are excluded to avoid mistaking consecutive or overlapping scans as candidate frames for loop closure.

[0080] (2) Similarity verification and rough pose estimation.

[0081] Load the corresponding descriptor from the database for each possible candidate frame St and pole vector Rt, Obtained by the global semantic ICP method Rc and Rt 3-DOF pose between (3 Degrees of Freedom, three degrees of freedom), this pose is used to align the descriptors. Finally, the similarity score between the current frame point cloud and the candidate frame point cloud is obtained by comparing the descriptors. , only when Greater than threshold , then proceed to the subsequent steps. Less than threshold , it does not constitute a loop.

[0082] (3) Geometric verification and pose optimization.

[0083] In the final stage of loop closure detection, geometric verification and pose optimization ensure the accuracy of loop closure detection and eliminate cumulative errors through fine alignment and global optimization of candidate frames. This process uses the rough estimated pose obtained in the previous similarity verification step. As the initial value, the LOAM algorithm is used to accurately align the current frame point cloud and the candidate frame point cloud.

[0084] During the registration process, the aligned current frame point cloud and the candidate frame point cloud are firstly searched by nearest neighbor (KD tree), and for each point in the current frame point cloud Find corresponding points in candidate frames Then, by setting the distance threshold , statistics satisfy The number of interior points , and finally calculate the registration score between the two frames of point cloud , the formula is as follows:

[0085] in, Refers to the total number of points in the point cloud of the current frame. It reflects the overlap quality of the point cloud. The higher the score, the more consistent the geometric structure of the two frames of point cloud is, and the higher the reliability of loop detection is. If the value exceeds the preset threshold, the loop is confirmed to be established, otherwise it is rejected. When a loop closure is found, the optimized relative pose is added as a constraint to the pose graph. The pose graph optimization adjusts the pose of each frame by minimizing the error function.

[0086] like Figure 5 FIG. 1 is a schematic diagram of a loop detection system of the present application, which may include: The first calculation module is used to obtain the descriptor and polar vector of the current frame point cloud; The candidate module is used to determine the candidate area according to the current position corresponding to the current frame point cloud, and obtain the candidate frame point cloud located in the candidate area; The second computing module is used to obtain the descriptors and polar vectors of the candidate frame point cloud; A pose module is used to calculate the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud by combining the pole vector of the current frame point cloud and the pole vector of the candidate frame point cloud; the three-degree-of-freedom relative pose includes the yaw angle, and the horizontal coordinate translation and the vertical coordinate translation on the xy plane; The loop detection module is used to calculate the similarity score between the current frame point cloud and the candidate frame point cloud based on the descriptors of the current frame point cloud and the candidate frame point cloud; if the similarity score meets the preset requirements, the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud is used as the initial value, and the current frame point cloud and the candidate frame point cloud are aligned through the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and confirm whether the loop is established based on the alignment score.

[0087] It should be noted that in the several embodiments provided in this application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the system embodiments described above are merely schematic. For example, the division of each module is only a logical function division. There may be other division methods in actual implementation. For example, multiple modules can be combined or integrated into another device, or some features can be ignored or not executed. The modules described as separate components may or may not be physically separated. The components displayed as modules may be one physical unit or multiple physical units, that is, they may be located in one place, or they may be distributed in multiple different places. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment.

[0088] In addition, the modules in the various embodiments of the present invention may be integrated into a single processing unit, each module may exist physically separately, or two or more modules may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0089] An embodiment of the present application also provides an electronic device, which may include one or more processors, memories, and communication interfaces.

[0090] The memory, the communication interface, and the processor are coupled together. For example, the memory, the communication interface, and the processor may be coupled together via a bus.

[0091] The communication interface is used to transmit data with other devices. The memory stores computer program code. The computer program code includes computer instructions. When the computer instructions are executed by the processor, the electronic device performs the steps of the loop detection method.

[0092] Among them, the processor can be a processor or a controller, for example, a central processing unit (CPU), a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, transistor logic devices, hardware components or any combination thereof. It can implement or execute the various exemplary logic blocks, modules and circuits described in conjunction with the contents of this disclosure. The processor can also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, and the like. The processor can be used to support electronic devices in executing the method steps provided in the above embodiments.

[0093] The bus may be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The above buses may be divided into an address bus, a data bus, a control bus, etc.

[0094] An embodiment of the present application provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the steps of the above-mentioned loop detection method are implemented.

[0095] The computer-readable storage medium involved in this application includes random access memory (RAM), internal memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disks, removable disks, CD ROMs, or any other form of storage medium known in the technical field.

[0096] The above are merely preferred embodiments of the present application and are not intended to limit the present application. Those skilled in the art will readily appreciate that various modifications and variations are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present application shall be included within the scope of protection of the present application.

Claims

1. A loop detection method, characterized in that: include: Get the descriptor and pole vector of the current frame point cloud; Determine the candidate region according to the current position corresponding to the current frame point cloud, and obtain the candidate frame point cloud located in the candidate region; Get the descriptors and pole vectors of the candidate frame point cloud; Calculate the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud by combining the pole vector of the current frame point cloud and the pole vector of the candidate frame point cloud; the three-degree-of-freedom relative pose includes the yaw angle, and the horizontal coordinate translation and the vertical coordinate translation on the xy plane; Calculate the similarity score between the current frame point cloud and the candidate frame point cloud based on the descriptor of the current frame point cloud and the descriptor of the candidate frame point cloud; If the similarity score meets the preset requirements, the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud is used as the initial value, and the current frame point cloud and the candidate frame point cloud are aligned using the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and whether the loop is established is confirmed based on the alignment score.

2. A loop detection method according to claim 1, characterized in that: The method for calculating the yaw angle includes: For a given point cloud pair, part of the point cloud object is extracted through filtering based on semantic information to obtain a filtered point cloud; Convert the filtered point cloud to the polar coordinates of the xy plane to obtain the converted point cloud; The converted point cloud of each frame is divided into multiple sectors according to the yaw angle, and only the point with the smallest polar radius is retained in each sector, thus obtaining two frames of point clouds composed of multiple elements; Sort the points in the point cloud composed of multiple elements in the two frames according to the azimuth angle and save the corresponding polar vectors; The yaw angle is expressed as a column vector translation based on the number of sectors and the pole vector.

3. The loop detection method according to claim 1, wherein: The method for calculating the horizontal coordinate translation amount and the vertical coordinate translation amount on the xy plane includes: in, Indicates the horizontal coordinate translation on the xy plane, Indicates the vertical coordinate translation on the xy plane, represents the loss function, Representing point clouds Semantic tags, Representing point clouds Semantic tags, Indicates The closest point, Indicates that a point cloud Rotate to match another point cloud The same direction, the rotated point cloud obtained, express Middle The coordinates of a point.

4. A loop detection method according to claim 1, characterized in that: The method for obtaining corresponding descriptors according to the divided blocks includes: in, is a descriptor, Is an encoding function used to When the features are encoded , =0, is a function used to evaluate the representation ability of each semantic object, , , is the number of divisions along the azimuth, is the number of divisions along the radial direction, It is along the radial direction , along the azimuth blocks.

5. A loop detection method according to claim 1, characterized in that: The method for calculating the similarity score includes: in, is the similarity score, is a descriptor corresponding to a point cloud, is the descriptor corresponding to another point cloud, and I is the indicator function.

6. A loop detection method according to claim 1, characterized in that: The method for registering the current frame point cloud and the candidate frame point cloud using the LOAM algorithm includes: Align the current frame point cloud and the candidate frame point cloud according to their three-degree-of-freedom relative poses; Perform nearest neighbor search on the aligned current frame point cloud and candidate frame point cloud, and for each point in the current frame point cloud Find corresponding points in candidate frames ; Set point and point The distance threshold between the junction points and point The distance between them is used to calculate the registration score between the current frame point cloud and the candidate frame point cloud; The registration is performed based on the registration score.

7. A loop detection method according to claim 1, characterized in that: The method for calculating the registration score between the current frame point cloud and the candidate frame point cloud includes: in, is the registration score, To satisfy The number of points, is the total number of points in the current frame point cloud, is the set distance threshold.

8. A loop detection system, characterized in that: include: The first calculation module is used to obtain the descriptor and polar vector of the current frame point cloud; The candidate module is used to determine the candidate area according to the current position corresponding to the current frame point cloud, and obtain the candidate frame point cloud located in the candidate area; The second computing module is used to obtain the descriptors and polar vectors of the candidate frame point cloud; A pose module is used to calculate the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud by combining the pole vector of the current frame point cloud and the pole vector of the candidate frame point cloud; the three-degree-of-freedom relative pose includes the yaw angle, and the horizontal coordinate translation and the vertical coordinate translation on the xy plane; The loop detection module is used to calculate the similarity score between the current frame point cloud and the candidate frame point cloud based on the descriptor of the current frame point cloud and the descriptor of the candidate frame point cloud; If the similarity score meets the preset requirements, the three-degree-of-freedom relative pose of the current frame point cloud and the candidate frame point cloud is used as the initial value, and the current frame point cloud and the candidate frame point cloud are aligned using the LOAM algorithm to obtain the alignment score and the optimized three-degree-of-freedom relative pose, and whether the loop is established is confirmed based on the alignment score.

9. An electronic device, characterized in that: include: A memory, one or more processors; the memory is coupled to the processor; wherein the memory stores computer program code, the computer program code includes computer instructions, and when the computer instructions are executed by the processor, the electronic device performs the steps of the loop detection method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the loop detection method according to any one of claims 1 to 7 are implemented.

Images