Robust SLAM loopback registration method and device and storage medium

Abstract

The invention discloses a robust SLAM (Simultaneous Localization and Mapping) loopback registration method and device and a storage medium. A plurality of registration algorithms are integrated in the registration frame, the algorithms are sequenced according to a pre-configured sequence, the faster algorithms are sequenced in the front, the slower and robust algorithms are sequenced in the rear, finally, a registration pose result is calculated, and loopback registration quality detection is carried out. Based on the above scheme, calculation fails only after all registration algorithms fail; meanwhile, the registration quality module can detect a result with a registration problem to a great extent. Therefore, according to the robust loopback registration system and method provided by the invention, the accuracy and recall degree of an SLAM loopback registration result can be ensured to a great extent, and a stable and reliable loopback registration result is provided for a total SLAM system. A reliable mapping module can be realized, and high-reliability SLAM loopback consistency can be realized without manual intervention or only with little manual intervention.

Description

Technical Field

[0001] The present invention relates to the field of mapping in the autonomous driving and robotics industries, and in particular to a method, device and storage medium for robust SLAM loop registration. Background Art

[0002] Loop detection and loop calculation are important components of SLAM technology. Loop detection can find similar relationships of the same location in the data, and loop calculation will calculate the relative posture relationship based on the similarity results of loop detection to obtain an accurate registration result, providing a highly consistent relative relationship for the subsequent loop stitching.

[0003] The mainstream methods of SLAM loop registration currently include optimization-based methods, such as ICP and its variants, NDT, LOAM and its variants, graph search-based methods such as TEASER++, and machine learning-based methods such as HRegNet and BUFFER. Some of these methods are fast but have average accuracy, while others are time-consuming but have high accuracy. However, they generally cannot achieve very high accuracy, which means that registration failure still occurs.

[0004] Current SLAM loop closure registration methods all have robustness issues. In some scenarios, the registration results may reach local optimality or fail to diverge. Therefore, when these situations occur, we can only: 1) use these inaccurate registration results out of necessity. Or, 2) discard these registration results, resulting in no loop closure constraints. In both cases, the final result of the SLAM system will have additional precision errors of varying degrees, thereby reducing the performance of the entire system.

[0005] At the same time, loop registration quality detection is also an important aspect. It is difficult for general methods to reliably detect whether the registration results are reliable in different environments. Especially when the point clouds corresponding to the loop trajectory only partially overlap, this detection level is difficult to meet the general environment requirements. Summary of the invention

[0006] In order to solve the above problems of the prior art solutions, the present invention proposes a method, device and storage medium for robust SLAM loop registration. The robust loop registration system and method proposed in the present invention can largely guarantee the accuracy and recall of the SLAM loop registration results, and provide stable and reliable loop registration results for the overall SLAM system.

[0007] In order to achieve the above object, the present invention adopts the following technical solution: a method for robust SLAM loop registration, comprising the following steps:

[0008] S1, loop detection, used to perform loop detection on the odometer output results;

[0009] S2, a registration framework which integrates several registration algorithms and sorts these algorithms according to a pre-configured order. Among them, faster algorithms are ranked higher, while slower but more robust algorithms are ranked lower. Finally, the registration pose result is calculated. In this way, the overall configuration can take into account both the speed of registration and ensure the effectiveness of the final registration result.

[0010] S3, loop registration quality detection, which performs quality detection on the registration pose result. If it fails, it returns to the registration framework to recalculate. If it passes, it outputs the robust pose.

[0011] Based on the above scheme, calculation of failure will only occur after all registration algorithms have failed. At the same time, the registration quality module can, to a large extent, detect problematic registration results.

[0012] Therefore, the robust loop registration system and method proposed by the present invention can, to a large extent, ensure the accuracy and recall of the SLAM loop registration result, providing a stable and reliable loop registration result for the overall SLAM system. Based on this system, a reliable mapping module can be implemented, and high-reliability SLAM loop consistency can be achieved without manual intervention or with only a small amount of manual intervention.

[0013] Further, the steps of the algorithm sorting are as follows:

[0014] Obtain the historical data of all registration algorithms, then perform weight calculation, and finally sort the registration algorithms according to the weights. The specific process is as follows:

[0015] For each registration algorithm, calculate based on the average calculation time and registration accuracy rate of its historical data. When a new algorithm appears, roughly test the average registration time and write the default value of the accuracy rate into the historical data. After all algorithms enter the calculation, each time a calculation is performed again, the result of this time will be updated to the historical data. The calculation method of the weight is as follows, where Th1 and Th2 are two preset thresholds:

