A loop detection method and device for SLAM system

Abstract

The present application discloses a loop detection method and device for a SLAM system, including: obtaining environmental characteristics of the environment in which a carrier is located and obtaining motion characteristics of the carrier, determining a candidate loop position of the carrier based on a bag-of-words model, and calculating a loop detection confidence parameter of the candidate loop position determined based on the bag-of-words model according to the environmental characteristics and motion characteristics, wherein the loop detection confidence parameter characterizes the confidence that the candidate loop position is a historical loop position that the carrier has passed through; if the loop detection confidence parameter is greater than a preset threshold, the candidate loop position is determined as a loop detection result. It can be seen that, according to the environmental characteristics and motion characteristics of the carrier, the confidence that the candidate loop position is a historical loop position that the carrier has passed through can be determined, and then, after the loop detection position with a higher confidence is determined as the final loop detection result, the accuracy of the loop detection result finally determined can be improved.

Description

[0001] This application is a divisional application of the Chinese patent application filed with the State Intellectual Property Office of China on August 28, 2019, with application number CN201910804329.6 and invention name “A loop detection method and device for a SLAM system”. Technical Field

[0002] The present application relates to the technical field of loop closure detection, and in particular to a loop closure detection method and device for a SLAM system. Background Art

[0003] At present, SLAM (Simultaneous Localization And Mapping) systems are usually used for positioning and navigation of target carriers in environments without or with weak GPS (Global Positioning System). The target carrier can usually be an autonomous mobile carrier such as an unmanned vehicle, a drone, an unmanned ship or a robot. In SLAM systems, loop detection methods are usually used to improve global optimization accuracy and target positioning accuracy.

[0004] Loop detection, also known as closed-loop detection, refers to the autonomous mobile carrier identifying that the current scene is a scene it has reached before, so that the map established by the autonomous mobile carrier during the movement forms a closed loop.

[0005] In practical applications, the bag-of-words model can be used to perform loop detection on autonomous mobile carriers, that is, the features in the image perceived by the autonomous mobile carrier in real time are classified into words, and the words corresponding to the image are compared with the words in the pre-established dictionary to determine whether the scene the autonomous mobile carrier is currently in is consistent with the scene it has reached before, thereby achieving loop detection. However, the accuracy of the loop detection results obtained by using the bag-of-words model for loop detection is not high, which is prone to misjudgment. Summary of the invention

[0006] The embodiments of the present application provide a method and device for loop closure detection of a SLAM system, aiming to improve the accuracy of loop closure detection results determined based on a bag-of-words model.

[0007] In a first aspect, an embodiment of the present application provides a loop detection method for a SLAM system, the method comprising:

[0008] Acquire environmental characteristics of the environment in which the carrier is located, acquire motion characteristics of the carrier, and determine candidate loop positions of the carrier based on a bag-of-words model;

[0009] Calculating a loop detection confidence parameter for the candidate loop position according to the environmental features and the motion features, wherein the loop detection confidence parameter represents a degree of confidence that the candidate loop position is a historical loop position passed by the carrier;

[0010] If the loop detection confidence parameter is greater than a preset threshold, the candidate loop position is determined as the loop detection result.

[0011] In some possible implementations, the motion characteristics include a pitch angle, a roll angle, and a heading angle of the carrier, and the environmental characteristics include magnetic field information of an environment in which the carrier is located.

[0012] In some possible implementations, acquiring the motion characteristics of the carrier includes:

[0013] Acquiring specific force information output by an accelerometer installed on the carrier;

[0014] The pitch angle, roll angle and heading angle of the carrier are calculated according to the specific force information and the magnetic field information.

[0015] In some possible implementations, calculating the loop detection confidence parameter for the candidate loop position according to the environmental feature and the motion feature includes:

[0016] Determining a quantized index vector of the carrier at the current position according to the environmental characteristics and the motion characteristics;

[0017] Obtaining a quantization index vector corresponding to the candidate loop position;

[0018] A loop detection confidence parameter for the candidate loop position is calculated according to the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position.

[0019] In some possible implementations, calculating a loop detection confidence parameter for the candidate loop position according to a quantization index vector of the carrier at the current position and a quantization index vector corresponding to the candidate loop position includes:

