Loopback detection method of environment frame

Abstract

The invention provides a loopback detection method for environment frames, and the method comprises the steps: collecting environment information in a moving process of electronic equipment, and obtaining a plurality of environment frames, and environment collection data and pose data corresponding to each environment frame; according to a preset interval frame number and the environment acquisition data, determining a plurality of first candidate environment frames with the maximum feature matching degree with the current environment frame; according to the pose data, obtaining first two second candidate environment frames from the plurality of first candidate environment frames, the pose distance between the first two second candidate environment frames and the current environment frame being minimum; obtaining an effective error of the two second candidate environment frames according to the pose data and the environment acquisition data; and determining the second candidate environment frame with the minimum effective error as a loopback environment frame. The loopback environment frame can be accurately detected, and the accuracy of the loopback detection result is improved.

Description

Technical Field

[0001] The present application relates to the technical field of loop closure detection of environmental frames, and in particular to a loop closure detection method for environmental frames. Background Art

[0002] With the development of intelligence, AGV, AMR, drones, unmanned ships, self-driving cars, etc. are widely used in various intelligent industries. Mapping and positioning technology is the key technology, and the accuracy of loop detection algorithm directly determines the quality and accuracy of mapping and positioning. The current loop detection methods are based on topological structure, visual semantic information, lidar scan context features, and visual ORB features. Their defects are that they will detect wrong loop results in similar environments, thus affecting the accuracy of mapping and positioning. Summary of the invention

[0003] The purpose of this application is to provide a loop detection method for an environmental frame, which can overcome the shortcomings and deficiencies in the prior art.

[0004] A first aspect of an embodiment of the present application provides a loop detection method for an environment frame, comprising:

[0005] During the movement of the electronic device, environmental information is collected to obtain multiple environmental frames, as well as environmental collection data and posture data corresponding to each environmental frame;

[0006] Determine, according to a preset interval frame number and the environmental collection data, a first plurality of first candidate environmental frames having the greatest feature matching degree with the current environmental frame;

[0007] According to the posture data, obtaining, from the plurality of first candidate environment frames, first two second candidate environment frames having the smallest posture distances to the current environment frame;

[0008] Obtaining effective errors of two second candidate environment frames according to the posture data and the environment acquisition data;

[0009] The second candidate environment frame with the smallest effective error is determined as the loopback environment frame.

[0010] Furthermore, the step of determining the first several first candidate environment frames with the greatest feature matching degree with the current environment frame according to the preset interval frame number and the environment collection data includes:

[0011] Determine an environment frame whose absolute value of the frame number difference with the current environment frame is greater than the interval frame number as a candidate environment frame;

[0012] Performing feature matching on the first data feature of the environment collection data of each candidate environment frame and the second data feature of the environment collection data of the current environment frame to obtain a feature matching degree between each candidate environment frame and the current environment frame;

[0013] The first several candidate environment frames with the largest feature matching degree are determined as the first candidate environment frames.

[0014] Further, the step of obtaining, from the plurality of first candidate environment frames, first two second candidate environment frames having the smallest posture distance to the current environment frame according to the posture data comprises:

[0015] Obtaining a pose difference between the pose data of each of the first candidate environment frames and the pose data of the current environment frame;

[0016] The absolute value of each of the posture differences is determined as the posture distance between the corresponding first candidate environment frame and the current environment frame, and the first two first candidate environment frames with the smallest posture distance are determined as the second candidate environment frames.

[0017] Further, the step of obtaining effective errors of two second candidate environment frames according to the posture data and the environment acquisition data includes:

[0018] According to the pose data of the two second candidate environment frames and the pose data of the current environment frame, respectively obtaining the first pose relationship of the two second candidate environment frames;

[0019] According to the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame, respectively obtain the second posture relationship of the two second candidate environment frames;

[0020] According to the first posture relationship and the second posture relationship of each of the second candidate environment frames, the effective error of each of the second candidate environment frames is obtained.

[0021] Further, the step of respectively obtaining the first pose relationship between two second candidate environment frames according to the pose data of the two second candidate environment frames and the pose data of the current environment frame comprises:

[0022] The first posture relationship of the second candidate environment frame with the smallest posture distance is obtained by the following formula:

[0023] Transform[distanc_min]=pose_vector[distanc_min]×pose.inverse()