[0016] Weight = (Th1 / TimeAvg) 3 ·(Precision / Th2)

[0017] First, perform rough filtering on Precision and Weight. When Precision < Th3 or Weight < Th4, delete this algorithm and do not participate in the calculation. Use this weight to sort all algorithms. The higher the weight, the higher the algorithm is ranked.

[0018] Further, when the number of registration algorithms is no more than 10, the sorting algorithms include but are not limited to selection bubble sort or merge sort.

[0019] Furthermore, after the registration algorithms are discarded and sorted, the calculation process of the registration algorithm framework obtains the next registration algorithm according to the algorithm queuing result, runs the registration on the remaining data, obtains the result of the registration quality detection, and determines whether it is necessary to run the subsequent algorithm, and so on. If all algorithms fail to run in the end, an error will be reported for the stitching result and the program will be exited.

[0020] Furthermore, the step of loop registration quality detection includes:

[0021] S31, calculating the original weights of the registration pose results;

[0022] S32, calculate the total weight value and determine whether the total weight value passes. If not, determine that the registration fails. If passed, proceed to S33;

[0023] S33, calculating the registration error in the large grid and statistically analyzing the error distribution in the large grid;

[0024] S34, determining whether there is an abnormality in the error distribution, if so, determining that the registration has failed, if not, determining that the registration has succeeded.

[0025] Furthermore, the steps of calculating the weighted sum are as follows:

[0026] S321, finding adjacent grids according to the original weights;

[0027] S322, calculate the coefficient Coeff of the adjacent grids that have not been touched;

[0028] S323, calculating a weighting coefficient theta-reweight;

[0029] S324, calculating the sum of weighted coefficients.

[0030] Furthermore, the weight θ of each grid is calculated according to “CN115511929A Point cloud registration result evaluation method based on distribution characteristics” final , specifically looking for its 6 adjacent grids in the 3D space, up, down, left, right, front and back, and according to the original weight of each adjacent grid The original weight of this grid is weighted to get the weighted weight θ ReWeight :

[0031]

[0032]

[0033] The u() function is a step function. When <=0, the result is 0, and when >0, the result is 1. Th5 and Th6 are two preset thresholds. This weight is used to calculate Coeff i. This value is either Th6 or 0. When there are too few adjacent grid point cloud points, it does not exist. When , the calculated Coeff i is set to 0;

[0034] The optimized weights comprehensively reflect the status of the surrounding grids in the actual results. In effect, when grid A is surrounded by well-matched grids, the probability that this grid itself is a well-matched grid will be greatly increased.

[0035] Finally, the weighted sum of the overall grid is counted, and it is determined whether the weighted weight is higher than a certain threshold Th7. If so, the judgment is passed, as shown in the following formula:

[0036] sum = (∑θ ReWeight )>Th7 then pass.

[0037] Furthermore, the steps of calculating the registration error are as follows:

[0038] 1) First, perform 2D grid division on the plane area where all the registration results are located. Generally, the grid division can be based on the level of tens of meters to hundreds of meters;

[0039] 2) Calculate the center of all registration position pairs,

[0040] 3) Count the midpoints of each block The corresponding relationship between the trajectory (the same trajectory can be identified by the SLAM keyframe ID), the pose before and after registration, and these physical quantities; if there are too few identical trajectory pairs in a grid (<2), the calculation of this grid is abandoned;

[0041] 4) Calculate the pose difference before and after registration for all the same tracks in the same grid;

[0042] δtf i =tf1 i ·tf2 i .inverse()

[0043] 5) For each difference, δtf i Conduct statistical hypothesis tests, such as single population T test (the threshold can be set to Th8), to obtain a conclusion that there is a significant difference or no significant difference;

[0044] 6) If there are obvious differences, it means that there is a problem with the registration result; if there are no obvious differences, it means that the registration result is passed.

[0045] The present invention also provides a device for robust SLAM loop registration, including:

[0046] A loop detection module for performing loop detection on the output result of the odometer;

[0047] A registration framework module, in which several registration algorithms are integrated, and the algorithms are sorted according to a pre-configured order. Among them, the faster algorithms are ranked靠前, and the slower but more robust algorithms are ranked靠后. Finally, the registration pose result is calculated. In this way, the overall configuration can not only take into account the registration speed but also ensure the effectiveness of the final registration result;