[0020] Calculating the absolute value of the difference between the modulus lengths of each component in the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position;

[0021] According to the absolute value of the difference between the modulus lengths of the components, a loop detection confidence parameter for the candidate loop position is calculated.

[0022] In a second aspect, an embodiment of the present application further provides a loop detection device for a SLAM system, the device comprising:

[0023] An acquisition module, used to acquire environmental characteristics of the environment in which the carrier is located, acquire motion characteristics of the carrier, and determine candidate loop positions of the carrier based on a bag-of-words model;

[0024] A calculation module, configured to calculate a loop detection confidence parameter for the candidate loop position according to the environmental characteristics and the motion characteristics, wherein the loop detection confidence parameter represents a confidence that the candidate loop position is a historical loop position passed by the carrier;

[0025] A determination module is used to determine the candidate loop position as the loop detection result if the loop detection confidence parameter is greater than a preset threshold.

[0026] In some possible implementations, the motion characteristics include a pitch angle, a roll angle, and a heading angle of the carrier, and the environmental characteristics include magnetic field information of an environment in which the carrier is located.

[0027] In some possible implementations, the acquisition module includes:

[0028] A first acquisition unit, used to acquire specific force information output by an accelerometer installed on the carrier;

[0029] The first calculation unit is used to calculate the pitch angle, roll angle and heading angle of the carrier according to the specific force information and the magnetic field information.

[0030] In some possible implementations, the computing module includes:

[0031] A determination unit, configured to determine a quantization index vector of the carrier at the current position according to the environmental characteristics and the motion characteristics;

[0032] A second acquisition unit, used to acquire a quantization index vector corresponding to the candidate loop position;

[0033] The second calculation unit is used to calculate the loop detection confidence parameter for the candidate loop position according to the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position.

[0034] In some possible implementations, the second computing unit includes:

[0035] A first calculation subunit, configured to calculate an absolute value of a difference between the modulus lengths of each component in a quantization index vector of the carrier at the current position and a quantization index vector corresponding to the candidate loop position;

[0036] The second calculation subunit is used to calculate the loop detection confidence parameter for the candidate loop position according to the absolute value of the difference between the module lengths of the components.

[0037] In the above implementation of the embodiment of the present application, the confidence of the candidate loop position determined based on the bag-of-words model is determined according to the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier, so as to improve the accuracy of the loop detection result finally determined according to the confidence. Specifically, the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier, and the candidate loop position of the carrier determined based on the bag-of-words model can be obtained, and then, based on the environmental characteristics and the motion characteristics, the loop detection confidence parameter of the candidate loop position determined based on the bag-of-words model can be further calculated, wherein the loop detection confidence parameter characterizes the confidence that the candidate loop position is a historical loop position that the carrier has passed through; if the loop detection confidence parameter is greater than a preset threshold, the candidate loop position is determined as the loop detection result. It can be seen that although directly determining the candidate loop position of the carrier obtained based on the bag-of-words model as the loop detection result will make the accuracy of the final loop detection result lower, the confidence that the candidate loop position is a historical loop position that the carrier has passed through can be determined based on the environmental characteristics of the environment in which the carrier is located and the movement characteristics of the carrier. It can be understood that the higher the confidence, the higher the possibility that the candidate loop position is a historical loop position that the carrier has passed through, that is, the more likely the scene the carrier is currently in is a scene that it has passed through before, so the accuracy of using the candidate loop position as the loop detection result is higher, and further, after determining the loop detection position with a higher confidence as the final loop detection result, the accuracy of the final determined loop detection result can be improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present application. For ordinary technicians in this field, other drawings can also be obtained based on these drawings.

[0039] Figure 1 This is a schematic diagram of an exemplary application scenario in an embodiment of the present application;

[0040] Figure 2 A schematic diagram of a loop detection method for a SLAM system in an embodiment of the present application;

[0041] Figure 3 Schematic diagram of the structure of a loop detection device of a SLAM system in an embodiment of the present application. DETAILED DESCRIPTION