[0024] Transform[distanc_min] is the first pose relationship of the second candidate environment frame with the smallest pose distance; pose_vector[distanc_min] is the pose data of the second candidate environment frame with the smallest pose distance; pose is the pose data of the current environment frame; .inverse() represents the matrix inversion operation;

[0025] The first posture relationship of the second candidate environment frame with the second smallest posture distance is obtained by the following formula:

[0026] Transform[distance_second_min]=pose_vector[distance_second_min]×pose.inverse()

[0027] Transform[distance_second_min] is the first pose relationship of the second candidate environment frame with the second smallest pose distance; pose_vector[distance_second_min] is the pose data of the second candidate environment frame with the second smallest pose distance.

[0028] Further, the step of respectively obtaining the second posture relationship between the two second candidate environment frames according to the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame comprises:

[0029] A matching algorithm is performed based on the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame to obtain a second posture relationship between the two second candidate environment frames.

[0030] Further, the step of obtaining the effective error of each of the second candidate environment frames according to the first posture relationship and the second posture relationship of each of the second candidate environment frames includes:

[0031] Comparing the first posture relationship with a preset posture relationship threshold to obtain a posture comparison result;

[0032] Obtaining a posture relationship error of the second candidate environment frame according to the first posture relationship and the second posture relationship of the same second candidate environment frame;

[0033] If the pose comparison result of the second candidate environment frame is that the first pose relationship is less than the pose relationship threshold, and the corresponding pose relationship error of the second candidate environment frame is less than the preset error threshold, it is determined that the pose relationship error is the effective error of the second candidate environment frame.

[0034] Further, the step of obtaining a posture relationship error of the second candidate environment frame according to the first posture relationship and the second posture relationship of the same second candidate environment frame includes:

[0035] The pose relationship error of the second candidate environment frame with the smallest pose distance is obtained by the following formula:

[0036] Error[distanc_min]=Transform[distanc_min]×Transform[distanc_min]'.inverse()

[0037] Error[distanc_min] is the posture relationship error of the second candidate environment frame with the smallest posture distance; Transform[distanc_min] is the first posture relationship of the second candidate environment frame with the smallest posture distance; Transform[distanc_min]' is the second posture relationship of the second candidate environment frame with the smallest posture distance; .inverse() represents the matrix inversion operation;

[0038] The pose relationship error of the second candidate environment frame with the second smallest pose distance is obtained by the following formula:

[0039] Error[distance_second_min]=Transform[distance_second_min]×Transform[distance_second_min]'.inverse()

[0040] Error[distance_second_min] is the posture relationship error of the second candidate environment frame with the second smallest posture distance; Transform[distance_second_min] is the first posture relationship of the second candidate environment frame with the second smallest posture distance; Transform[distance_second_min]' is the second posture relationship of the second candidate environment frame with the second smallest posture distance.

[0041] Furthermore, it also includes:

[0042] Obtaining a third posture relationship between the current environment frame and the previous key frame; wherein the first environment frame is a key frame;

[0043] If the third posture relationship is greater than a preset key posture relationship threshold, determining the current environment frame as a key frame;

[0044] The environment acquisition data and the position and posture data of the current environment frame are saved.

[0045] Further, the step of obtaining the third posture relationship between the current environment frame and the previous key frame includes:

[0046] The third pose relationship between the current environment frame and the previous key frame is obtained through the following formula:

[0047] T=pose×pose key .inverse()

[0048] Among them, T is the third pose relationship between the current environment frame and the previous key frame; pose is the pose data of the current environment frame; pose key It is the pose data of the previous key frame; .i nverse() represents the matrix inverse operation.

[0049] Compared with the prior art, the present application performs feature matching on the environmental collection data to determine the first several first candidate environmental frames with the greatest feature matching degree with the current environmental frame, and then obtains the first two second candidate environmental frames with the smallest posture distance with the current environmental frame according to the posture distance, and then judges the validity of the second candidate environmental frames according to the posture data and the environmental collection data to obtain the effective error of the corresponding valid second candidate environmental frame, so as to determine the second candidate environmental frame with the smallest effective error as the loopback environmental frame, and realizes the screening of the most reasonable loopback detection results based on multiple loopback detections of several first candidate environmental frames, thereby improving the accuracy of obtaining loopback environmental frames, and can correctly detect loopbacks in similar environments, thereby improving the stability of loopback detection.

[0050] In order to provide a clearer understanding of the present application, the specific implementation of the present application will be described below in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 A flowchart of a loop detection method for an environment frame according to an embodiment of the present application is shown.