[0048] A loop registration quality detection module for performing quality detection on the registration pose result. If it fails, it returns to the registration framework to recalculate. If it passes, it outputs the robust pose.

[0049] Further, the steps of the algorithm sorting are as follows:

[0050] Obtain the historical data of all registration algorithms, then calculate the weights, and finally sort the registration algorithms according to the weights. The specific process is as follows:

[0051] For each registration algorithm, calculate according to its average calculation time-consuming and registration accuracy rate in the historical data. When a new algorithm appears, roughly test the average registration time and write the default value of the accuracy rate into the historical data. After all algorithms enter the calculation, each time a calculation is performed again, the result of this time will be updated to the historical data. The calculation method of the weight is as follows, where Th1 and Th2 are two pre-set thresholds:

[0052] Weight=(Th1 / TimeAvg) 3 ·(Precision / Th2)

[0053] First, perform rough filtering on Precision and Weight. When Precision<Th3 or Weight<Th4, delete this algorithm and do not participate in the calculation. Use this weight to sort all algorithms. The higher the weight, the higher the algorithm is ranked.

[0054] The present invention also provides a computer-readable storage medium containing a computer program, which, when executed by one or more processors, implements the method for robust SLAM loop registration described in any one of the above. Description of the Drawings

[0055] Figure 1 It is a schematic diagram of a method for robust SLAM loop registration of the present invention;

[0056] Figure 2 and Figure 3A flowchart for weight calculation and algorithm sorting;

[0057] Figure 4 A schematic diagram of the calculation process of the registration algorithm framework;

[0058] Figure 5 Schematic diagram of the overall process of loop registration quality detection;

[0059] Figure 6 Schematic diagram of the steps for calculating weights for registration quality;

[0060] Figure 7 Schematic diagram of the steps for calculating the registration error. DETAILED DESCRIPTION

[0061] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0062] like Figure 1 As shown, a method for robust SLAM loop registration includes the following steps:

[0063] S1, loop detection, used to perform loop detection on the odometer output results;

[0064] S2, a registration framework, which integrates several registration algorithms and sorts the algorithms in a pre-configured order, wherein faster algorithms are ranked first and slower but more robust algorithms are ranked later, and finally the registration pose result is calculated; such an overall configuration can take into account both the speed of registration and the validity of the final registration result;

[0065] S3, loop closure registration quality detection, performs quality detection on the registration pose result. If it fails, it returns to the re-registration framework for recalculation. If it passes, it outputs the robust pose.

[0066] Based on the above scheme, the calculation will fail only when all the registration algorithms fail; at the same time, the registration quality module can detect the problematic results of the registration to a large extent.

[0067] Therefore, the robust loop registration system and method proposed in the present invention can largely guarantee the accuracy and recall of the SLAM loop registration results, and provide a stable and reliable loop registration result for the overall SLAM system. Based on this system, a reliable mapping module can be implemented, and high-reliability SLAM loop consistency can be achieved without manual intervention or with only a small amount of manual intervention.

[0068] As Figure 2 , Figure 3 shown, in some embodiments, the steps of the algorithm sorting are as follows:

[0069] Obtain the historical data of all registration algorithms, then calculate the weights, and finally sort the registration algorithms according to the weights. The specific process is as follows:

[0070] For each registration algorithm, calculate based on the average calculation time-consuming and registration accuracy rate of its historical data. When a new algorithm appears, roughly test the average registration time and write the default value of the accuracy rate into the historical data; after all algorithms enter the calculation, the result of this calculation will be updated to the historical data each time a calculation is performed again; the calculation method of the weight is as follows, where Th1 and Th2 are two preset thresholds:

[0071] Weight=(Th1 / TimeAvg) 3 ·(Precision / Th2)

[0072] First, perform a rough filtering on Precision and Weight. When Precision<Th3 or Weight<Th4, delete this algorithm and do not participate in the calculation; use this weight to sort all algorithms. The higher the weight, the higher the algorithm is ranked.

[0073] Sometimes, when the number of registration algorithms is no more than 10, the sorting algorithms include but are not limited to selection bubble sort or merge sort.

[0074] The registration framework of the present invention is significantly different from the general registration framework in that the registration framework integrates a variety of registration algorithm plugins such as optimization-based methods like ICP and its variants, NDT, LOAM and its variants, graph search-based methods like TEASER++, and machine learning-based methods like HRegNet, BUFFER, etc., and will judge whether to continue using another registration algorithm for calculation according to the registration result.