[0042] In actual applications, when loop closure detection is performed on autonomous mobile carriers based on the bag-of-words model, false positive or false negative detection results are likely to occur. Among them, a false positive refers to the erroneous identification of a scene that the carrier has not passed through (that is, the scene the carrier is currently in) as a scene that the carrier has passed through; and a false negative refers to the failure to identify a scene that the carrier has repeatedly passed through, that is, the failure to identify the scene the carrier is currently in as a scene that the carrier has passed through, or the erroneous correspondence between the scene the carrier is currently in and a scene that is actually different from the past. Therefore, the loop closure detection results obtained based on the existing bag-of-words model are not very accurate.

[0043] Based on this, the embodiment of the present application provides a loop detection method for a SLAM system, which determines the confidence of the candidate loop position determined based on the bag-of-words model according to the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier, so as to improve the accuracy of the loop detection result finally determined according to the confidence. Specifically, the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier, and the candidate loop position of the carrier determined based on the bag-of-words model can be obtained, and then, according to the environmental characteristics and the motion characteristics, the loop detection confidence parameter of the candidate loop position determined based on the bag-of-words model can be further calculated, wherein the loop detection confidence parameter characterizes the confidence that the candidate loop position is a historical loop position that the carrier has passed through; if the loop detection confidence parameter is greater than a preset threshold, the candidate loop position is determined as the loop detection result. It can be seen that although directly determining the candidate loop position of the carrier obtained based on the bag-of-words model as the loop detection result will make the accuracy of the final loop detection result lower, the confidence that the candidate loop position is a historical loop position that the carrier has passed through can be determined based on the environmental characteristics of the environment in which the carrier is located and the movement characteristics of the carrier. It can be understood that the higher the confidence, the higher the possibility that the candidate loop position is a historical loop position that the carrier has passed through, that is, the more likely the scene the carrier is currently in is a scene that it has passed through before, so the accuracy of using the candidate loop position as the loop detection result is higher, and further, after determining the loop detection position with a higher confidence as the final loop detection result, the accuracy of the final determined loop detection result can be improved.

[0044] As an example, the embodiments of the present application can be applied to Figure 1The exemplary application scenario shown. In this scenario, the user 101 can instruct (for example, by using a remote control or other controller) the robot 102 to move and build a map; since the robot 102 will have some accumulated errors in the process of building a map, the robot 102 can use the loop detection based on the bag-of-words model to optimize the posture. Specifically, the robot 102 can determine the candidate loop position of the robot 102 based on the bag-of-words model. Since the candidate loop position is not necessarily accurate, if the candidate loop position is directly used as the final loop detection result, the possibility of false positive or false negative is high. Therefore, the robot 102 can also obtain the environmental characteristics of the current environment in which it is located and obtain its own motion characteristics, and calculate the loop detection confidence parameter for the candidate loop position based on the environmental characteristics and motion characteristics. The loop detection confidence parameter represents the confidence that the candidate loop position is a historical loop position that the robot 102 has passed through; if the loop detection confidence parameter is greater than a preset threshold, the robot 102 can determine the candidate loop position as the final loop detection result.

[0045] It is understandable that the above scenario is only an example scenario provided by the embodiment of the present application, and the embodiment of the present application is not limited to this scenario. For example, in other possible application scenarios, the technical solution of the embodiment of the present application can also be applied to autonomously movable carriers such as drones, unmanned vehicles, and unmanned ships. For example, in some other possible application scenarios, a corresponding operation interface can be provided on the robot, and the user 101 can directly control the robot on the operation interface. In short, the embodiment of the present application can be applied to any applicable scenario, and is not limited to the above scenario examples.

[0046] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, various non-limiting implementation methods in the embodiments of the present application are exemplarily described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of this application.

[0047] See also Figure 2 , Figure 2 A schematic flow chart of a loop detection method of a SLAM system in an embodiment of the present application is shown, and the method may specifically include:

[0048] S201: Acquire environmental features of the environment in which the carrier is located, acquire motion features of the carrier, and determine candidate loop positions of the carrier based on a bag-of-words model.