[0052] Figure 2 A logical diagram of a loop detection method for an environment frame according to an embodiment of the present application.

[0053] Figure 3 1 is a flow chart of steps S21-S23 of a loop detection method for an environment frame according to an embodiment of the present application. DETAILED DESCRIPTION

[0054] In order to make the objectives, technical solutions and advantages of the present application more clear, the embodiments of the present application will be further described in detail below with reference to the accompanying drawings.

[0055] It should be clear that the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the embodiments of the present application.

[0056] When the following description relates to the accompanying drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. In the description of the present application, it should be understood that the terms "first", "second", "third", etc. are only used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence, nor can they be understood as indicating or implying relative importance. For those of ordinary skill in the art, the specific meanings of the above terms in the present application can be understood according to the specific circumstances. The singular forms of "a", "said" and "the" used in the present application and the appended claims are also intended to include the majority form, unless the context clearly indicates other meanings. The words "if" / "if" used herein can be interpreted as "at the time of" or "when" or "in response to determination".

[0057] In addition, in the description of this application, unless otherwise specified, "plurality" means two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the associated objects are in an "or" relationship.

[0058] See also Figure 1-3 , a loop detection method for an environment frame in one embodiment of the present application includes:

[0059] S1: When the electronic device moves, environmental information is collected to obtain multiple environmental frames, as well as environmental collection data and posture data corresponding to each environmental frame.

[0060] Among them, the electronic device can be AGV (automatic guided vehicle), AMR (intelligent robot), drone, unmanned ship, self-driving car, etc. The environmental information can be collected by information collection equipment such as camera, millimeter wave radar, ultrasonic radar, laser radar, etc.

[0061] S2: Determine the first several first candidate environment frames with the greatest feature matching degree with the current environment frame according to the preset interval frame number and the environment collection data.

[0062] The current environment frame refers to the most recently collected environment frame. The environment collection data and pose data of the current environment frame will be saved. For example, the environment collection data scan of the current environment frame is stored in the data vector scan_vector of the environment collection data, with the serial number i, which is the environment collection data of the i-th environment frame. The pose pose of the current environment frame is stored in the pose vector pose_vector of the pose data, with the serial number i, which is the pose data of the i-th environment frame.

[0063] Among them, the number of first candidate environment frames is greater than 2. The number of first candidate environment frames can be set by the user. For example, when it is set to 3, the top three first candidate environment frames with the largest feature matching degree can be obtained, and their corresponding feature serial numbers can be recorded as candidate1, candidate2, candidate3. At the same time, the corresponding pose data pose_vector[candidate1], pose_vector[candidate2], pose_vector[candidate3] are recorded, and the corresponding environment acquisition data scan_vector[candidate1], scan_vector[candidate2], scan_vector[candidate3] are recorded.

[0064] Wherein, the step S2: determining the first several first candidate environment frames with the greatest feature matching degree with the current environment frame according to the preset interval frame number and the environment collection data, comprises:

[0065] S21: Determine an environment frame whose absolute value of the frame number difference with the current environment frame is greater than the interval frame number as a candidate environment frame.

[0066] S22: Perform feature matching on the first data feature of the environment collection data of each candidate environment frame and the second data feature of the environment collection data of the current environment frame to obtain a feature matching degree between each candidate environment frame and the current environment frame.

[0067] The first data feature and the second data feature may be scancontext features, line and surface features, image features, etc. The scancontext feature is an encoder that describes the spatial distribution of laser radar data, and scancontext is an open source algorithm used for laser radar feature extraction.

[0068] The first data feature and the second data feature may be stored as a feature vector sc_vector.

[0069] S23: Determine the first several candidate environment frames with the largest feature matching degree as the first candidate environment frames.

[0070] S3: According to the posture data, obtain, from the plurality of first candidate environment frames, first two second candidate environment frames having the smallest posture distances to the current environment frame.

[0071] Wherein, the step S3: obtaining, from the plurality of first candidate environment frames, the first two second candidate environment frames having the smallest posture distance to the current environment frame according to the posture data, comprises:

[0072] S31: Obtaining the posture difference between the posture data of each of the first candidate environment frames and the posture data of the current environment frame. The posture distances between the three first candidate environment frames and the current environment frame in the above example can be obtained by the following formula:

[0073] distance1=||pose_vector[candidate1]-pose|| 2

[0074] distance2=||pose_vector[candidate2]-pose|| 2