[0075] Taking the registration framework integrating several registration algorithms such as A, B, C, D, etc. as an example, and sorting various algorithms in the pre-configured order, such as D, C, B, A. The general standard for sorting is that the faster algorithms are ranked relatively in the front, and the slower but more robust algorithms are ranked behind, so that the overall configuration can take into account both the speed of registration and ensure the effectiveness of the final registration result.

[0076] As Figure 4As shown in the figure, after the registration algorithms are discarded and sorted, the calculation process of the registration algorithm framework obtains the next registration algorithm according to the algorithm queuing result, runs the registration on the remaining data, obtains the result of the registration quality detection, and determines whether it is necessary to run the subsequent algorithms, and so on. If all algorithms fail to run in the end, an error will be reported for the stitching result and the program will exit.

[0077] like Figure 5 As shown, the steps of loop registration quality detection include:

[0078] S31, calculating the original weights of the registration pose results;

[0079] S32, calculate the total weight value and determine whether the total weight value passes. If not, determine that the registration fails. If passed, proceed to S33;

[0080] S33, calculating the registration error in the large grid and statistically analyzing the error distribution in the large grid;

[0081] S34, determining whether there is an abnormality in the error distribution, if so, determining that the registration has failed, if not, determining that the registration has succeeded.

[0082] like Figure 6 As shown, the steps of calculating the weighted sum are as follows:

[0083] S321, finding adjacent grids according to the original weights;

[0084] S322, calculate the coefficient Coeff of the adjacent grids that have not been touched;

[0085] S323, calculating a weighting coefficient theta-reweight;

[0086] S324, calculating the sum of weighted coefficients.

[0087] In a specific implementation, the weight θ of each grid calculated by the point cloud registration result evaluation method based on distribution characteristics in CN115511929A can be used. final , specifically looking for its 6 adjacent grids in the 3D space, up, down, left, right, front and back, and according to the original weight of each adjacent grid The original weight of this grid is weighted to get the weighted weight θ ReWeight :

[0088]

[0089]

[0090] The u() function is a step function. When <=0, the result is 0, and when >0, the result is 1. Th5 and Th6 are two preset thresholds. This weight is used to calculate Coeff i. This value is either Th6 or 0. When there are too few adjacent grid point cloud points, it does not exist. When , the calculated Coeffi is set to 0;

[0091] The optimized weights comprehensively reflect the status of the surrounding grids in the actual results. In effect, when grid A is surrounded by well-matched grids, the probability that this grid itself is a well-matched grid will be greatly increased.

[0092] Finally, the weighted sum of the overall grid is counted, and it is determined whether the weighted weight is higher than a certain threshold Th7. If so, the judgment is passed, as shown in the following formula:

[0093] sum = (∑θ ReWeight )>Th7 then pass.

[0094] like Figure 7 As shown, the steps of calculating the registration error are as follows:

[0095] 1) First, perform 2D grid division on the plane area where all the registration results are located. Generally, the grid division can be based on the level of tens of meters to hundreds of meters;

[0096] 2) Calculate the center of all registration position pairs,

[0097] 3) Count the midpoints of each block The corresponding relationship between the trajectory (the same trajectory can be identified by the SLAM keyframe ID), the pose before and after registration, and these physical quantities; if there are too few identical trajectory pairs in a grid (<2), the calculation of this grid is abandoned;

[0098] 4) Calculate the pose difference before and after registration for all the same tracks in the same grid;

[0099] δtfi=tf1 i ·tf2 i .inverse()

[0100] 5) For each difference, δtf i Conduct statistical hypothesis tests, such as single population T test (the threshold can be set to Th8), to obtain a conclusion that there is a significant difference or no significant difference;

[0101] 6) If there are obvious differences, it means that there is a problem with the registration result; if there are no obvious differences, it means that the registration result is passed.

[0102] The loop registration quality detection module is based on the existing point cloud registration result evaluation method based on distribution characteristics in CN115511929A of the applicant's group subsidiary, and some optimization adjustments have been made to the loop correction, including:

[0103] 1) When the visible areas of some collected sections only partially overlap during collection or crowdsourcing, some optimizations are made to the registration quality weight results. The registration quality weight is adjusted to be more favorable for this local visual environment to prevent the point cloud consistency error from being too large due to too little relative relationship between some areas, such as the main road and the auxiliary road.