[0049] In practical applications, if the candidate loop position determined based on the bag-of-words model is directly determined as the final loop detection result, false positives or false negatives are likely to occur, resulting in low accuracy of the loop detection result. To this end, in this embodiment, the candidate loop positions determined based on the bag-of-words model are screened based on confidence to improve the accuracy of the loop detection result. It should be noted that for determining the candidate loop positions of the carrier based on the bag-of-words model, there is a corresponding specific implementation process in the prior art, which will not be repeated here. In addition, the carrier described in this embodiment may specifically be a carrier with autonomous mobility, such as a drone, an unmanned vehicle, an unmanned ship, and a robot.

[0050] In this embodiment, the confidence of the candidate loop position can be determined based on the environmental characteristics of the environment in which the carrier is currently located and the motion characteristics of the carrier, so as to screen the candidate loop positions suitable as loop detection results. Therefore, while obtaining the candidate loop positions, the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier can also be obtained.

[0051] In an exemplary specific implementation, the environmental characteristics of the environment in which the carrier is located can specifically be the magnetic field information of the environment in which the carrier is located. In practical applications, a magnetic sensor can be installed in the carrier, and the magnetic field information output by the magnetic sensor can be read during the mapping process of the carrier. Among them, in the three-dimensional coordinate system established by the carrier itself (or can be called the b system, body coordinate system or carrier coordinate system), the magnetic field information read can specifically be the components in the three directions of the x-axis, y-axis and z-axis, such as (Mx_b, My_b, Mz_b) and the like.

[0052] The acquired motion characteristics of the carrier may specifically be the carrier pitch angle θ, roll angle γ and heading angle Ψ. Then, an accelerometer may be installed in the carrier. In this way, when acquiring the motion characteristics of the carrier, the specific force information output by the accelerometer in the b system may be read. The specific force information may also be the components in the three directions of the x-axis, y-axis and z-axis, such as (fx_b, fy_b, fz_b), etc. Then, based on the specific force information and the acquired magnetic field information, the pitch angle θ, roll angle γ and heading angle Ψ of the carrier may be calculated respectively according to the following formula (1).

[0053]

[0054] Wherein, "g" in formula (1) represents the acceleration due to gravity. In one example, g can be taken as 9.80665 m / s2; "D" is the magnetic declination at the position of the carrier on the earth, which can be obtained by looking up a specific table.

[0055] Furthermore, since the magnetic field information read from the magnetic sensor is based on the magnetic field information in the b system (Mx_b, My_b, Mz_b), in actual applications, when processing the magnetic field information, it may be necessary to convert the magnetic field information in the b system into the magnetic field information in the n system (i.e., navigation frame, navigation coordinate system) (Mx_n, My_n, Mz_n). Therefore, in the process of converting the magnetic field information in the b system into the magnetic field information in the n system, the coordinate conversion matrix between the two coordinate systems can be calculated according to formula (2): Based on the coordinate transformation matrix And the magnetic field information in the b system (Mx_b, My_b, Mz_b), the magnetic field information in the n system (Mx_n, My_n, Mz_n) is calculated according to formula (3).

[0056]

[0057]

[0058] S202: Calculate a loop detection confidence parameter for the candidate loop position based on the acquired environmental features and motion features, wherein the loop detection confidence parameter represents the confidence that the candidate loop position is a historical loop position that the carrier has passed through.

[0059] When the carrier moves to the same position successively (i.e., the current position of the carrier coincides with the historical loop position that the carrier has passed), the environmental features at the two same positions at different time points and the motion features of the carrier are usually the same. Therefore, based on the environmental features of the environment in which the carrier is located and the motion features of the carrier, the possibility that the candidate loop position is a historical loop position can be verified. In specific implementation, in this embodiment, for the candidate loop position determined by the bag-of-words model, the obtained environmental features and motion features can be used to calculate the loop detection confidence parameter, and the loop detection confidence parameter is used to characterize the confidence that the candidate loop position is the actual historical loop position. Among them, the greater the confidence, the higher the possibility that the candidate loop position is the historical loop position that the carrier has passed, and accordingly, the more reliable it is to use the candidate loop position as the final loop detection result; conversely, the smaller the confidence, the lower the possibility that the candidate loop position is a historical loop position, and accordingly, the less suitable it is for the candidate loop position to be determined as the final loop detection result.