[0075] distance3=||pose_vector[candidate3]-pose|| 2

[0076] distance1, distance2 and distance3 are the Euclidean distances between the three first candidate environment frames and the current environment frame, which are also pose distances. pose_vector[candidate1], pose_vector[candidate2] and pose_vector[candidate3] are the pose data of the three first candidate environment frames, and pose is the pose data of the current environment frame.

[0077] S32: Determine the absolute value of each of the posture differences as the corresponding posture distance between the first candidate environment frame and the current environment frame, and determine the first two first candidate environment frames with the smallest posture distance as the second candidate environment frames.

[0078] S4: Obtain effective errors of two second candidate environment frames according to the posture data and the environment acquisition data.

[0079] Wherein, the step S4: obtaining effective errors of two second candidate environment frames according to the posture data and the environment acquisition data comprises:

[0080] S41: According to the pose data of the two second candidate environment frames and the pose data of the current environment frame, respectively obtain the first pose relationship of the two second candidate environment frames.

[0081] S42: Obtaining second posture relationships between two second candidate environment frames respectively according to the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame.

[0082] S43: Obtaining effective errors of each of the second candidate environment frames according to the first posture relationship and the second posture relationship of each of the second candidate environment frames.

[0083] In a feasible embodiment, the step S41: obtaining the first pose relationship between two second candidate environment frames according to the pose data of the two second candidate environment frames and the pose data of the current environment frame, respectively, comprises:

[0084] The first posture relationship of the second candidate environment frame with the smallest posture distance is obtained by the following formula:

[0085] Transform[distanc_min]=pose_vector[distanc_min]×pose.inverse()

[0086] Transform[distanc_min] is the first pose relationship of the second candidate environment frame with the smallest pose distance; pose_vector[distanc_min] is the pose data of the second candidate environment frame with the smallest pose distance; pose is the pose data of the current environment frame; .inverse() represents the matrix inversion operation;

[0087] The first posture relationship of the second candidate environment frame with the second smallest posture distance is obtained by the following formula:

[0088] Transform[distance_second_min]=pose_vector[distance_second_min]×pose.inverse()

[0089] Transform[distance_second_min] is the first pose relationship of the second candidate environment frame with the second smallest pose distance; pose_vector[distance_second_min] is the pose data of the second candidate environment frame with the second smallest pose distance.

[0090] In a feasible embodiment, the step S42: obtaining the second posture relationship between the two second candidate environment frames according to the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame, respectively, comprises:

[0091] A matching algorithm is performed based on the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame to obtain a second posture relationship between the two second candidate environment frames.

[0092] The matching algorithm can be an NDT matching algorithm, an ICP matching algorithm and a variant of the ICP matching algorithm, an image feature matching algorithm, etc. The NDT matching algorithm is an open source algorithm from the PCL library, which aims to calculate the position transformation relationship between point clouds.

[0093] In a feasible embodiment, the step of obtaining the effective error of each of the second candidate environment frames according to the first pose relationship and the second pose relationship of each of the second candidate environment frames in S43 includes:

[0094] S431: Compare the first posture relationship with a preset posture relationship threshold to obtain a posture comparison result.

[0095] S432: Obtaining a posture relationship error of the second candidate environment frame according to the first posture relationship and the second posture relationship of the same second candidate environment frame.

[0096] S433: If the pose comparison result of the second candidate environment frame is that the first pose relationship is less than the pose relationship threshold, and the corresponding pose relationship error of the second candidate environment frame is less than the preset error threshold, it is determined that the pose relationship error is a valid error of the second candidate environment frame.

[0097] In a feasible embodiment, the step S432: obtaining a posture relationship error of the second candidate environment frame according to the first posture relationship and the second posture relationship of the same second candidate environment frame, comprises:

[0098] The pose relationship error of the second candidate environment frame with the smallest pose distance is obtained by the following formula:

[0099] Error[distanc_min]=Transform[distanc_min]×Transform[distanc_min]'.inverse()

[0100] Error[distanc_min] is the posture relationship error of the second candidate environment frame with the smallest posture distance; Transform[distanc_min] is the first posture relationship of the second candidate environment frame with the smallest posture distance; Transform[distanc_min]' is the second posture relationship of the second candidate environment frame with the smallest posture distance; .inverse() represents the matrix inversion operation;

[0101] The pose relationship error of the second candidate environment frame with the second smallest pose distance is obtained by the following formula:

[0102] Error[distance_second_min]=Transform[distance_second_min]×Transform[distance_second_min]'.inverse()

[0103] Error[distance_second_min] is the posture relationship error of the second candidate environment frame with the second smallest posture distance; Transform[distance_second_min] is the first posture relationship of the second candidate environment frame with the second smallest posture distance; Transform[distance_second_min]' is the second posture relationship of the second candidate environment frame with the second smallest posture distance.

[0104] S5: Determine the second candidate environment frame with the smallest effective error as the loopback environment frame.

[0105] In a feasible embodiment, it also includes:

[0106] Obtaining a third posture relationship between the current environment frame and the previous key frame; wherein the first environment frame is a key frame;

[0107] If the third posture relationship is greater than a preset key posture relationship threshold, determining the current environment frame as a key frame;

[0108] The environment acquisition data and the position and posture data of the current environment frame are saved.

[0109] Wherein, the step of obtaining the third posture relationship between the current environment frame and the previous key frame includes:

[0110] The third pose relationship between the current environment frame and the previous key frame is obtained through the following formula:

[0111] T=pose×pose key .inverse()

[0112] Among them, T is the third pose relationship between the current environment frame and the previous key frame; pose is the pose data of the current environment frame; pose key It is the pose data of the previous key frame; .i nverse() represents the matrix inverse operation.

[0113] Compared with the prior art, the present application performs feature matching on the environmental collection data to determine the first several first candidate environmental frames with the greatest feature matching degree with the current environmental frame, and then obtains the first two second candidate environmental frames with the smallest posture distance with the current environmental frame according to the posture distance, and then judges the validity of the second candidate environmental frames according to the posture data and the environmental collection data to obtain the effective error of the corresponding valid second candidate environmental frame, so as to determine the second candidate environmental frame with the smallest effective error as the loopback environmental frame, and realizes the screening of the most reasonable loopback detection results based on multiple loopback detections of several first candidate environmental frames, thereby improving the accuracy of obtaining loopback environmental frames, and can correctly detect loopbacks in similar environments, thereby improving the stability of loopback detection.

[0114] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0115] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific way, so that the instructions stored in the computer-readable memory produce a product including an instruction device, which implements the function selected in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 function selected in a box or multiple boxes.

[0116] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 steps for the function selected in a box or multiple boxes.

[0117] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0118] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0119] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0120] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0121] The above are only embodiments of the present application and are not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included within the scope of the claims of the present application.

Claims

1. A loop detection method for an environment frame, characterized in that: include: During the movement of the electronic device, environmental information is collected to obtain multiple environmental frames, as well as environmental collection data and posture data corresponding to each environmental frame; Determine, according to a preset interval frame number and the environmental collection data, a first plurality of first candidate environmental frames having the greatest feature matching degree with the current environmental frame; According to the posture data, obtaining, from the plurality of first candidate environment frames, first two second candidate environment frames having the smallest posture distances to the current environment frame; Obtaining effective errors of two second candidate environment frames according to the posture data and the environment acquisition data; The second candidate environment frame with the smallest effective error is determined as the loopback environment frame.

2. The loop detection method of the environment frame according to claim 1, characterized in that: The step of determining the first several first candidate environment frames having the greatest feature matching degree with the current environment frame according to the preset interval frame number and the environment collection data comprises: Determine an environment frame whose absolute value of the frame number difference with the current environment frame is greater than the interval frame number as a candidate environment frame; Performing feature matching on the first data feature of the environment collection data of each candidate environment frame and the second data feature of the environment collection data of the current environment frame to obtain a feature matching degree between each candidate environment frame and the current environment frame; The first several candidate environment frames with the largest feature matching degree are determined as the first candidate environment frames.

3. The loop detection method of the environment frame according to claim 1, characterized in that: The step of obtaining, from the plurality of first candidate environment frames, first two second candidate environment frames having the smallest posture distance to the current environment frame according to the posture data comprises: Obtaining a pose difference between the pose data of each of the first candidate environment frames and the pose data of the current environment frame; The absolute value of each of the posture differences is determined as the posture distance between the corresponding first candidate environment frame and the current environment frame, and the first two first candidate environment frames with the smallest posture distance are determined as the second candidate environment frames.

