Loop-back consistency detection method, robot and storage medium
By constructing a local map and calculating the number of points within the match and the average distance of the map under the multi-level distance threshold conditions, the map matching distance is generated, and the problem of map failure caused by incorrect loop frames is solved, and accurate loop consistency detection is achieved.
Patent Information
- Application Number
- CN202311587982.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-11-24
- Publication Date
- 2025-05-27
AI Technical Summary
In the prior art, the error loopback frame is matched to perform map optimization, resulting in the problem of map failure.
By constructing the local map corresponding to the current frame and loopback frame, setting multi-level distance threshold conditions, calculating the number of points in the match and the average distance of the map, generating the map matching distance to judge the loopback consistency.
Accurately judge map consistency and loop optimization consistency, avoid map failure, and improve the accuracy of robot navigation.
Smart Images
Figure CN120047699A_ABST
Abstract
Description
Technical Field
[0001] The embodiments of the present application relate to the technical field of robot vision, and in particular, to a loop consistency detection method, a robot, and a storage medium. Background Art
[0002] Autonomous navigation has a wide range of applications in the fields of robotics and autonomous driving. The Simultaneous Localization and Mapping (SLAM) technology can build a map of the environment and perform localization, which is the prerequisite and key for a robot to achieve autonomous navigation. During the process of map building using the SLAM technology, when the robot passes through the same place, loop detection and map optimization are performed to eliminate cumulative errors and ensure global Figure 1 consistency.
[0003] However, when the robot is in some adjacent and similar environments, incorrect loop detection may occur, that is, the key frames are matched with inconsistent key frames in another environment, and the global map is incorrectly optimized with the inconsistent key frames, resulting in ghosting or distortion of the optimized global map and making the map invalid. Summary of the Invention
[0004] The present application provides a loop consistency detection method, a robot, and a storage medium to solve the technical problem that the map is invalidated due to optimizing the map with incorrect loop frames in the prior art.
[0005] In a first aspect, the present application provides a method for detecting loop consistency, including:
[0006] Determine a current local map and a historical local map, where the current local map is generated based on a current frame, and the historical local map is generated based on a loop frame;
[0007] Calculate the number of matching inliers and the average map distance that meet a preset multi-level distance threshold condition according to the current local map and the historical local map;
[0008] Calculate a map matching distance according to the number of matching inliers and the average map distance;
[0009] Generate consistency detection information for the loop frame according to the map matching distance.
[0010] Optionally, the current local map includes a plurality of current position points, the historical local map includes a plurality of historical position points, and calculating the number of matching inliers and the average map distance that meet a preset multi-level distance threshold condition according to the current local map and the historical local map includes:
[0011] Find the historical position point closest to the current position point in the historical local map as the closest point;
[0012] Determine the closest point distance based on the distance between the current position point and the closest point;
[0013] Calculate the number of matching inlier points and the average map distance according to the closest point distance and the multi-level distance threshold condition.
[0014] Optionally, the multi-level distance threshold condition includes an inlier distance threshold condition, and the calculating the number of matching inlier points and the average map distance according to the closest point distance and the multi-level distance threshold condition includes:
[0015] Determine the position points whose closest point distance satisfies the inlier distance threshold condition as matching inlier points;
[0016] Determine the number of matching inlier points according to the number of the matching inlier points;
[0017] Calculate the total matching distance according to the closest point distance of the current position point;
[0018] Calculate the average map distance according to the total matching distance and the number of matching inlier points.
[0019] Optionally, the determining the target position points whose closest point distance satisfies the inlier distance threshold condition as matching inlier points and obtaining the number of matching inlier points includes:
[0020] Determine the inlier distance threshold;
[0021] Judge whether the closest point distance of each current position point is less than the inlier distance threshold;
[0022] If it is less than, determine the position point as a matching inlier point.
[0023] Optionally, the multi-level distance threshold condition further includes a maximum matching distance threshold condition. Before calculating the total matching distance according to the closest point distance of the position point, the method further includes:
[0024] Determine the current position points whose closest point distance satisfies the maximum matching distance threshold condition as target position points;
[0025] Then, the calculating the total matching distance according to the closest point distance of the current position point is:
[0026] Calculate the total matching distance according to the closest point distance of the target position point.
[0027] Optionally, the determining the current position points whose closest point distance satisfies the maximum matching distance threshold condition as target position points includes:
[0028] Determine the maximum matching distance threshold;
[0029] Judge whether the nearest point distance of each current position point is less than the maximum matching distance threshold;
[0030] If it is less, determine the current position point as the target position point.
[0031] Optionally, the multi-level distance threshold condition further includes a hit distance threshold condition. Before calculating the total matching distance according to the nearest point distance of the current position point, the method further includes:
[0032] Determine the current position point whose nearest point distance satisfies the hit distance threshold condition as the hit position point;
[0033] Reset the nearest point distance of the hit position point to 0.
[0034] Optionally, determining the position point whose nearest point distance satisfies the hit distance threshold condition as the hit position point includes:
[0035] Determine the hit distance threshold;
[0036] Judge whether the nearest point distance of each current position point is less than the hit distance threshold;
[0037] If it is less, determine the current position point as the hit position point.
[0038] Optionally, after calculating the number of inlier matches and the average map distance according to the nearest point distance and the multi-level distance threshold condition, the method further includes:
[0039] Judge whether the number of inlier matches is less than a preset threshold;
[0040] If it is less, terminate the loop consistency detection method.
[0041] Optionally, calculating the map matching distance according to the number of inlier matches and the average map distance includes:
[0042] Determine the number of point clouds in the current local map;
[0043] Calculate the map overlap weight according to the number of inlier matches and the number of point clouds in the current local map;
[0044] Calculate the map matching distance according to the average map distance and the map overlap weight.
[0045] Optionally, calculating the map overlap weight according to the number of inlier matches and the number of point clouds in the current local map includes:
[0046] Calculate the map overlap degree according to the number of matched interior points and the number of point clouds in the current local map;
[0047] Perform a non-linear transformation on the map overlap degree to calculate the map overlap degree weight.
[0048] Optionally, the determining the current local map includes:
[0049] Determine the current frame;
[0050] Determine the key frames near the current frame that satisfy the adjacent distance constraint condition as adjacent key frames;
[0051] Stitch according to the current frame and the adjacent key frames to obtain the current local map.
[0052] Optionally, the determining the historical local map includes:
[0053] Determine the loop closure frame corresponding to the current frame;
[0054] Determine the key frames near the loop closure frame that satisfy the adjacent distance constraint condition as adjacent key frames;
[0055] Stitch according to the loop closure frame and the adjacent key frames to obtain the historical local map.
[0056] Optionally, the finding the historical position point closest to the current position point in the historical local map as the closest point includes:
[0057] Construct a search tool according to the historical local map;
[0058] According to the search tool, find the historical position point closest to the current position point in the historical local map as the closest point.
[0059] Optionally, the finding the historical position point closest to the current position point in the historical local map as the closest point includes:
[0060] Downsample the current local map and the historical local map;
[0061] According to the downsampled current local map and the historical local map, find the historical position point closest to the current position point in the historical local map as the closest point. Optionally,
[0062] Optionally, the generating the consistency detection information of the loop closure frame according to the map matching distance includes:
[0063] Judge whether the map matching distance is less than a preset consistency threshold;
[0064] If it is less than, it is determined that the consistency detection information is excellent;
[0065] If it is greater than, it is determined that the consistency detection information is poor.
[0066] Optionally, before determining the current local map and the historical local map, it further includes:
[0067] Back up all key frames to obtain backup key frames;
[0068] Optimize the global map according to the current frame and the loop closure frame optimization to obtain optimized key frames;
[0069] Then, after detecting the loop closure consistency of the loop closure frame according to the map matching distance, it further includes:
[0070] If the loop closure consistency is determined to be excellent, save the optimized key frames and update the global map according to the optimized key frames;
[0071] If the loop closure consistency is determined to be poor, save the backup key frames and update the global map according to the backup key frames.
[0072] In a second aspect, the present application further provides a robot, including a memory and a processor, the memory is connected to the processor, the processor is configured to execute one or more computer programs stored in the memory, and when the processor executes the one or more computer programs, the robot is enabled to implement the method as described in the first aspect.
[0073] In a third aspect, the present application further provides a computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program, the computer program includes program instructions, and when the program instructions are executed by a processor, the processor is enabled to execute the method as described in the first aspect.
[0074] The technical solution of the present application respectively constructs local maps around the current frame and the loop closure frame, and sets multi-level distance threshold conditions to calculate two parameters, namely the number of inlier points and the average map distance, which respectively represent the similarity of the local map in two dimensions. The number of inlier points and the average distance are weighted to obtain the map matching distance to generate consistency detection information, so as to accurately judge the Figure 1 consistency and the loop closure optimization consistency. Description of the Drawings
[0075] To more clearly illustrate the technical solutions of the embodiments of the present application, the following briefly introduces the drawings required for the description of the embodiments of the present application. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.
[0076] Figure 1 Schematic diagram of the application environment of the loop consistency detection method provided by an embodiment of the present application;
[0077] Figure 2 Schematic diagram of the flow of the loop consistency detection method provided by an embodiment of the present application;
[0078] Figure 3 Schematic diagram of the flow of the method for determining the current local map provided by an embodiment of the present application;
[0079] Figure 4 Schematic diagram of the flow of the method for calculating the number of inlier matches and the average map distance that meet the preset multi-level distance threshold conditions provided by an embodiment of the present application;
[0080] Figure 5 Schematic diagram of the flow of the method for calculating the map matching distance based on the number of inlier matches and the average map distance provided by an embodiment of the present application;
[0081] Figure 6 Schematic diagram of the structure of a computer device provided by an embodiment of the present application. Detailed implementation manners
[0082] In order to make the purpose, technical solutions and advantages of the present application more clear and understandable, the following further details the present application in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the protection scope of the present application.
[0083] It should be noted that if there is no conflict, the various features in the embodiments of the present application can be combined with each other, and all are within the protection scope of the present application. In addition, although the functional modules are divided in the device schematic diagram and the logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order from the module division in the device or the flowchart. Furthermore, the terms "first", "second", "third", etc. used in the present application do not limit the data and execution order, but only distinguish the same items or similar items with basically the same functions and effects.
[0084] First, to facilitate the description of the loop consistency detection method provided by the embodiments of the present application, the application environment of the method provided by the embodiments of the present application is introduced.
[0085] Please refer to Figure 1 , Figure 1 , which is a schematic diagram of the application environment of the loop consistency detection method provided by an embodiment of the present application. The application environment includes a target space 10, in which there are a robot 20 and multiple target objects 30. The robot 20 can move to different positions in the target space 10 and obtain environmental information in the vicinity including the target objects 30. The robot 20 generates a global map 40 of the target space 10 based on different environmental information about the target objects 30 obtained at multiple positions.
[0086] The target space 10 is a space that provides functions for any area. For example, the target space 10 is a living room in this embodiment. It can be understood that the target space 10 can also include indoor spaces, shopping malls, living rooms, kitchens, offices and other spaces. The robot 20 can be deployed in any of the above types of target spaces 10 to perform tasks in the target space 10, such as cleaning the floor or carrying workpieces.
[0087] The target object 30 is an object existing in the target space 10. For example, the target object 30 is a curtain and a window in this embodiment. It can be understood that the target object 30 can also include stationary objects such as paintings, table lamps, sofas, and coffee tables, or moving objects such as humans, pets, and balls, which are not limited here. For the robot 20 at different positions, the environmental information obtained by observing the target object 30 is different due to factors such as distance, angle, light, and occlusion by other objects. Therefore, for the same target 30, if the environmental information observed by the robot 20 at different positions has sufficient similarity, it is added to the global map, otherwise it may be judged as a different object or the target object 30 is directly deleted from the global map.
[0088] The robot 20 includes a moving component, a sensing component, and a controller. The moving component is used to drive the robot to move between different positions within the target space. The sensing component is used to obtain environmental information near the robot. The controller is communicatively connected to the moving component and the sensing component, and generates a global map based on the environmental information obtained by the robot at different positions. For example, the moving component can be wheels, tracks, or mechanical feet. The sensing component can be a binocular camera, a radar, a motion sensor, etc. The controller can be a general-purpose processor, a digital signal processor, an application-specific integrated circuit, a field-programmable gate array, a single-chip microcomputer, an ARM, or other programmable logic devices, discrete gate or transistor logic, discrete hardware components, or any combination of these components. It can also be a combination of computing devices. For example, a combination of a DSP and a microprocessor, multiple microprocessors, one or more microprocessors combined with a DSP and / or any other such configuration is not limited in this regard.
[0089] The robot 20 obtains environmental information near it at different positions within the target space 10 to construct a global map 40 of the entire target space 10. The environmental information obtained by the robot 20 at a specific position, including the environmental information near that position, is the current frame 50. When the robot 20 moves within the target space 10, it is possible that the robot 20 repeatedly moves to the same position that has been previously recorded, and the previously obtained environmental information is the loop closure frame 60. It can be understood that since the environmental information is mainly obtained through the sensors of the robot 20, limited by the accuracy of the sensors and the interference of other factors in the environment, there are certain errors in the environmental information obtained by the robot 20. By comparing multiple different environmental information obtained by the robot 20 at the same position, the errors in the environmental information can be corrected. Specifically, since the loop closure frame 60 and the current frame 50 record the same or similar position points, there is a large similarity. Using the loop closure frame 60 can help the robot 20 determine its position or optimize and correct the global map 40. However, the selection of the loop closure frame 60 is determined by the robot 20 through a loop closure detection operation, mainly based on the similarity of the images and the positions where the images are taken to determine the loop closure frame 60. This may result in the selection of an incorrect loop closure frame 60 to match the current frame 50. For example, the current frame 50 records curtains and a window, and the loop closure frame records the curtains and the window to the right of a hanging painting. Just based on the image of the current frame 50, it is impossible to determine whether the window is to the right or left of the hanging painting. Therefore, the loop closure frame 60 matched by the robot 20 may be incorrect, and when the robot 20 optimizes the global map 40 based on the loop closure frame 60 later, it may cause the optimized global map 40 to be distorted or have ghosting, rendering the global map 40 invalid.
[0090] Based on the above scenario schematic diagram, the loop closure consistency detection method provided by the embodiments of the present application will be introduced below.
[0091] Please refer to Figure 2 , Figure 2Schematic flow chart of the loop consistency detection method provided by an embodiment of the present application. The method includes:
[0092] S21. Determine the current local map and the historical local map.
[0093] In this step, the current local map is the local map generated according to the current frame. Specifically, the current local map is a map used to represent the environmental information near the current frame. Compared with the global map, the current local map only records the environmental information near the current frame, and other positions irrelevant to the environmental information near the current frame are not recorded in the current local map to reduce the difficulty of map maintenance. For example, if the current frame records object information such as windows and curtains, the current local map constructed according to the current frame mainly includes object information such as windows and curtains, and may also include object information such as wall paintings on the nearby walls, but does not include object information such as sofas and coffee tables that are farther away.
[0094] In this step, the historical local map is the local map generated according to the loop frame. Compared with the current local map, the historical local map is a map used to represent the environmental information near the loop frame. The consistency of loop optimization is judged by comparing the historical local map with the current local map. Specifically, the loop frame is determined by the robot selecting from the stored key frames according to the loop detection operation. The frequency of loop detection can be a loop detection every other certain distance or a loop detection every other certain time. The specific loop detection algorithm is not limited here, and those skilled in the art can make adaptive adjustments according to the actual situation.
[0095] In this step, the current local map is determined by determining the nearby key frames according to the current frame and then constructing the current local map according to the current frame and the nearby key frames. Correspondingly, the historical local map is determined by determining the nearby key frames according to the loop frame and then constructing the historical local map according to the loop frame and the nearby key frames. The specific map construction method will be described in detail later and will not be elaborated here.
[0096] S22. Calculate the number of matching inliers and the average map distance that meet the preset multi-level distance threshold conditions according to the current local map and the historical local map.
[0097] In this step, the multi-level distance threshold condition is a constraint condition for assisting in calculating the number of inlier matches and the average map matching distance. Specifically, both the current local map and the historical local map include multiple position points. The multi-level distance threshold condition includes multiple constraint conditions, which are respectively used to assign different label values to the position points in the current local map for differentiation. Position points with different label values adopt different processing algorithms when calculating the number of inlier matches and the average map distance. For example, the multi-level distance threshold condition includes the maximum matching distance condition. If a position point does not meet the maximum matching distance condition, then this position point will not be included in the number of inlier matches, nor will it affect the average map distance, but will be completely excluded from subsequent calculations.
[0098] In this step, the number of inlier matches is the quantity of inlier matches. An inlier match is a position point in the current local map that can match a position point in the historical local map. The number of inlier matches is used to represent the matching degree between the current local map and the historical local map. The more inlier matches there are, the more similar the current local map and the historical local map are, and the higher the loop consistency. On the contrary, the fewer inlier matches there are, the greater the difference between the current local map and the historical local map, and the lower the loop consistency. An inlier match is specifically defined as some of the position points in the current local map that meet the matching distance threshold condition. The detailed determination method of inlier matches will be described in detail later and will not be elaborated here.
[0099] In this step, the average map distance is the average value of the distances between the inlier matches of the current local map and the corresponding position points on the historical local map. The average map distance is also a parameter used to represent the similarity between the current local map and the historical local map at another level, that is, the matching degree of each successfully matched inlier match with the corresponding matching position point on the historical local map. The smaller the average map distance, the higher the average matching degree of each inlier match; on the contrary, the larger the average map distance, the lower the average matching degree of each inlier match. It can be understood that the number of inlier matches is used to represent how many parts of the current local map and the historical local map are similar, while the average map matching distance is used to represent the similarity degree of the similar parts in the two local maps.
[0100] S23. Calculate the map matching distance according to the number of inlier matches and the average map distance.
[0101] In this step, the map matching distance is a parameter obtained by weighted processing based on the average map distance and the number of inner matching points, so as to avoid the situation where when there are only a few position points for matching, the calculated average map distance is relatively small, resulting in an incorrect judgment that the similarity between the current local map and the historical local map is relatively high. The map matching distance after weighted processing has good robustness and can better reflect both the similarity ratio and the similarity degree between the two maps. Specifically, the calculation method of the map matching distance will be described in detail later and will not be elaborated here.
[0102] S24. Generate consistency detection information for the loop closure frame according to the map matching distance.
[0103] In this step, the consistency detection information is used to represent the consistency of the map after optimizing the map according to the loop closure frame, that is, the consistency detection information is used to judge the consistency of the optimized global map, and further judge the consistency of the loop closure optimization. Specifically, the consistency detection information is generated according to the map matching distance. The smaller the map matching distance, the higher the optimization effect of the map according to the loop closure frame and the higher the consistency; on the contrary, the larger the map matching distance, the worse the optimization effect of the map according to the loop closure frame and the lower the consistency. Figure 1 That is, the consistency detection information is used to judge the consistency of the optimized global map, and further judge the consistency of the loop closure optimization. Figure 1 Specifically, the consistency detection information is generated according to the map matching distance. The smaller the map matching distance, the higher the optimization effect of the map according to the loop closure frame and the higher the consistency; on the contrary, the larger the map matching distance, the worse the optimization effect of the map according to the loop closure frame and the lower the consistency. Figure 1 That is, the consistency detection information is used to judge the consistency of the optimized global map, and further judge the consistency of the loop closure optimization. Figure 1 Specifically, the consistency detection information is generated according to the map matching distance. The smaller the map matching distance, the higher the optimization effect of the map according to the loop closure frame and the higher the consistency; on the contrary, the larger the map matching distance, the worse the optimization effect of the map according to the loop closure frame and the lower the consistency.
[0104] In summary, the technical solution of this application constructs local maps around the current frame and the loop closure frame respectively, sets multi-level distance threshold conditions to calculate two parameters, namely the number of inner matching points and the average map distance, which represent the similarity of the local map in two dimensions respectively. Then, the number of inner matching points and the average distance are weighted to obtain the map matching distance to generate consistency detection information, so as to accurately judge the consistency and the loop closure optimization consistency. Figure 1 That is, the consistency detection information is used to judge the consistency of the optimized global map, and further judge the consistency of the loop closure optimization.
[0105] Next, the methods for determining the current local map and the historical local map in the embodiments of this application will be introduced respectively.
[0106] Please refer to Figure 3 , Figure 3 which is a schematic flowchart of the method for determining the current local map provided by an embodiment of this application. It includes:
[0107] S31. Determine the current frame.
[0108] S32. Determine the key frames near the current frame that meet the adjacent distance constraint condition as adjacent key frames.
[0109] S33. Stitch according to the current frame and the adjacent key frames to obtain the current local map.
[0110] In step S31, the current frame is an image unit obtained after the robot acquires environmental factors such as photos, robot poses, and camera parameters from sensors at the current moment and then processes them through machine algorithms.
[0111] In step S32, the adjacent key frame is a key frame having the same or similar target features as the current frame, and the distance constraint condition is a condition for determining the adjacent key frame. For example, the distance constraint condition includes key frames within a radius of 2 meters near the determined position of the current frame, that is, key frames within 2 meters of the position of the current frame can be determined as adjacent key frames. It can be understood that the distance constraint condition can also include other conditions, such as the three key frames closest to the current frame. The distance constraint conditions can include multiple ones, and the different distance constraint conditions are in an AND relationship, that is, only when both conditions of "within 2 meters of the current frame" and "being among the top 3 in the distance sorting from the current frame" are satisfied can it be determined as an adjacent key frame.
[0112] In step S33, both the current frame and the adjacent key frames include multiple position points. In some embodiments, the position points are specifically represented as point clouds. Some of the position points in the adjacent key frames represent the same position in the target space as some of the position points in the current frame, and this position point is the same position point. The remaining part of the position points represents positions not directly recorded in the current frame, and this position point is a different position point. The same position points in the adjacent key frames are merged with the corresponding position points in the current frame, and the different position points are spliced into the map, thereby completing the construction of the current local map. The position points in the current local map are determined as the current position points.
[0113] The construction of the historical local map is the same as that of the current local map, including: determining the loop closure frame corresponding to the current frame; determining the key frames near the loop closure frame that satisfy the adjacent distance constraint condition as adjacent key frames; and splicing according to the loop closure frame and the adjacent key frames to obtain the historical local map, which totals three steps. The position points in the historical local map are determined as the historical position points. Among them, the loop closure frame is obtained by the robot according to the loop closure detection operation, and the remaining steps are the same as the principle of the construction method of the current local map. For the detailed process, please refer to the construction of the current local map and will not be elaborated here.
[0114] The method for calculating the number of matching interior points and the average map distance in the embodiments of the present application will be introduced below.
[0115] Please refer to Figure 4 , Figure 4 which is a schematic flowchart of the method for calculating the number of matching interior points and the average map distance that meet the preset multi-level distance threshold conditions provided by an embodiment of the present application. It includes:
[0116] S41. Find the historical position point closest to the current position point in the historical local map as the nearest point.
[0117] S42. Determine the nearest point distance according to the distance between the current position point and the nearest point.
[0118] S43. Calculate the number of inlier points and the average map distance according to the nearest point distance and the multi-level distance threshold conditions.
[0119] In step S41, the nearest point is among the historical position points that are closest to the specific current position point. For example, for a current position point Z(3, 2, 2) in the current local map, the historical local map includes three points A(3, 1, 2), B(6, 2, 2), and C(4, 4, 3). The distances between A, B, C and point Z are 1, 3, and √3 respectively. Thus, it can be determined that the distance between point A and point Z is the shortest, and point A is the nearest point.
[0120] In some embodiments, before determining the nearest point, it further includes the step of downsampling the current local map and the historical local map. Specifically, the downsampling operation is to collect map information at another downsampling resolution that is lower than the resolution of the map itself. Subsequently, the operation of determining the nearest point is performed on the map after the downsampling step, which can reduce the amount of position point data to be processed and thus improve the calculation efficiency. For example, for the current local map, its original map resolution is 0.01 meters, and the downsampling resolution is 0.1 meters. Thus, the current local map may originally have 10,000 current position points for which the nearest point needs to be determined, and after downsampling, only 100 current position points need to determine the nearest point, reducing the calculation burden. For the historical local map, originally all historical position points may need to be traversed to determine the nearest point that matches the current position point. After downsampling, the number of position points to be traversed also decreases by several orders of magnitude, also reducing the calculation burden.
[0121] Furthermore, in some other embodiments, before determining the nearest point in the historical local map, it further includes the step of constructing a search tool according to the historical local map. Then, when determining the nearest point, according to the search tool, the historical position point in the historical local map that is closest to the current position point is the nearest point. Specifically, the search tool can help the historical local map quickly determine the nearest point that matches a specific current position point, improving the search efficiency of the nearest point. For example, the search tool is a k-dimensional tree search tool constructed according to the historical local map and the Kd-tree algorithm. Through the k-dimension, it can continuously make choices in various regions of the historical local map until the nearest point is finally determined, which is more efficient than traversing all points. It can be understood that in addition to tree-based algorithms such as Kd-tree, the search tool can also include hash algorithms, inverted index algorithms, clustering algorithms, etc., which are not limited herein.
[0122] In step S42, the nearest point distance is the distance between the current position point and the nearest point (i.e., the matched historical position point). It can be understood that since the determination of the nearest point is based on the distance between the historical position point and the current position point, the nearest point distance has been calculated and known in step S41, so it will not be elaborated here. It should be noted that although the current position point and the historical position point are respectively located under two maps, they belong to the same map coordinate system, so the coordinate system of the position points is also unified, and the coordinate distance can be calculated according to the coordinates of the current position point and the historical position point.
[0123] In step S43, the multi-level distance threshold condition includes multiple distance constraint conditions, and different labels are assigned to the position points according to different nearest point distances to facilitate subsequent differential algorithm processing. Specifically, the multi-level distance threshold condition at least includes the inlier distance threshold condition, which is used to determine whether the current position point belongs to the matched inliers. The matched inliers are the position points with a relatively high matching degree between the current position point and the historical position point. For example, first, according to the inlier distance threshold condition, the inlier distance threshold is determined to be 1.2 times the map resolution. For example, if the downsampling resolution is 0.1 meter, the inlier distance threshold is 0.12 meter. Then, it is judged whether the nearest point distance is less than the inlier distance threshold. If the nearest point distance is less than the inlier distance threshold, the current position point corresponding to the nearest point distance belongs to the matched inliers, otherwise it does not belong to the matched inliers. For example, for the current position points X and Y, the nearest point distance of X is 0.1 meter, and the nearest point distance of Y is 0.2 meter, then X belongs to the matched inliers and Y does not belong to the matched inliers. The current position point belonging to the matched inliers represents that the matching degree between the current position point and the historical position point is relatively high. The more the number of matched inliers, the more similar points there are between the current local map and the historical local map. After judging whether all current position points belong to the matched inliers, the number of matched inliers is counted and determined as the number of matched inliers, then the nearest point distances of all current position points are added up to determine the total matching distance, and finally the map average distance is calculated by dividing the total matching distance by the number of matched inliers. This map average distance is used to represent the similarity degree between the current local map and the historical local map, which has been described above and will not be repeated here.
[0124] In some embodiments, in addition to the inlier distance threshold condition, the multi-level distance threshold condition further includes a maximum matching distance threshold condition, which is used to determine whether the current position point belongs to the target position point. The target position point is the point where the current position point and the historical position point are correctly matched. In contrast, the non-target position point is the position point where the current position point and the historical position point are incorrectly matched. For example, first, according to the maximum matching distance threshold condition, the maximum matching distance threshold is determined to be 10 times the map resolution. For example, if the downsampling resolution is 0.1 meter, then the maximum matching distance threshold is 1 meter. Then, it is judged whether the nearest point distance is less than the maximum matching distance threshold. If the nearest point distance is less than the maximum matching distance threshold, the current position point corresponding to the nearest point distance belongs to the target position point; otherwise, it belongs to the non-target position point. It can be understood that the maximum matching distance threshold is larger than the inlier matching distance threshold. Therefore, more target position points are selected, and fewer non-target position points are selected. For non-target position points, their nearest point distances are greater than the maximum matching distance threshold. For example, the nearest point of the current position point Z(3, 2, 2) is D(12, 25, 31), and the nearest point distance reaches 39.6, which far exceeds the maximum matching distance threshold. Such non-target position points often belong to the position points where the current local map or the historical local map does not overlap, resulting in incorrect matching, or the noise points that do not exist themselves, which are meaningless for the map. Therefore, the non-target position points need to be removed from the map. Specifically, for non-target position points, it is obvious that non-target position points do not belong to the matching inliers, and whether the target position points belong to the matching inliers is judged according to the inlier distance threshold condition. When calculating the total matching distance, non-target position points are not considered, but instead, the total matching distance is calculated according to the nearest point distance of the target position points, that is, the obvious abnormal value of 39.6 will not be included. Thus, the maximum matching distance threshold realizes the limitation of the incorrectly matched current position points, and improves the robustness of the current local map and the historical local map in the non-overlapping area.
[0125] In some embodiments, in addition to the inlier distance threshold condition, the multi-level distance threshold condition further includes a hit distance threshold condition. The hit distance threshold condition is used to determine whether the current position point belongs to a hit position point, and the hit position point is the position point where the current position point coincides exactly with the historical position point. For example, first, according to the hit distance threshold condition, the hit distance threshold is determined to be 0.5 times the map resolution. For example, if the downsampling resolution is 0.1 meter, then the hit distance threshold is 0.05 meter. Then, it is judged whether the nearest point distance is less than the hit distance threshold. If the nearest point distance is less than the hit distance threshold, the current position point corresponding to the nearest point distance belongs to the hit position point; otherwise, it does not belong to the hit position point. For example, for the current position points U and W, the nearest point distance of U is 0.08 meter, and the nearest point distance of W is 0.02 meter. Then W belongs to the hit position point, and U does not belong to the hit position point. It can be understood that in the matching process of the current position point and the historical position point, the probability that the two position points coincide exactly is relatively low. Therefore, by setting the hit distance threshold to judge whether the two position points coincide exactly, that is, as long as the nearest point distance is lower than the hit distance threshold, it is considered that the two position points coincide exactly. Thus, before calculating the total matching distance subsequently, the nearest point distance of the hit position point is reset to 0, that is, the nearest point distance of the hit position point will not affect the numerical value of the total matching distance. Thereby, the discretized distance caused by the map resolution can be reduced, and the relatively small nearest point distance can be reasonably ignored, so as to reduce the calculation burden and improve the calculation efficiency.
[0126] In step S43, in some embodiments, the steps of calculating the number of matching inliers specifically include: traversing each current position point, judging whether the current position point belongs to a matching inlier. If it belongs, it is determined as a matching inlier and counted into the number of matching inliers. After the traversal is completed, the number of matching inliers can be determined. The steps of calculating the average map distance specifically include: traversing each current position point, judging whether the current position point belongs to the target position point. If it does not belong to the target position point, skip the current position point and continue to judge the next current position point. If it belongs to the target position point, continue to judge whether the target position point belongs to the hit position point. If it belongs, reset its nearest point distance to 0 and then count it into the total matching distance. If it does not belong, directly count the nearest point distance of the target position point into the total matching distance. After the traversal is completed, the total matching distance can be determined. The calculation result obtained by dividing the total matching distance by the number of matching inliers is the average map distance. The average map distance can be calculated according to the following exemplary formula:
[0127] ave_match_dis = sum_match_dis / inlier_num
[0128] where ave_match_dis is the average map distance, sum_match_dis is the total matching distance, and inlier_num is the number of matching inliers.
[0129] In some embodiments, after step S43, the method further includes: determining whether the number of inlier points is less than a preset threshold; if it is less, the loop consistency detection method is terminated. For example, the threshold is set to 100. If the number of inlier points is 50 and the total number of current position points in the current local map is 1000, it is obvious that the matching part between the current local map and the historical local map is too small, and it can be directly determined that the consistency is very poor and there is no need to perform subsequent steps, so the loop consistency detection method is terminated. It can be understood that in some other embodiments, the threshold can be the ratio of the number of inlier points to the current position points, and the present application does not limit this. Figure 1 The following introduces the method for calculating the map matching distance in the embodiments of the present application.
[0130] Please refer to
[0131] Please refer to Figure 5 , Figure 5 which is a schematic flowchart of the method for calculating the map matching distance according to the number of inlier points and the average map distance provided in an embodiment of the present application. The method includes:
[0132] S51. Determine the number of current position points in the current local map.
[0133] S52. Calculate the map overlap weight according to the number of inlier points and the number of current position points.
[0134] S53. Calculate the map matching distance according to the average map distance and the map overlap weight.
[0135] In step S51, the number of current position points is determined according to the current local map. In some embodiments, if the current local map is a point cloud map, the number of current position points is specifically the number of point clouds.
[0136] In step S52, the map overlap weight is a parameter used to determine how much of the similar part between the current local map and the historical local map. Specifically, the calculation steps of the map overlap weight include: first, calculate the map overlap according to the number of inlier points and the number of current position points, and then perform a non-linear transformation on the map overlap to calculate the map overlap weight. Among them, the map overlap can be calculated according to the following exemplary formula:
[0137] overlap_ratio = inlier_num / cloud_size
[0138] where overlap_ratio is the map overlap, inlier_num is the number of inlier points, and cloud_size is the number of current position points.
[0139] The map overlap degree is also a parameter indicating the amount of the similar part between the current local map and the historical local map. Therefore, in some embodiments, the map overlap degree can be directly used as the map overlap degree weight. In other embodiments, the change gradient of the map overlap degree is not sufficient to represent the influence of the map overlap degree on the map matching distance. When the map overlap degree is small, regardless of the map average distance, the map matching distance should indicate that Figure 1 the consistency is poor, that is, the influence of the map overlap degree on the map matching distance is greater than the influence of the map average distance on the map. Therefore, it is necessary to perform a non-linear transformation on the map overlap degree to increase the influence degree of the map overlap degree on the map matching distance. Specifically, in some embodiments, the steps of performing a non-linear transformation on the map overlap degree to obtain the map overlap degree weight include: first, performing a scaling transformation on the map overlap degree to obtain an overlap degree transformation value, and then performing a power transformation on the overlap degree transformation value to obtain the map overlap degree weight. The specific calculation process can be calculated with reference to the following exemplary formula:
[0140] overlap_trans = a * overlap_ratio
[0141] overlap_weight = (overlap_trans) b
[0142] where overlap_trans is the overlap degree transformation value, overlap_ratio is the map overlap degree, overlap_weight is the map overlap degree weight, and a and b are reference constants.
[0143] For example, under the conditions of a = 1.4 and b = 3, if the map overlap degree is 1, the maximum map overlap degree weight is 2.74. If the map overlap degree is 0.72, the map overlap degree weight is 1. When the map overlap degree drops to 0.3, the map overlap weight is only 0.074. Thus, it can be seen that the map overlap degree weight is very sensitive to the change of the map overlap degree. By the map overlap degree weight, the change gradient of the map overlap degree is increased, and the influence of the map overlap degree on the map matching distance is increased.
[0144] In step S53, the map matching distance is a comprehensive parameter indicating the similarity between the current local map and the historical local map, and is used to directly judge the consistency between the current local map and the historical local map. Specifically, the map matching distance is the calculation result of the map average distance divided by the map overlap degree weight, and can be calculated according to the following exemplary formula:
[0145] map_match_dis = ave_match_dis / overlap_weight
[0146] Among them, map_match_dis is the map matching distance, ave_match_dis is the average map distance, and overlap_weight is the map overlap weight. A more accurate average matching distance can be calculated through multi-level distance threshold conditions, and the average matching distance is nonlinearly adaptively weighted by the map overlap, which further improves the robustness of the map matching distance result.
[0147] The following describes a method for generating consistency detection information of a loop frame in an embodiment of the present application.
[0148] In this step, the consistency detection information is used to determine the global Figure 1 Specifically, the method for generating consistency detection information of the loop frame mainly includes: determining whether the map matching distance is less than a preset consistency threshold, if less, determining that the consistency detection information is excellent; if greater, determining that the consistency detection information is poor. For example, if the consistency threshold is set to 0.2 and the map matching distance is 0.1, it can be determined that the global map after loop optimization is Figure 1 The consistency is good, and then it is determined that the consistency of loop optimization is good. Further, in some embodiments, when the robot passes a certain position point to obtain the current frame, and performs a loop detection operation based on the current frame to obtain a loop frame, at this time, the robot backs up all the key frames of the global map and obtains the backup key frames. Then the robot optimizes the global map and the key frames based on the loop frame and the current frame to obtain the optimized global map and the optimized key frames. At this time, the robot performs the loop consistency detection method provided in the embodiment of the present application to determine the optimized global map. Figure 1 If the consistency detection information is good, the optimization result is considered good, the backup keyframe is discarded, and the optimized keyframe and the optimized global map are saved. On the contrary, if the consistency detection information is poor, the optimization result is considered poor, the global map is restored using the backup keyframe, and the loop constraints generated according to the loop frame are discarded.
[0149] In summary, the technical solution provided by the embodiments of the present application uses the current local map and the historical local map for matching and comparison to calculate the parameters representing similarity in two dimensions, namely the map overlap degree and the average map distance. Then, the parameters in the two dimensions are adaptively weighted and transformed to obtain the comprehensive judgment parameter of the map matching distance, and the consistency of loop optimization is judged through the map matching distance. In this process, the average matching distance is calculated under the condition of high-efficiency calculation through the inlier distance threshold condition, the maximum matching distance threshold condition, and the hit distance threshold condition, which correspondingly improves the accuracy of the map matching distance. In the process of calculating the map matching distance, the robustness of the map overlap degree weight to the calculation of the map matching distance is improved by means of non-linear transformation of the map overlap degree. Therefore, the loop consistency detection method provided by the embodiments of the present application can better solve the technical problem in the prior art that map optimization is performed on the loop frames with matching errors, resulting in map failure.
[0150] It should be noted that in the above various embodiments, there is not necessarily a certain order between the above steps. Those of ordinary skill in the art can understand according to the description of the embodiments of the present application that in different embodiments, the above steps can have different execution orders, that is, they can be executed in parallel or exchanged, etc.
[0151] The embodiments of the present application further provide a robot, which includes a memory and a processor. The memory is connected to the processor, and the processor is used to execute one or more computer programs stored in the memory. When the processor executes the one or more computer programs, the robot implements the loop consistency detection method provided by the embodiments of the present application.
[0152] See Figure 6 , Figure 6 is a schematic structural diagram of a computer device provided by the embodiments of the present application. The computer device includes one or more processors and a memory. The memory is connected to the one or more processors, for example, connected to the processor through a bus.
[0153] The processor is configured to support the computer device in performing the corresponding functions in the methods of the above method embodiments. The processor may be a central processing unit (CPU), a network processor (NP), a hardware chip, or any combination thereof. The above hardware chip may be an application specific integrated circuit (ASIC), a programmable logic device (PLD), or a combination thereof. The above PLD may be a complex programmable logic device (CPLD), a field-programmable gate array (FPGA), a generic array logic (GAL), or any combination thereof.
[0154] The memory is used to store program codes, etc. The memory may include volatile memory (VM), such as random access memory (RAM); the memory may also include non-volatile memory (NVM), such as read-only memory (ROM), flash memory, hard disk drive (HDD), or solid-state drive (SSD); the memory may also include a combination of the above types of memory.
[0155] The memory can be used to store non-volatile software programs, non-volatile computer executable programs, and modules, such as the program instructions / modules corresponding to the loop consistency detection method in the embodiments of the present application. The processor executes various functional applications and data processing of the loop consistency detection method by running the non-volatile software programs, instructions, and modules stored in the memory, that is, implements the loop consistency detection method provided by the above method embodiments and the functions of each module or unit of the robot.
[0156] The memory may include a program storage area and a data storage area. Among them, the program storage area can store an operating system and application programs required for at least one function. The data storage area can store data created according to the use of the robot, etc. In some embodiments, the memory may optionally include a memory remotely set relative to the processor, and these remote memories can be connected to the robot through a network. Examples of the above network include but are not limited to the Internet, intranet, local area network, mobile communication network, and combinations thereof.
[0157] The one or more modules are stored in the memory and, when executed by the one or more processors, execute the loop consistency detection method in any of the above method embodiments. For example, execute the method steps described in the above method embodiments to implement the functions of the modules described in the above device embodiments.
[0158] An embodiment of the present application also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer is caused to execute the method as described in the foregoing embodiments.
[0159] Those of ordinary skill in the art can understand that all or part of the processes of implementing the methods in the above embodiments can be completed by instructing relevant hardware through a computer program. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above method embodiments. Among them, the storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.
[0160] The above-disclosed are only the preferred embodiments of the present application. Of course, the scope of the rights of the present application cannot be limited by this. Therefore, equivalent changes made according to the claims of the present application still fall within the scope covered by the present application.
Claims
1. A method for detecting loop consistency, characterized in that, it includes: Determine the current local map and the historical local map, where the current local map is generated according to the current frame, and the historical local map is generated according to the loop frame; According to the current local map and the historical local map, calculate the number of matching inliers that meet the preset multi-level distance threshold condition and the average map distance; According to the number of matching inliers and the average map distance, calculate the map matching distance; Generate the consistency detection information of the loop frame according to the map matching distance.
2. The method according to claim 1, characterized in that, The current local map includes a plurality of current position points, and the historical local map includes a plurality of historical position points. The calculating the number of matching inliers that meet the preset multi-level distance threshold condition and the average map distance according to the current local map and the historical local map includes: Find the historical position point closest to the current position point in the historical local map as the closest point; Determine the closest point distance according to the distance between the current position point and the closest point; Calculate the number of matching inliers and the average map distance according to the closest point distance and the multi-level distance threshold condition.
3. The method according to claim 2, characterized in that, The multi-level distance threshold condition includes an inlier distance threshold condition. The calculating the number of matching inliers and the average map distance according to the closest point distance and the multi-level distance threshold condition includes: Determine the position point whose closest point distance meets the inlier distance threshold condition as a matching inlier; Determine the number of matching inliers according to the number of the matching inliers; Calculate the total matching distance according to the closest point distance of the current position point; Calculate the average map distance according to the total matching distance and the number of matching inliers.
4. The method according to claim 3, characterized in that, The determining the target position point whose closest point distance meets the inlier distance threshold condition as a matching inlier and obtaining the number of matching inliers includes: Determine the inlier distance threshold; Judge whether the closest point distance of each current position point is less than the inlier distance threshold; If it is less, determine that the position point is a matching inlier.
5. The method according to claim 3, characterized in that, The multi-level distance threshold condition further includes a maximum matching distance threshold condition. Before calculating the total matching distance according to the closest point distance of the position point, the method further includes: Determine the current position point whose closest point distance meets the maximum matching distance threshold condition as the target position point; Then, the calculating the total matching distance according to the closest point distance of the current position point is: Calculate the total matching distance according to the closest point distance of the target position point.
6. The method according to claim 5, characterized in that, The determining the current position point whose closest point distance meets the maximum matching distance threshold condition as the target position point includes: Determine the maximum matching distance threshold; Judge whether the closest point distance of each current position point is less than the maximum matching distance threshold; If it is less, determine that the current position point is the target position point.
7. The method according to claim 3, It is characterized in that the multi-level distance threshold condition further includes a hit distance threshold condition. Before calculating the total matching distance according to the nearest point distance of the current position point, the method further includes: determining the current position point whose nearest point distance satisfies the hit distance threshold condition as the hit position point; resetting the nearest point distance of the hit position point to 0.
8. The method according to claim 7, it is characterized in that the determining the position point whose nearest point distance satisfies the hit distance threshold condition as the hit position point includes: determining the hit distance threshold; judging whether the nearest point distance of each current position point is less than the hit distance threshold; if it is less, determining the current position point as the hit position point.
9. The method according to claim 2, it is characterized in that after calculating the number of matching inliers and the average map distance according to the nearest point distance and the multi-level distance threshold condition, the method further includes: judging whether the number of matching inliers is less than a preset threshold; if it is less, terminating the loop consistency detection method.
10. The method according to claim 1, it is characterized in that the calculating the map matching distance according to the number of matching inliers and the average map distance includes: determining the number of current position points of the current local map; calculating the map overlap weight according to the number of matching inliers and the number of current position points; calculating the map matching distance according to the average map distance and the map overlap weight.
11. The method according to claim 10, it is characterized in that the calculating the map overlap weight according to the number of matching inliers and the number of current position points includes: calculating the map overlap according to the number of matching inliers and the number of current position points; performing a non-linear transformation on the map overlap to calculate the map overlap weight.
12. The method according to claim 1, it is characterized in that the determining the current local map includes: determining the current frame; determining the key frames near the current frame that satisfy the adjacent distance constraint condition as adjacent key frames; performing splicing according to the current frame and the adjacent key frames to obtain the current local map.
13. The method according to claim 1, it is characterized in that the determining the historical local map includes: determining the loop frame corresponding to the current frame; determining the key frames near the loop frame that satisfy the adjacent distance constraint condition as adjacent key frames; performing splicing according to the loop frame and the adjacent key frames to obtain the historical local map.
14. The method according to claim 2, it is characterized in that the finding the historical position point closest to the current position point in the historical local map as the nearest point includes: constructing a search tool according to the historical local map; finding the historical position point closest to the current position point in the historical local map as the nearest point according to the search tool.
15. The method according to claim 2, it is characterized in that the finding the historical position point closest to the current position point in the historical local map as the nearest point includes: Downsample the current local map and the historical local map; Based on the downsampled current local map and the historical local map, find the historical position point closest to the current position point in the historical local map as the nearest point.
16. The method according to claim 1, wherein, the generating the loop closure frame consistency detection information according to the map matching distance includes: judging whether the map matching distance is less than a preset consistency threshold; if it is less, determining that the consistency detection information is excellent; if it is greater, determining that the consistency detection information is poor.
17. The method according to claim 16, wherein, before determining the current local map and the historical local map, further includes: backing up all key frames to obtain backup key frames; optimizing the global map according to the current frame and the loop closure frame optimization to obtain optimized key frames; then, after detecting the loop closure consistency of the loop closure frame according to the map matching distance, further includes: if the loop closure consistency is determined to be excellent, saving the optimized key frames and updating the global map according to the optimized key frames; if the loop closure consistency is determined to be poor, saving the backup key frames and updating the global map according to the backup key frames.
18. A robot, wherein, comprises a memory and a processor, the memory is connected to the processor, the processor is configured to execute one or more computer programs stored in the memory, and when the processor executes the one or more computer programs, the robot implements the method according to any one of claims 1 to 17.
19. A computer-readable storage medium, wherein, the computer-readable storage medium stores a computer program, the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the method according to any one of claims 1 to 17.