[0060] As an exemplary implementation of calculating loop detection confidence parameters, the quantization index vector of the carrier at the current position can be determined according to environmental characteristics and motion characteristics, and the quantization index vector corresponding to the candidate loop position can be obtained. It can be understood that if the candidate loop position determined based on the bag-of-words model is indeed a historical loop position (the historical loop position coincides with the current position of the carrier), the quantization index vector of the carrier at the current position should be roughly the same as the quantization index vector corresponding to the candidate loop position; conversely, if the candidate loop position determined based on the bag-of-words model is not a historical loop position, the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position will usually be quite different. Therefore, in this embodiment, the loop detection confidence parameter for the candidate loop position can be calculated based on the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position.

[0061] As an example, when calculating the loop detection confidence parameter based on the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position, the absolute value of the difference between the modulus lengths of each component in the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position can be calculated, and then the corresponding loop detection confidence parameter can be further calculated based on the absolute value of the difference between the modulus lengths of each component.

[0062] For example, assuming that the current position of the carrier is Pi, the quantized index vector of the carrier at Pi can be Qi = [Ψ, Mx_b, My_b, Mz_b, Mx_n, My_n, Mz_n], where Ψ is the heading angle of the carrier at the position Pi. The quantized index vector corresponding to the candidate loop position Px can be Qx = [Ψ', Mx_b', My_b', Mz_b', Mx_n', My_n', Mz_n'], where Ψ' is the heading angle corresponding to the carrier at the candidate loop position, Mx_b', My_b', Mz_b' are the components of the magnetic field information based on the b system in the x-axis, y-axis and z-axis directions when the carrier is at the candidate loop position, and Mx_n', My_n', Mz_n' are the components of the magnetic field information based on the n system in the x-axis, y-axis and z-axis directions when the carrier is at the candidate loop position. Then, according to formula (4), the absolute value of the difference between the modulus lengths of each component in the quantization index vector Qi and the quantization index vector Qx can be calculated. Formula (4) is as follows:

[0063] ΔQ[k]=|Qi[k]|-|Qx[k]| (4)

[0064] Among them, ΔQ[k] represents the absolute value of the difference between the modulus of the kth component in the quantization indicator vector Qi and Qx. Since each quantization indicator vector in this example contains 7 components, the value of k can be 1 to 7 (in other embodiments, the value of k is determined by the number of components contained in the quantization indicator vector), |Qi[k]| represents the absolute value of the modulus of the kth component in the quantization indicator vector Qi, and |Qx[k]| represents the absolute value of the modulus of the kth component in the quantization indicator vector Qx.

[0065] Then, the corresponding loop detection confidence parameter M can be further calculated according to formula (5) and formula (6). Formula (5) and formula (6) are as follows:

[0066]

[0067]

[0068] S203: If the loop detection confidence parameter is greater than a preset threshold, the candidate loop position is determined as the loop detection result.

[0069] It can be understood that if the loop detection confidence parameter is large, specifically greater than a preset threshold, it indicates that the candidate loop position determined based on the bag-of-words model is more reliable as a historical loop position, and the candidate loop position can be determined as the final loop detection result; and if the loop detection confidence parameter is small, specifically not greater than the preset threshold, this indicates that if the candidate loop position is determined as the final loop detection result, the loop detection result has a greater possibility of false positive or false negative, so in some embodiments, the candidate loop position can be rejected as the loop detection result, and the loop detection can be performed again. In this way, using the loop detection confidence parameter, the candidate loop positions can be screened to a certain extent, so that only the candidate loop positions with higher confidence can be determined as the loop detection results, and some candidate loop positions with a greater possibility of false positive or false negative misjudgment can not be determined as the loop detection results, which can effectively improve the accuracy of the final loop detection result and reduce the misjudgment rate as much as possible.

[0070] In some possible implementations, the value of the preset threshold may be any value within a range of 0.7 to 1. Furthermore, when the accuracy of the loop detection result is required to be high, the value of the preset threshold may be appropriately increased, for example, the preset threshold may be set within a range of 0.9 to 1.

[0071] Furthermore, in practical applications, after the loop detection position is determined as the final loop detection result, the carrier (SLAM system on the carrier) can perform global pose optimization based on the candidate loop position. Otherwise, the carrier can continue to perform loop detection and repeat the above steps to screen the candidate loop positions re-determined by the bag-of-words model based on the loop detection confidence parameters.

[0072] Of course, if the movement of the carrier has not ended, the above process can be repeated to perform loop detection; and if the movement of the carrier has ended, the loop detection process can be stopped and the SLAM system can be turned off.

[0073] In this embodiment, the environmental characteristics of the environment in which the carrier is located, the motion characteristics of the carrier, and the candidate loop position of the carrier determined based on the bag-of-words model can be obtained. Then, based on the environmental characteristics and the motion characteristics, the loop detection confidence parameter of the candidate loop position determined based on the bag-of-words model can be further calculated, wherein the loop detection confidence parameter represents the confidence that the candidate loop position is a historical loop position that the carrier has passed through; if the loop detection confidence parameter is greater than a preset threshold, the candidate loop position is determined as the loop detection result. It can be seen that although directly determining the candidate loop position of the carrier obtained based on the bag-of-words model as the loop detection result will make the accuracy of the final loop detection result lower, based on the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier, the confidence that the candidate loop position is a historical loop position that the carrier has passed through can be determined. It can be understood that the higher the confidence, the higher the possibility that the candidate loop position is a historical loop position that the carrier has passed through, that is, the more likely the scene the carrier is currently in is a scene that the carrier has passed through before, and thus the accuracy of using the candidate loop position as the loop detection result is higher. Furthermore, after determining the loop detection position with a higher confidence as the final loop detection result, the accuracy of the final determined loop detection result can be improved.

[0074] In addition, the present application also provides a loop detection device for a SLAM system. Figure 3 , Figure 3 A loop detection device for a SLAM system in an embodiment of the present application is shown, and the device 300 includes:

[0075] An acquisition module 301 is used to acquire environmental features of the environment in which the carrier is located, acquire motion features of the carrier, and determine a candidate loop position of the carrier based on a bag-of-words model;

[0076] A calculation module 302 is used to calculate a loop detection confidence parameter for the candidate loop position according to the environmental characteristics and the motion characteristics, wherein the loop detection confidence parameter represents the confidence that the candidate loop position is a historical loop position passed by the carrier;

[0077] The determination module 303 is configured to determine the candidate loop position as the loop detection result if the loop detection confidence parameter is greater than a preset threshold.

[0078] In some possible implementations, the motion characteristics include a pitch angle, a roll angle, and a heading angle of the carrier, and the environmental characteristics include magnetic field information of an environment in which the carrier is located.

[0079] In some possible implementations, the acquisition module 301 includes:

[0080] A first acquisition unit, used to acquire specific force information output by an accelerometer installed on the carrier;

[0081] The first calculation unit is used to calculate the pitch angle, roll angle and heading angle of the carrier according to the specific force information and the magnetic field information.

[0082] In some possible implementations, the calculation module 302 includes:

[0083] A determination unit, configured to determine a quantization index vector of the carrier at the current position according to the environmental characteristics and the motion characteristics;

[0084] A second acquisition unit, used to acquire a quantization index vector corresponding to the candidate loop position;

[0085] The second calculation unit is used to calculate the loop detection confidence parameter for the candidate loop position according to the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position.

[0086] In some possible implementations, the second computing unit includes:

[0087] A first calculation subunit, configured to calculate an absolute value of a difference between the modulus lengths of each component in a quantization index vector of the carrier at the current position and a quantization index vector corresponding to the candidate loop position;

[0088] The second calculation subunit is used to calculate the loop detection confidence parameter for the candidate loop position according to the absolute value of the difference between the module lengths of the components.

[0089] It is worth noting that the loop detection device of the SLAM system described in this embodiment corresponds to the loop detection method of the SLAM system described in the above method embodiment. Therefore, its specific implementation method can refer to the relevant description of the method embodiment and will not be repeated here.

[0090] In this embodiment, although directly determining the candidate loop position of the carrier obtained based on the bag-of-words model as the loop detection result will make the accuracy of the final loop detection result lower, the confidence that the candidate loop position is a historical loop position that the carrier has passed through can be determined based on the environmental characteristics of the environment in which the carrier is located and the motion characteristics of the carrier. It can be understood that the higher the confidence, the higher the possibility that the candidate loop position is a historical loop position that the carrier has passed through, that is, the more likely the scene the carrier is currently in is a scene that it has passed through before, so the accuracy of using the candidate loop position as the loop detection result is higher, and further, after determining the loop detection position with a higher confidence as the final loop detection result, the accuracy of the final determined loop detection result can be improved.

[0091] The "first" in the names of "first acquisition unit", "first calculation unit", "first calculation subunit" and the like mentioned in the embodiments of the present application is only used as a name identifier and does not mean the first in order. The same rule applies to "second" and the like.

[0092] Through the description of the above implementation methods, it can be known that those skilled in the art can clearly understand that all or part of the steps in the above-mentioned embodiment method can be implemented by means of software plus a general hardware platform. Based on such an understanding, the technical solution of the present application can be embodied in the form of a software product, which can be stored in a storage medium, such as a read-only memory (ROM) / RAM, a magnetic disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network communication device such as a router) to execute the methods described in each embodiment of the present application or some parts of the embodiments.