[0104] 2) Generally speaking, since the error of SLAM odometer is slowly divergent, in a certain area grid, the loop error correction values ​​of two odometer tracks on the front and rear tracks in this grid are relatively constant. When the calculated loop error values ​​of two track pose points are significantly different from the loop calculation error values ​​between the other two tracks in this block, the probability that the loop pose result is problematic is very high. Therefore, this judgment can further filter out problematic loop registration results and improve the overall loop accuracy.

[0105] Based on the above scheme, the calculation will fail only when all the registration algorithms fail; at the same time, the registration quality module can detect the problematic results of the registration to a large extent.

[0106] Therefore, the robust loop registration system and method proposed in the present invention can largely guarantee the accuracy and recall of the SLAM loop registration results, and provide a stable and reliable loop registration result for the overall SLAM system. Based on this system, a reliable mapping module can be implemented, and high-reliability SLAM loop consistency can be achieved without manual intervention or with only a small amount of manual intervention.

[0107] The present invention also provides a device for robust SLAM loop registration, comprising:

[0108] The loop detection module is used to perform loop detection on the odometer output results;

[0109] A registration framework module, in which several registration algorithms are integrated and various algorithms are sorted according to a pre-configured order, wherein faster algorithms are ranked first and slower but more robust algorithms are ranked later, and finally the registration pose result is calculated; such an overall configuration can take into account both the speed of registration and the validity of the final registration result;

[0110] The loop registration quality detection module performs quality detection on the registration pose result. If it fails, it returns to the re-registration framework for recalculation. If it passes, it outputs the robust pose.

[0111] Further, the steps of the algorithm sorting are as follows:

[0112] Obtain the historical data of all registration algorithms, then calculate the weights, and finally sort the registration algorithms according to the weights. The specific process is as follows:

[0113] For each registration algorithm, calculate based on the average calculation time-consuming and registration accuracy rate of its historical data. When a new algorithm appears, roughly test the average registration time, and write the default value of the accuracy rate into the historical data; after all algorithms enter the calculation, each time a calculation is performed again, the result of this time will be updated to the historical data. The calculation method of the weight is as follows, where Th1 and Th2 are two preset thresholds:

[0114] Weight = (Th1 / TimeAvg) 3 ·(Precision / Th2)

[0115] First, perform rough filtering on Precision and Weight. When Precision < Th3 or Weight < Th4, delete this algorithm and do not participate in the calculation; use this weight to sort all algorithms, and the higher the weight, the more forward the algorithm is sorted.

[0116] The present invention also provides a computer-readable storage medium containing a computer program. When the computer program is executed by one or more processors, the method for robust SLAM loop registration described in any one of the above is implemented.

[0117] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution of the present invention and its inventive concept, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. A method for robust SLAM loop registration, characterized in that: It includes the following steps: S1, loop detection, which is used to perform loop detection on the output result of the odometer; S2, registration framework. Several registration algorithms are integrated in the registration framework, and the algorithms are sorted according to a pre-configured order. The faster algorithms are ranked higher, and the slower but more robust algorithms are ranked lower. Finally, the registration pose result is calculated; S3, loop registration quality detection. The quality of the registration pose result is detected. If it fails to pass, it returns to the registration framework to recalculate. If it passes, the robust pose is output.