4. The loop detection method of the environment frame according to claim 1, characterized in that: The step of obtaining effective errors of two second candidate environment frames according to the posture data and the environment acquisition data comprises: According to the pose data of the two second candidate environment frames and the pose data of the current environment frame, respectively obtaining the first pose relationship of the two second candidate environment frames; According to the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame, respectively obtain the second posture relationship of the two second candidate environment frames; According to the first posture relationship and the second posture relationship of each of the second candidate environment frames, the effective error of each of the second candidate environment frames is obtained.

5. The loop detection method of the environment frame according to claim 4, characterized in that: The step of respectively obtaining the first pose relationship between two second candidate environment frames according to the pose data of the two second candidate environment frames and the pose data of the current environment frame comprises: The first posture relationship of the second candidate environment frame with the smallest posture distance is obtained by the following formula: Transform[distanc_min]=pose_vector[distanc_min]×pose.inverse() Transform[distanc_min] is the first pose relationship of the second candidate environment frame with the smallest pose distance; pose_vector[distanc_min] is the pose data of the second candidate environment frame with the smallest pose distance; pose is the pose data of the current environment frame; .inverse() represents the matrix inversion operation; The first posture relationship of the second candidate environment frame with the second smallest posture distance is obtained by the following formula: Transform[distance_second_min]=pose_vector[distance_second_min]×pose.inverse() Transform[distance_second_min] is the first pose relationship of the second candidate environment frame with the second smallest pose distance; pose_vector[distance_second_min] is the pose data of the second candidate environment frame with the second smallest pose distance.

6. The loop detection method of the environment frame according to claim 4, characterized in that: The step of respectively obtaining the second posture relationship between the two second candidate environment frames according to the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame comprises: A matching algorithm is performed based on the environment collection data of the two second candidate environment frames and the environment collection data of the current environment frame to obtain a second posture relationship between the two second candidate environment frames.

7. The loop detection method of the environment frame according to claim 4, characterized in that: The step of obtaining the effective error of each of the second candidate environment frames according to the first posture relationship and the second posture relationship of each of the second candidate environment frames comprises: Comparing the first posture relationship with a preset posture relationship threshold to obtain a posture comparison result; Obtaining a posture relationship error of the second candidate environment frame according to the first posture relationship and the second posture relationship of the same second candidate environment frame; If the pose comparison result of the second candidate environment frame is that the first pose relationship is less than the pose relationship threshold, and the corresponding pose relationship error of the second candidate environment frame is less than the preset error threshold, it is determined that the pose relationship error is the effective error of the second candidate environment frame.

8. The loop detection method of the environment frame according to claim 7, characterized in that: The step of obtaining a posture relationship error of the second candidate environment frame according to the first posture relationship and the second posture relationship of the same second candidate environment frame comprises: The pose relationship error of the second candidate environment frame with the smallest pose distance is obtained by the following formula: Error[distanc_min]= Transform[distanc_min]×Transform[distanc_min]'.inverse() Error[distanc_min] is the posture relationship error of the second candidate environment frame with the smallest posture distance; Transform[distanc_min] is the first posture relationship of the second candidate environment frame with the smallest posture distance; Transform[distanc_min]' is the second posture relationship of the second candidate environment frame with the smallest posture distance; .inverse() represents the matrix inversion operation; The pose relationship error of the second candidate environment frame with the second smallest pose distance is obtained by the following formula: Error[distance_second_min]=Transform[distance_second_min]×Trans form[distance_second_min]'.inverse() Error[distance_second_min] is the posture relationship error of the second candidate environment frame with the second smallest posture distance; Transform[distance_second_min] is the first posture relationship of the second candidate environment frame with the second smallest posture distance; Transform[distance_second_min]' is the second posture relationship of the second candidate environment frame with the second smallest posture distance.

9. The loop detection method of the environment frame according to any one of claims 1 to 8, characterized in that: Also includes: Obtaining a third posture relationship between the current environment frame and the previous key frame; wherein the first environment frame is a key frame; If the third posture relationship is greater than a preset key posture relationship threshold, determining the current environment frame as a key frame; The environment acquisition data and the position and posture data of the current environment frame are saved.

10. The loop detection method of the environment frame according to claim 9, characterized in that: The step of obtaining the third posture relationship between the current environment frame and the previous key frame includes: The third pose relationship between the current environment frame and the previous key frame is obtained through the following formula: T=pose×pose key .inverse() Among them, T is the third pose relationship between the current environment frame and the previous key frame; pose is the pose data of the current environment frame; pose key It is the pose data of the previous key frame; .inverse() represents the matrix inverse operation.

Images