[0093] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can refer to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, in which the modules described as separate components may or may not be physically separated, and the components displayed as modules may or may not be physical modules, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative work.

[0094] The above description is merely an exemplary embodiment of the present application and is not intended to limit the protection scope of the present application.

Claims

1. A loop detection method for a SLAM system, characterized in that: The method comprises: Acquire environmental characteristics of the environment in which the carrier is located, acquire motion characteristics of the carrier, and determine candidate loop positions of the carrier based on a bag-of-words model; Calculating loop detection confidence parameters corresponding to the candidate loop positions respectively according to the environmental features and the motion features, the loop detection confidence parameters representing the confidence that the candidate loop positions corresponding to the loop detection confidence parameters are historical loop positions passed by the carrier; Determine the candidate loop closure positions whose loop closure detection confidence parameters are greater than a preset threshold value as loop closure detection results; The step of calculating the loop detection confidence parameters corresponding to the candidate loop positions respectively according to the environmental features and the motion features includes: For the candidate loop position, determine the quantization index vector of the carrier at the current position according to the environmental characteristics and the motion characteristics; obtain the quantization index vector corresponding to the candidate loop position; calculate the loop detection confidence parameter for the candidate loop position according to the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position; The step of calculating the loop detection confidence parameter for the candidate loop position according to the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position includes: Calculating the absolute value of the difference between the modulus lengths of each component in the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position; and calculating the loop detection confidence parameter for the candidate loop position according to the absolute value of the difference between the modulus lengths of each component; The calculation of the absolute value of the difference between the modulus lengths of each component in the quantization index vector of the carrier at the current position and the quantization index vector corresponding to the candidate loop position is specifically: ΔQ[k]=|Qi[k]|-|Qx[k]|; Wherein, ΔQ[k] represents the absolute value of the difference between the modulus of the kth component in the quantization index vector Qi and Qx, k=1,2,…,7, |Qi[k]| represents the absolute value of the modulus of the kth component in the quantization index vector Qi, |Qx[k]| represents the absolute value of the modulus of the kth component in the quantization index vector Qx; Qi=[Ψ, Mx_b, My_b, Mz_b, Mx_n, My_n, Mz_n], wherein Ψ is the heading angle of the carrier at the current position, Mx_b, My_b, Mz_b are the components of the magnetic field information of the carrier based on the b system in the x-axis, y-axis and z-axis directions respectively when the carrier is at the current position, Mx_n, My_n, M z_n are the components of the magnetic field information of the carrier based on the n system in the x-axis, y-axis and z-axis directions when the carrier is at the current position; Qx = [Ψ', Mx_b', My_b', Mz_b', Mx_n', My_n', Mz_n'], wherein Ψ' is the heading angle corresponding to the carrier at the candidate loop position, Mx_b', My_b', Mz_b' are the components of the magnetic field information of the carrier based on the b system in the x-axis, y-axis and z-axis directions when the carrier is at the candidate loop position, Mx_n', My_n', Mz_n' are the components of the magnetic field information of the carrier based on the n system in the x-axis, y-axis and z-axis directions when the carrier is at the candidate loop position; Among them, the loop detection confidence parameter for the candidate loop position is calculated according to the absolute value of the difference between the modulus lengths of each component, specifically: Wherein, M is the loop detection confidence parameter of the candidate loop position.

2. The method according to claim 1, characterized in that The motion characteristics include the pitch angle, roll angle and heading angle of the carrier, and the environmental characteristics include magnetic field information of the environment in which the carrier is located.

3. The method according to claim 2, characterized in that The obtaining of the motion characteristics of the carrier includes: Acquiring specific force information output by an accelerometer installed on the carrier; The pitch angle, roll angle and heading angle of the carrier are calculated according to the specific force information and the magnetic field information.

4. A loop detection device for a SLAM system, characterized in that: The device comprises: An acquisition module, used to acquire environmental characteristics of the environment in which the carrier is located, acquire motion characteristics of the carrier, and determine candidate loop positions of the carrier based on a bag-of-words model; A calculation module, configured to calculate loop detection confidence parameters corresponding to the candidate loop positions respectively according to the environmental features and the motion features, wherein the loop detection confidence parameters represent the confidence that the candidate loop positions corresponding to the loop detection confidence parameters are historical loop positions passed by the carrier; A determination module, configured to determine, among the candidate loop positions, a candidate loop position whose loop detection confidence parameter is greater than a preset threshold as a loop detection result; The computing module comprises: A determination unit, configured to determine, for the candidate loop position, a quantization index vector of the carrier at a current position according to the environmental characteristics and the motion characteristics; A second acquisition unit, used to acquire a quantization index vector corresponding to the candidate loop position; A second calculation unit, configured to calculate a loop detection confidence parameter for the candidate loop position according to a quantization index vector of the carrier at the current position and a quantization index vector corresponding to the candidate loop position; The second computing unit comprises: A first calculation subunit, configured to calculate an absolute value of a difference between the modulus lengths of each component in a quantization index vector of the carrier at the current position and a quantization index vector corresponding to the candidate loop position; A second calculation subunit is used to calculate a loop detection confidence parameter for the candidate loop position according to the absolute value of the difference between the modulus lengths of the components; The first calculation subunit is specifically used to calculate according to ΔQ[k]=|Qi[k]|-|Qx[k]|; wherein ΔQ[k] represents the absolute value of the difference between the modulus lengths of the kth component in the quantization index vector Qi and Qx, k=1,2,…,7, |Qi[k]| represents the absolute value of the modulus length of the kth component in the quantization index vector Qi, and |Qx[k]| represents the absolute value of the modulus length of the kth component in the quantization index vector Qx; Qi=[Ψ, Mx_b, My_b, Mz_b, Mx_n, My_n, Mz_n], wherein Ψ is the heading angle of the carrier at the current position, Mx_b, My_b, and Mz_b are respectively the x-axis and y-axis values ​​of the carrier based on the magnetic field information of the b system when the carrier is at the current position. and the components in the z-axis direction, Mx_n, My_n, Mz_n are respectively the components of the magnetic field information of the carrier based on the n-system in the x-axis, y-axis and z-axis directions when the carrier is at the current position; Qx = [Ψ', Mx_b', My_b', Mz_b', Mx_n', My_n', Mz_n'], wherein Ψ' is the heading angle corresponding to the carrier at the candidate loop position, Mx_b', My_b', Mz_b' are respectively the components of the magnetic field information of the carrier based on the b-system in the x-axis, y-axis and z-axis directions when the carrier is at the candidate loop position, and Mx_n', My_n', Mz_n' are respectively the components of the magnetic field information of the carrier based on the n-system in the x-axis, y-axis and z-axis directions when the carrier is at the candidate loop position; The first calculation subunit is specifically used to perform calculation according to the following formula: Wherein, M is the loop detection confidence parameter of the candidate loop position.

5. The device according to claim 4, characterized in that The motion characteristics include the pitch angle, roll angle and heading angle of the carrier, and the environmental characteristics include magnetic field information of the environment in which the carrier is located.

6. The device according to claim 5, characterized in that The acquisition module comprises: A first acquisition unit, used to acquire specific force information output by an accelerometer installed on the carrier; The first calculation unit is used to calculate the pitch angle, roll angle and heading angle of the carrier according to the specific force information and the magnetic field information.

Images