2. The method for robust SLAM loop registration according to claim 1, characterized in that The steps of the algorithm sorting are as follows: Obtain the historical data of all registration algorithms, then calculate the weights, and finally sort the registration algorithms according to the weights. The specific process is as follows: For each registration algorithm, calculate according to the average calculation time and registration accuracy rate of its historical data. When a new algorithm appears, roughly test the average registration time and write the default value of the accuracy rate into the historical data; after all algorithms enter the calculation, each time the calculation is performed again, the result of this time will be updated to the historical data. The calculation method of the weight is as follows, where Th1 and Th2 are two pre-set thresholds: Weight=(Th1 / TimeAvg) 3 ·((Precision / Th2) First, perform a rough filtering on Precision and Weight. When Precision < Th3 or Weight < Th4, this algorithm is deleted and does not participate in the calculation; use this weight to sort all algorithms. The higher the weight, the higher the algorithm is ranked.

3. The method for robust SLAM loop registration according to claim 2, characterized in that When the number of registration algorithms is no more than 10, the sorting algorithms include but are not limited to selection bubble sort or merge sort.

4. The method for robust SLAM loop registration according to claim 2, characterized in that After completing the discard and sorting of the registration algorithms, the calculation process of the registration algorithm framework obtains the next registration algorithm according to the algorithm queuing result, runs the registration on the remaining data, obtains the result of the registration quality detection, judges whether it is necessary to run the subsequent algorithms, and so on. If all algorithms finally fail to run, an error is reported and exited for this splicing result.

5. The method for robust SLAM loop registration according to any one of claims 1 to 4, characterized in that: The steps of the loop registration quality detection include: S31, calculate the original weight of the registration pose result; S32, calculate the weighted weight sum, and determine whether the total weight passes. If not, it is determined that this registration fails. If it passes, it enters S33; S33, calculate the registration error in the large grid and count the error distribution in the large grid; S34, determine whether the error distribution is abnormal. If it is abnormal, it is determined that this registration fails. If there is no abnormality, it is determined that this registration is successful.

6. The method for robust SLAM loop registration according to claim 5, characterized in that: The steps of the weighted weight sum calculation are as follows: S321, find the adjacent grids according to the original weight; S322, calculate the coefficient Coeff of each adjacent grid; S323, calculate the weighted coefficient theta-reweight; S324, calculate the sum of the weighted coefficients.

7. The method for robust SLAM loop registration according to claim 6, characterized in that: The weight θ of each grid is calculated according to the point cloud registration result evaluation method based on distribution characteristics in CN115511929A final , specifically looking for its 6 adjacent grids in the 3D space, up, down, left, right, front and back, and according to the original weight of each adjacent grid The original weight of this grid is weighted to get the weighted weight θ ReWeight : The u() function is a step function. When <=0, the result is 0, and when >0, the result is 1. Th5 and Th6 are two preset thresholds. This weight is used to calculate Coeffi. The calculated value is either Th6 or 0. When there are too few adjacent grid point cloud points, they do not exist. When , the calculated Coeffi is set to 0; Finally, count the weighted weight sum of the overall grid and determine whether the weighted weight is higher than a certain threshold Th7. If so, the judgment passes, as shown in the following formula: sum=(Σθ ReWeight )>Th7 then passes.

8. The method for robust SLAM loop registration according to claim 5, characterized in that: The steps of the registration error calculation are as follows: 1) First, perform 2D grid division on the plane area where all registration results are located; 2) Calculate the center of all registration position pairs, 3) Count the midpoints of each block The corresponding relationship between the trajectory (the same trajectory can be identified by the SLAM keyframe ID), the pose before and after registration, and these physical quantities; if there are too few identical trajectory pairs in a grid (<2), the calculation of this grid is abandoned; 4) Calculate the pose difference before and after registration of all the same trajectory within the same grid; δtf i =tf1 i ·tf2 i ·inverse() 5) For each difference, δtf i Conduct statistical hypothesis tests and obtain conclusions of significant or no significant differences; 6) When obvious differences are found, it indicates that there are problems with this registration result; when there are no obvious differences, it indicates that the registration result passes.

9. A device for robust SLAM loop registration, characterized in that: Including: A loop detection module for performing loop detection on the output result of the odometer; A registration framework module, in which several registration algorithms are integrated, and the algorithms are sorted according to a pre-configured order. Among them, the faster algorithms are ranked higher, and the slower but more robust algorithms are ranked lower. Finally, the registration pose result is calculated; A loop registration quality detection module for performing quality detection on the registration pose result. If it fails, it returns to the registration framework to recalculate. If it passes, it outputs the robust pose.

10. The apparatus for robust SLAM loop registration according to claim 9, characterized in that: The steps of the algorithm sorting are as follows: Obtain the historical data of all registration algorithms, then calculate the weights, and finally sort the registration algorithms according to the weights. The specific process is as follows: For each registration algorithm, calculate based on the average calculation time and registration accuracy rate of its historical data. When a new algorithm appears, roughly test the average registration time and write the default value of the accuracy rate into the historical data; after all algorithms enter the calculation, each time a calculation is performed again, the result of this time will be updated to the historical data. The calculation method of the weight is as follows, where Th1 and Th2 are two pre-set thresholds: Weight=(Th1 / TimeAvg) 3 ·(Precision / Th2) First, perform rough filtering on Precision and Weight. When Precision < Th3 or Weight < Th4, this algorithm is deleted and does not participate in the calculation; use this weight to sort all algorithms, and the higher the weight, the higher the algorithm is ranked.

11. A computer-readable storage medium containing a computer program, characterized in that: When the computer program is executed by one or more processors, the method for robust SLAM loop registration according to any one of claims 1-8 is implemented.

Images