Loopback detection method and device, vehicle, robot, storage medium and program product

Abstract

The invention discloses a loopback detection method and device, a vehicle, a robot, a storage medium and a program product. The method comprises the following steps: fusing point cloud information of a surrounding environment acquired by a laser odometer on a moving target, measurement information acquired by an inertial measurement unit and position information acquired by a satellite positioning sensor to obtain pose information of the moving target under global coordinates; performing nearest neighbor search on the pose information to determine candidate frame point cloud information forming a loop; and matching the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information, and determining loopback frame point cloud information. According to the method, information collected by three sensors is comprehensively utilized, accurate initial information and matching initial values can be provided for loopback detection, loopback frame point cloud information can be accurately detected through loopback detection, the detection effect is not affected by different scenes, and the optimal loopback detection effect can be obtained in various scenes.

Description

Technical Field

[0001] Embodiments of the present application relate to the field of computer vision technology, and in particular, to a loop detection method, device, vehicle, robot, storage medium, and program product. Background Art

[0002] Loop detection is an important component of the Simultaneous Localization and Mapping (SLAM) system. Loop detection can correct the accumulated error of the odometer in the SLAM system after a period of time and mileage. If the accumulated error of the odometer is not corrected, it will cause the sub-image constructed first and the sub-image constructed later to appear ghosting when passing through the same scene twice.

[0003] Loop detection is mainly based on the fact that a moving target usually goes around and returns to the starting point. In the process of building a global map within a certain range, in order to obtain a globally consistent map, it is necessary to match the local map currently scanned or photographed with the local map recorded in the history, and then adjust the relative position relationship between the current local map and the historical local map. This process can be used for loop detection to eliminate the accumulated error of the odometer.

[0004] Most existing loop detection methods use a single sensor to provide matching initial values ​​for loop detection, such as using a laser odometer. Among them, the motion trajectory of the laser odometer in the local coordinate system will have a large drift error over long distances. Even if a loop is found, it will give a poor initial value for loop matching. In the case of poor initial values, loop matching will fail and the number of valid loops detected will be greatly reduced. Summary of the invention

[0005] The embodiments of the present application provide a loop detection method, device, vehicle, robot, storage medium and program product, which can solve or partially solve the above-mentioned deficiencies in the prior art or other deficiencies in the prior art.

[0006] According to the first aspect of the present application, a loop detection method is provided, including: fusing point cloud information of the surrounding environment collected by a laser odometer on a moving target, measurement information collected by an inertial measurement unit, and position information collected by a satellite positioning sensor to obtain the posture information of the moving target in global coordinates; determining candidate frame point cloud information that forms a loop based on a nearest neighbor search of the posture information; and matching the candidate frame point cloud information based on the posture information corresponding to the candidate frame point cloud information to determine the loop frame point cloud information.

[0007] In one embodiment of the present application, the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are integrated to obtain the posture information of the moving target in the global coordinates, including: selecting the position information whose covariance is less than or equal to the preset covariance threshold from the position information collected by the satellite positioning sensor; adjusting the noise of the corresponding position information based on the covariance of the selected position information to optimize the selected position information; and fusing the optimized position information with the point cloud information collected by the laser odometer and the measurement information collected by the inertial measurement unit to obtain the posture information.

[0008] In one embodiment of the present application, the noise of the corresponding location information is adjusted based on the covariance of the selected location information to optimize the selected location information, including: determining the covariance weight of the corresponding location information based on the covariance of the selected location information and a preset noise function; determining the noise of the corresponding location information based on the covariance and covariance weight of the selected location information; and weighting the corresponding location information with an optimization weight determined based on the noise of the selected location information to obtain the optimized location information.

[0009] In one embodiment of the present application, the preset covariance threshold is 5.

[0010] In one embodiment of the present application, the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are fused to obtain the pose information of the moving target in the global coordinates, including: converting the pose data of the point cloud information collected by the laser odometer to the coordinate system of the position information collected by the satellite positioning sensor to obtain initial pose information; determining relative pose information based on the pose data of the point cloud information of adjacent frames collected by the laser odometer; and fusing the initial pose information and the relative pose information with the position information collected by the satellite positioning sensor and the measurement information collected by the inertial measurement unit to obtain the pose information.

[0011] In one embodiment of the present application, the pose data of the point cloud information collected by the laser odometer is converted to the coordinate system of the position information collected by the satellite positioning sensor to obtain initial pose information, including: determining a translation vector and a rotation matrix based on the position coordinate data of the point cloud information collected by the laser odometer and the position coordinate data of the position information collected by the satellite positioning sensor; constructing a transformation matrix based on the translation vector and the rotation matrix; and converting the pose data of the point cloud information to the coordinate system of the position information through the transformation matrix to obtain the initial pose information.

[0012] In one embodiment of the present application, the position coordinate data of the point cloud information collected by the laser odometer and the position coordinate data of the position information collected by the satellite positioning sensor are used to determine the translation vector and the rotation matrix, including: determining the initial position information collected by the satellite positioning sensor corresponding to the first frame of point cloud information collected by the laser odometer based on the timestamp, and determining the translation vector based on the position coordinate data of the first frame of point cloud information and the position coordinate data of the initial position information; determining the farthest position information collected by the satellite positioning sensor that is farthest from the initial position information based on the Euclidean distance, and determining the first plane vector based on the position coordinate data of the initial position information and the position coordinate data of the farthest position information; determining the farthest frame of point cloud information collected by the laser odometer corresponding to the farthest position information based on the timestamp, and determining the second plane vector based on the position coordinate data of the first frame of point cloud information and the position coordinate data of the farthest frame of point cloud information; and determining the rotation angle based on the first plane vector and the second plane vector, and determining the rotation matrix based on the rotation angle.

[0013] In one embodiment of the present application, the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor are integrated to obtain the posture information of the moving target in the global coordinate, including: accumulating the measurement information collected by the inertial measurement unit a preset number of times to form a group, wherein the preset number of times is determined based on the measurement frequency of the inertial measurement unit and the measurement frequency of the laser odometer; and fusing a group of measurement information with the point cloud information collected by the laser odometer and the position information collected by the satellite positioning sensor to obtain the posture information of the moving target in the global coordinate.

[0014] In one embodiment of the present application, the accumulation of the measurement information collected by the inertial measurement unit a preset number of times forms a group, including: accumulating the measurement information collected by the inertial measurement unit 10 times to form a group, wherein the measurement frequency of the inertial measurement unit is 100 Hz, and the measurement frequency of the laser odometer is 10 Hz.

[0015] In one embodiment of the present application, the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are fused to obtain the pose information of the moving target in the global coordinate, including: based on a preset global factor graph model, the point cloud information collected by the laser odometer, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are fused to obtain the pose information; wherein the measurement information collected by the inertial measurement unit is used as a pre-integration factor, the position information collected by the satellite positioning sensor is used as a unary factor, and the relative pose information determined by the adjacent frame point cloud information collected by the laser odometer is used as a binary factor; and the pose data of the point cloud information collected by the laser odometer is converted to the initial pose information obtained in the coordinate system of the position information collected by the satellite positioning sensor as a variable.

[0016] In one embodiment of the present application, the candidate frame point cloud information that forms a loop is determined based on a nearest neighbor search of the pose information, including: performing a nearest neighbor search on the pose information corresponding to the key frame point cloud information collected by the laser odometer to obtain search frame point cloud information and nearest neighbor frame point cloud information; determining the cumulative mileage between the nearest neighbor frame point cloud information and the search frame point cloud information, and using the search frame point cloud information and the nearest neighbor frame point cloud information whose cumulative mileage is greater than a preset mileage threshold as the candidate frame point cloud information that forms a loop.

[0017] In one embodiment of the present application, a nearest neighbor search is performed on the posture information corresponding to the key frame point cloud information collected by the laser odometer to obtain search frame point cloud information and nearest neighbor frame point cloud information, including: determining the search frame point cloud information at every preset distance based on the posture information corresponding to the key frame point cloud information; and performing a nearest neighbor search of a preset radius on the posture information corresponding to the search frame point cloud information, and taking other key frame point cloud information of the posture information within the preset radius as the nearest neighbor frame point cloud information.

[0018] In one embodiment of the present application, the candidate frame point cloud information is matched based on the pose information corresponding to the candidate frame point cloud information to determine the loop frame point cloud information, including: determining the relative pose information of the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information; matching the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information, and using the candidate frame point cloud information whose matching result meets a preset threshold as the loop frame point cloud information.

[0019] In one embodiment of the present application, the candidate frame point cloud information is matched based on the relative pose information of the candidate frame point cloud information, and the candidate frame point cloud information whose matching result meets a preset threshold is used as the loop frame point cloud information, including: extracting line and surface features of the candidate frame point cloud information; matching the line and surface features of the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information to obtain a matching score, and the candidate frame point cloud information whose matching score meets a preset score threshold is used as the loop frame point cloud information.

[0020] In one embodiment of the present application, the matching of the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information to determine the loop frame point cloud information also includes: splicing the candidate frame point cloud information with the point cloud information of a preset number of frames before and / or after it to obtain a candidate sub-image; the matching of the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information, and using the candidate frame point cloud information whose matching result meets a preset threshold as the loop frame point cloud information, includes: matching the candidate sub-image based on the relative pose information of the candidate frame point cloud information, and using the candidate sub-image whose matching result meets a preset threshold as the loop sub-image.

[0021] According to the second aspect of the present application, a loop detection device is provided, including: an information fusion module, configured to fuse point cloud information of the surrounding environment collected by a laser odometer on a moving target, measurement information collected by an inertial measurement unit, and position information collected by a satellite positioning sensor to obtain the posture information of the moving target in global coordinates; a loop search module, configured to determine candidate frame point cloud information that forms a loop based on a nearest neighbor search for the posture information; and a loop matching module, configured to match the candidate frame point cloud information based on the posture information corresponding to the candidate frame point cloud information to determine the loop frame point cloud information.

[0022] The autonomous driving vehicle provided according to the third aspect of the present application may include: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the loop detection method described in the first aspect of the present application.

[0023] The mobile robot provided according to the fourth aspect of the present application may include: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the loop detection method described in the first aspect of the present application.

[0024] The computer-readable storage medium provided according to the fifth aspect of the present application includes a computer program, and when the computer program is executed by a processor, the loop detection method described in the first aspect of the present application is implemented.

[0025] The computer program product provided according to the sixth aspect of the present application stores a computer program, and when the computer program is executed by a processor, the loop detection method described in the first aspect of the present application is implemented.

[0026] According to the loop detection method, device, vehicle, robot, storage medium and program product provided in the implementation manner of the present application, full use is made of the existing sensors on the moving target, and by fusing the information collected by the three sensors, namely the laser odometer, the inertial measurement unit and the satellite positioning sensor, the accurate position and posture information of the moving target in the global coordinate system can be obtained. Accurate initial information and matching initial values ​​are provided for searching and matching candidate frame point cloud information for loop detection, and loop frame point cloud information can be accurately detected. The effect of loop detection will not be affected by different scenarios, and the best loop detection effect can be obtained in various scenarios.

[0027] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present application, nor is it intended to limit the scope of the present application. Other features of the present application will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Other features, purposes and advantages of the present application will become more apparent by reading the detailed description of the non-limiting embodiments made with reference to the following drawings. The drawings are used to better understand the present solution and do not constitute a limitation of the present application. Among them:

[0029] Figure 1 is a flow chart of a loop detection method according to an embodiment of the present application;

[0030] Figure 2 is a flow chart for fusing sensor information according to an embodiment of the present application;

[0031] Figure 3 is a flow chart of performing noise adjustment on position information according to an embodiment of the present application;

[0032] Figure 4 and Figure 5 This is a schematic diagram of two frames of point clouds obtained when a vehicle travels back and forth under a viaduct;

[0033] Figure 6 is a flow chart of fusing sensor information according to another embodiment of the present application;

[0034] Figure 7 is a flow chart of coordinate conversion of the position and posture data of a laser odometer according to an embodiment of the present application;

[0035] Figure 8 is a flow chart of obtaining a translation vector and a rotation matrix for coordinate transformation according to an embodiment of the present application;

[0036] Fig. 9 is a flow chart for fusing sensor information according to another embodiment of the present application;

[0037] Fig.10 is a schematic diagram of a global factor graph model for information fusion according to an embodiment of the present application;

[0038] Fig.11 is a flow chart of performing loop closure search through posture information according to an embodiment of the present application;

[0039] Fig.12 is a flowchart of performing a nearest neighbor search on posture information according to an embodiment of the present application;

[0040] Fig.13 is a flow chart of performing loop closure matching through posture information according to an embodiment of the present application;

[0041] Fig.14 is a flow chart of performing feature matching on point cloud information according to an implementation method of the present application;

[0042] Fig.15 is a flow chart of performing loop closure matching through posture information according to another embodiment of the present application;

[0043] Fig.16 It is a comparison diagram of the original trajectory provided by the laser odometer and the trajectory after eliminating the accumulated error of the odometer;

[0044] Fig.17is a block diagram of a loop detection device according to an embodiment of the present application;

[0045] Fig.18 It is a block diagram of an autonomous driving vehicle or mobile robot used to implement the loop detection method of the implementation mode of the present application. DETAILED DESCRIPTION

[0046] The following is a description of exemplary embodiments of the present application in conjunction with the accompanying drawings, including various details of the embodiments of the present application to facilitate understanding, which should be considered as merely exemplary. Therefore, it should be recognized by those of ordinary skill in the art that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the present application. Similarly, for the sake of clarity and conciseness, the description of well-known functions and structures is omitted in the following description.

[0047] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0048] An embodiment of the present application provides a loop detection method 1000.

[0049] Figure 1 The flowchart of the loop detection method 1000 according to the embodiment of the present application is shown. The loop detection method 1000 of the embodiment of the present application can be executed by a loop detection device, which can be hardware or software, or both hardware and software. The embodiment of the present application does not limit this. The loop detection device can be set on a moving target. The embodiment of the present application does not limit the type of moving target. For example, the moving target can be an autonomous driving vehicle or a mobile robot. Figure 1 As shown, the loop detection method 1000 may include the following steps:

[0050] S100, fusing the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor to obtain the position and posture information of the moving target in the global coordinates.

[0051] S200, determining candidate frame point cloud information that forms a loop based on a nearest neighbor search of the pose information.

[0052] S300, matching the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information to determine the loop frame point cloud information.

[0053] The laser odometer, inertial measurement unit (IMU) and satellite positioning sensor of the implementation mode of the present application can be existing sensors on the moving target. Among them, the laser odometer is used to collect point cloud information of the surrounding environment of the moving target, and the position data of the point cloud information can constitute the position trajectory of the moving target. IMU is used to collect acceleration information and angular velocity information of the moving target. The satellite positioning sensor is used to collect the position information of the moving target in the global coordinate system. For example, the satellite positioning sensor can use real-time kinematic (RTK) positioning, and the position information collected by the satellite positioning sensor can be the position coordinates in the universal transverse mercator grid system (UTM) coordinate system.

[0054] The fusion of laser odometer, IMU and satellite positioning sensor information in this application can be regarded as an optimization process for the pose trajectory of the moving target in the global coordinate system. Among them, the pose trajectory in the local coordinate system of the laser odometer can be converted to the global coordinate system of the satellite positioning sensor as the initial value for the optimization of the pose trajectory. The relative pose information between adjacent frame point cloud information collected by laser mileage can be used to provide relative pose constraints between adjacent trajectory points for the optimization of the pose trajectory, for example, it can include constraints of 6 degrees of freedom in movement and rotation. The position information collected by the satellite positioning sensor can be used to provide unilateral prior position constraints of the absolute position in the global coordinate system for the optimization of the pose trajectory, for example, it can only include constraints of 3 degrees of freedom in movement. The acceleration information and angular velocity information collected by the IMU can be used to provide relative pose constraints between adjacent trajectory points for the optimization of the pose trajectory, for example, it can include constraints of 6 degrees of freedom in movement and rotation. The initial value of the pose trajectory optimization can be optimized through the constraints provided by the laser odometer, satellite positioning sensor and IMU to obtain the accurate pose trajectory of the moving target in the global coordinate system. The pose information of the trajectory points in the optimized pose trajectory is the pose information of the moving target in the global coordinate system.

[0055] The execution subject of the method obtains the optimized posture trajectory by fusing the information of the laser odometer, IMU and satellite positioning sensor, and then the optimized posture trajectory can be used as the reference trajectory for loop detection, thereby ensuring the accuracy of loop detection. Among them, the trajectory points that may form a loop in the optimized posture trajectory can be obtained by performing a nearest neighbor search on the posture information of the trajectory points in the optimized posture trajectory, and the point cloud information collected at the trajectory points that may form a loop is used as the candidate frame point cloud information. The candidate frame point cloud information can be matched according to the posture information of the trajectory points that may form a loop in the optimized posture trajectory, and it is determined according to the matching result whether the trajectory point corresponding to the candidate frame point cloud information is the trajectory point that forms a loop in the optimized posture trajectory, and the point cloud information collected at the trajectory point that forms a loop is determined as the loop frame point cloud information.

[0056] From the above, it can be seen that the implementation method of the present application makes full use of the existing sensors on the moving target. By fusing the information collected by the laser odometer, inertial measurement unit and satellite positioning sensor, and comprehensively utilizing the information collected by the three sensors, the accurate position information of the moving target in the global coordinate system can be obtained, and accurate initial information and matching initial values ​​can be provided for the search and matching of candidate frame point cloud information for loop detection. The loop frame point cloud information can be accurately detected, and the effect of the loop detection will not be affected by different scenes. The best loop detection effect can be obtained in various scenes.

[0057] It should be understood that the steps shown in method 1000 are not exclusive, and other steps may be performed before, after, or between any of the steps shown. In addition, some of the steps shown may be performed simultaneously, or may be performed in different steps. Figure 1 Executed in the order shown.

[0058] The following is a detailed description of each step of the loop detection method 1000 in the implementation manner of the present application.

[0059] Figure 2 FIG. 1 shows a flow chart of fusing sensor information according to an embodiment of the present application. Figure 2 As shown, step S100 fuses the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor to obtain the position and posture information of the moving target in the global coordinates, which may include the following steps:

[0060] S110 , selecting position information whose covariance is less than or equal to a preset covariance threshold from the position information collected by the satellite positioning sensor.

[0061] S120 adjusts the noise of the corresponding position information based on the covariance of the selected position information to optimize the selected position information.

[0062] S130, fusing the optimized position information with the point cloud information collected by the laser odometer and the measurement information collected by the inertial measurement unit to obtain the position and posture information of the moving target in the global coordinates.

[0063] When the satellite positioning sensor outputs the latitude, longitude and altitude information, it also outputs the covariance of the latitude and altitude. In open scenes, the covariance of the latitude and altitude is usually 0.02, 0.02, and 0.03, and the confidence of the latitude and altitude information is high. In urban scenes with trees, buildings, and viaducts that sometimes block the view, the covariance of the latitude and altitude may not reach 0.02, 0.02, and 0.03, and the confidence of the latitude and altitude information is poor. For example, the information with the covariance of the latitude and altitude of 5 has a local jump in the elevation, and the horizontal position is relatively accurate. Usually, when using the location information collected by the satellite positioning sensor, most of the location information with a higher confidence is used. For example, by setting the covariance threshold of the latitude and altitude to 0.02, 0.02, and 0.03, the location information collected by the satellite positioning sensor is screened, and only the latitude and altitude information with the covariance of the latitude and altitude less than or equal to 0.02, 0.02, and 0.03 will be used, such as the information with the covariance of the latitude and altitude of 0.01.

[0064] This can ensure that the location information of the satellite positioning sensor used is completely accurate and does not introduce erroneous jump information. However, in urban scenes that are sometimes blocked by trees, buildings, and viaducts, it is difficult for the longitude and latitude covariance to maintain a high confidence level for a long time. If the location information of the satellite positioning sensor is filtered according to the longitude and latitude covariance thresholds of 0.02, 0.02, and 0.03, there may be no available information for a long time. If the drift of the laser odometer caused by long-term and long-distance is corrected using the location information collected by the satellite positioning sensor, the drift of the laser odometer will not be corrected for a long time.

[0065] The implementation mode of the present application recalls the location information of as much available satellite positioning sensors as possible by presetting a suitable covariance threshold, and adjusts the noise of the location information according to the covariance of the location information, so that the location information with a larger covariance has a larger noise, thereby reducing the role of the location information, and the location information with a smaller covariance has a smaller noise, thereby improving the role of the location information, so that even location information of slightly poor quality can serve as an anchor point without causing trouble, and strives to effectively use the location information of as much satellite positioning sensors as possible in urban scenes where semi-occlusion or long-term occlusion is often encountered, as a unilateral prior position constraint of the absolute position in the global coordinate system, so as to be well adapted to the loop detection of urban scenes.

[0066] Since the signals with elevation covariance greater than 5 in the position signals collected by the satellite positioning sensor have very large jumps in elevation and horizontal position, while the signals with elevation covariance less than or equal to 5 mostly have local jumps in elevation and relatively accurate horizontal position, we can preset the covariance threshold to 5, filter the position signals collected by the satellite positioning sensor, take out the signals with elevation covariance less than or equal to 5, and perform noise adjustment and optimization before fusing them with the point cloud information collected by the laser odometer and the measurement information collected by the IMU.

[0067] It should be noted that the present application does not limit the value of the preset covariance threshold. For different accuracy requirements and different application scenarios, the preset covariance threshold of the present application can also take other values ​​other than 5, for example, the preset covariance threshold can be 4, 1 or 0.5.

[0068] In some optional implementations, adjusting the noise of the position information according to the covariance of the position information may be to assign different noises to the position information according to the covariance of the position information, thereby assigning different optimization weights to the position information according to the noise of the position information, and fusing the weighted position information with the point cloud information collected by the laser odometer and the measurement information collected by the IMU. By assigning different optimization weights to the position information, the position information can be effectively utilized. Figure 3 As shown, step S120 adjusts the noise of the corresponding location information based on the covariance of the selected location information to optimize the selected location information, which may include: S121 determines the covariance weight of the corresponding location information based on the covariance of the selected location information and a preset noise function; S122 determines the noise of the corresponding location information based on the covariance and the covariance weight of the selected location information; and S123 determines the optimization weight based on the noise of the selected location information to weight the corresponding location information to obtain the optimized location information.

[0069] In some optional examples, the preset noise function may be a linear function with a gradient parameter K=0.02, and the covariance weights x_std_weight, y_std_weight, and z_std_weight of the position information whose covariances are x_std, y_std, and z_std may be set by the preset noise function:

[0070] x_std_weight=(x_std / K),

[0071] y_std_weight=(y_std / K),

[0072] z_std_weight = (z_std / K);

[0073] Then, according to the covariance weights x_std_weight, y_std_weight, z_std_weight and the covariances x_std, y_std, z_std, the noise values ​​of x, y, and z in the position information are assigned noise_x, noise_y, and noise_z, so that the noise with large covariance is larger and the noise with small covariance is smaller:

[0074] noise_x=x_std_weight*x_std,

[0075] noise_y=y_std_weight*y_std,

[0076] noise_z=z_std_weight*z_std;

[0077] Afterwards, the optimization weights of x, y, and z in the location information are assigned according to the noise noise_x, noise_y, and noise_z of the location information, and the x, y, and z in the location information are weighted by the optimization weights to achieve the optimization of the location information. The greater the noise, the smaller the optimization weight, and the smaller the noise, the greater the optimization weight.

[0078] It should be noted that, in addition to assigning different noises to the position information, the present application may also use other methods to adjust the noise of the position information, and the present application does not limit this. When assigning different noises to the position information according to the preset noise function, the present application does not limit the type of the preset noise function and the value of the gradient parameter K when the preset noise function is a linear function. For example, for situations where higher accuracy is required, the gradient parameter K may also be 1 or 2, etc.

[0079] like Figure 4 As shown in the figure, red and green represent two frames of point cloud collected by the laser odometer when the vehicle drove back and forth under the viaduct. Due to the cumulative error of the laser odometer, the vehicle drove back and forth twice through the viaduct, and the point cloud collected by the laser odometer showed large stratification in the horizontal and vertical directions of the viaduct columns. That is, after the two frames of point cloud were matched based on the pose information obtained by the laser odometer, for the same viaduct column in reality, ghosting appeared in the matched sub-image. Usually, the satellite signal is generally poor under the viaduct, and there is a great risk of Z value jump. Whether the information collected by the laser odometer or the satellite positioning sensor is used alone as the initial value of the loop matching, the result will be inaccurate.

[0080] The method of the present application optimizes the vehicle's posture trajectory by fusing the information collected by the laser odometer, IMU and satellite positioning sensor, and performs loop search and matching on the optimized posture trajectory. The obtained sub-graph is a matching sub-graph of two frames of point clouds collected by the laser odometer of the vehicle traveling back and forth on both sides of the viaduct, as shown in FIG. Figure 5 As shown in the figure, the red and white colors represent two frames of point clouds collected by the laser odometer when the vehicle is driving back and forth under the viaduct. Figure 5 In the proposed method, the accumulated error of the laser odometer is corrected by the constraints of the IMU and satellite positioning sensors.

[0081] Especially for long-distance viaducts, by setting a suitable covariance threshold for the position information collected by the satellite positioning sensor and adjusting the noise of the position information, as much position information collected by the satellite positioning sensor as possible can be used to correct the cumulative error generated by the laser odometer, which will not result in a long period of time without available position information collected by the satellite positioning sensor, causing the drift of the laser odometer to remain uncorrected for a long time.

[0082] Figure 6 FIG. 4 shows a flow chart of fusing sensor information according to another embodiment of the present application. Figure 6 As shown, step S100 fuses the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor to obtain the position and posture information of the moving target in the global coordinates, which may include the following steps:

[0083] S140, converting the position and posture data of the point cloud information collected by the laser odometer into the coordinate system of the position information collected by the satellite positioning sensor to obtain initial position and posture information.

[0084] S150, determining relative pose information based on the pose data of adjacent frame point cloud information collected by the laser odometer.

[0085] S160, fusing the initial posture information and the relative posture information with the position information collected by the satellite positioning sensor and the measurement information collected by the inertial measurement unit to obtain the posture information of the moving target in the global coordinates.

[0086] In this application, the laser odometer can provide two kinds of information for information fusion, one is the initial pose information, and the other is the relative pose information. The initial pose information is used as the initial value for pose trajectory optimization, and the relative pose information is used as the relative pose constraint between adjacent trajectory points. Since the position information in the global coordinate system collected by the satellite positioning sensor generally has a numerical value in the order of tens of thousands or hundreds of thousands, and the pose information in the local coordinate system provided by the laser odometer is generally in the order of thousands, if the pose information in the local coordinate system of the laser odometer is directly used as the initial value to fuse with the position information in the global coordinate system of the satellite positioning sensor, information distortion may occur in some positions, or a local optimum may be obtained, but it is difficult to achieve a global optimum, resulting in poor fusion results. The information collected by the satellite positioning sensor only includes position information, but does not contain pose information, and cannot be used as the initial value for pose trajectory optimization.

[0087] Since the accuracy of the pose trajectory optimization result in the global coordinate system is strongly related to the optimization initial value, before the pose information in the local coordinate system of the laser odometer is used as the initial value to fuse with the position information in the global coordinate system of the satellite positioning sensor, the application first aligns the local trajectory head and tail vectors provided by the laser odometer and the global trajectory head and tail vectors provided by the satellite positioning sensor, and converts the pose information in the local coordinate system of the laser odometer to the global coordinate system of the satellite positioning sensor as the initial value for pose trajectory optimization.

[0088] In some optional embodiments, such as Figure 7 As shown, step S140 converts the pose data of the point cloud information collected by the laser odometer into the coordinate system of the position information collected by the satellite positioning sensor, and obtaining the initial pose information may include: S141 determining the translation vector and the rotation matrix based on the position coordinate data of the point cloud information collected by the laser odometer and the position coordinate data of the position information collected by the satellite positioning sensor; S142 constructing a transformation matrix based on the translation vector and the rotation matrix; and S143 converting the pose data of the point cloud information into the coordinate system of the position information through the transformation matrix to obtain the initial pose information.

[0089] Alternatively, if Figure 8As shown, step S141 determines the translation vector and the rotation matrix based on the position coordinate data of the point cloud information collected by the laser odometer and the position coordinate data of the position information collected by the satellite positioning sensor, and may include: S1411 determines the initial position information collected by the satellite positioning sensor corresponding to the first frame of point cloud information collected by the laser odometer based on the timestamp, and determines the translation vector based on the position coordinate data of the first frame of point cloud information and the position coordinate data of the initial position information; S1412 determines the farthest position information collected by the satellite positioning sensor farthest from the initial position information based on the Euclidean distance, and determines the first plane vector based on the position coordinate data of the initial position information and the position coordinate data of the farthest position information; S1413 determines the farthest frame of point cloud information collected by the laser odometer corresponding to the farthest position information based on the timestamp, and determines the second plane vector based on the position coordinate data of the first frame of point cloud information and the position coordinate data of the farthest frame of point cloud information; and S1414 determines the rotation angle based on the first plane vector and the second plane vector, and determines the rotation matrix based on the rotation angle.

[0090] In some optional examples, for step S1411, the position coordinate data odom_pos_init of the first frame point cloud of the laser odometer can be searched, and the position coordinate data rtk_pos_init of a frame of the satellite positioning sensor closest to it in time can be found through the timestamp of the first frame point cloud. After finding the first frame of the laser odometer and the first frame of the satellite positioning sensor, the translation vector of odom_pos_init aligned with the initial position of rtk_pos_init is calculated, which is recorded as odom_to_rtk_trans_pose=((rtk_pos_init.x-odom_pos_init.x),(rtk_pos_init.y-odom_pos_init.y),(rtk_pos_init.z-odom_pos_init.z));

[0091] For step S1412, according to the first frame position coordinate data rtk_pos_init of the satellite positioning sensor found, that is, the initial position of the moving target in the global coordinate system, start traversing other position coordinate data after the satellite positioning sensor, and record the Euclidean geometric distance between the position corresponding to each timestamp and the initial position; select the position coordinate data rtk_pos_end of the position farthest from the initial position, and form a plane vector rtk_vec with the position coordinate data rtk_pos_init of the initial position, where rtk_vec=((rtk_pos_end.x-rtk_pos_init.x),(rtk_pos_end.y-rtk_pos_init.y),0.0);

[0092] For step S1413, the position coordinate data odom_pos_end of a frame point cloud of the laser odometer closest in time is found from the timestamp of the farthest position, and the plane vector odom_vec is formed with the position coordinate data odom_pos_init of the first frame point cloud, where odom_vec = ((odom_pos_end.x-odom_pos_init.x), (odom_pos_end.y-odom_pos_init.y), 0.0);

[0093] For step S1414, the angle θ of the plane vector odom_vec relative to the plane vector rtk_vec around the Z axis is calculated, and the angle θ is calculated by the formula The rotation angle θ, since the calculation is performed on the plane around the Z axis, the resulting rotation matrix

[0094] For step S142, the translation vector odom_to_rtk_trans_pose and the rotation matrix odom_to_rtk_rot are combined into a transformation matrix The pose data of each frame of point cloud collected by the laser odometer is converted to the global coordinate system of the satellite positioning sensor through the transformation matrix as the initial value for pose trajectory optimization.

[0095] It should be noted that the present application does not limit the implementation method of obtaining the transformation matrix, translation vector, and rotation matrix. For example, the transformation matrix, translation vector, and rotation matrix can be constructed using only the key frame point cloud information in the laser odometer. Among them, the key frame point cloud information can refer to the point cloud information collected when the displacement and rotation exceed the preset threshold.

[0096] Fig. 9 FIG. 4 shows a flow chart of fusing sensor information according to another embodiment of the present application. Fig. 9 As shown, step S100 fuses the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor to obtain the position and posture information of the moving target in the global coordinates, which may include the following steps:

[0097] S170, accumulating measurement information collected by the inertial measurement unit for a preset number of times to form a group, wherein the preset number of times is determined based on a measurement frequency of the inertial measurement unit and a measurement frequency of the laser odometer.

[0098] S180, fusing a set of measurement information with the point cloud information collected by the laser odometer and the position information collected by the satellite positioning sensor to obtain the position and posture information of the moving target in the global coordinates.

[0099] Usually, the measurement frequency of the IMU configured on the moving target is mostly higher than the measurement frequency of the laser odometer. For example, the measurement frequency of the IMU configured on the autonomous driving vehicle is 100Hz, and the measurement frequency of the laser odometer is 10Hz. The number of times the IMU collects measurement information can be preset according to the measurement frequency of the IMU and the measurement frequency of the laser odometer for accumulation, and after forming a group, it is fused with the point cloud information collected by the laser odometer. For example, the measurement information collected by the IMU 10 times can be accumulated to form a group, and the measurement information collected by the IMU 10 times can be combined together for processing. By making the IMU perform information fusion according to the frequency of the laser odometer, the position and attitude increments of two adjacent frames of point clouds can be obtained as the relative position and attitude constraints between adjacent trajectory points. Instead of the IMU collecting measurement information once and performing a calculation to obtain the position and attitude increments at a certain moment, for example, the measurement frequency of the IMU is 100Hz, and the measurement information is calculated once in 0.01s when performing information fusion, which will greatly waste computing resources.

[0100] In some optional embodiments, step S100 fuses the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor to obtain the pose information of the moving target in the global coordinates. Based on a preset global factor graph model, the point cloud information collected by the laser odometer, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor are fused to obtain the pose information. Among them, the measurement information collected by the inertial measurement unit can be used as a pre-integration factor. The position information collected by the satellite positioning sensor can be used as a unary factor. The relative pose information determined by the adjacent frame point cloud information collected by the laser odometer can be used as a binary factor. The pose data of the point cloud information collected by the laser odometer can be converted to the initial pose information obtained in the coordinate system of the position information collected by the satellite positioning sensor as a variable.

[0101] Fig.10 FIG. 1 shows a schematic diagram of a global factor graph model for information fusion according to an embodiment of the present application. Fig.10As shown, the GTSAM optimization library can be used to construct a global factor graph model, so as to utilize the existing IMU pre-integration module (Pre-integration) in GTSAM to construct a global factor graph model. Among them, the relative pose information determined by the adjacent frame point cloud information collected by the laser odometer can be added to the global factor graph model as a Between Factor. The position information collected by the satellite positioning sensor can be added to the global factor graph model as a GPS Factor. The measurement information collected by the IMU can be added to the global factor graph model as an IMUFactor and an IMU-bias Between Factor. The initial pose information obtained by converting the pose data of the point cloud information collected by the laser odometer to the coordinate system of the position information collected by the satellite positioning sensor can be added to the global factor graph model as X(n). After fusing the information collected by the three sensors, the global factor graph model can obtain the pose information of the moving target in the global coordinate system, that is, Fig.10 The optimization variable X(n) in .

[0102] It should be noted that this application integrates the information collected by the laser odometer, IMU, and satellite positioning sensor, and can be used simultaneously Figure 2 , Figure 6 , Fig. 9 and Fig.10 The embodiment shown can also be used only Figure 2 , Figure 6 , Fig. 9 and Fig.10 One of the embodiments shown can also be used Figure 2 , Figure 6 , Fig. 9 and Fig.10 The embodiments of the present application are not limited to any combination of the embodiments shown.

[0103] Fig.11 FIG. 4 shows a flow chart of loop closure search based on posture information according to an embodiment of the present application. Fig.11 As shown, step S200 may include the following steps:

[0104] S210, performing a nearest neighbor search on the pose information corresponding to the key frame point cloud information collected by the laser odometer to obtain the search frame point cloud information and the nearest neighbor frame point cloud information.

[0105] S220, determining the cumulative mileage between the nearest neighbor frame point cloud information and the search frame point cloud information, and taking the search frame point cloud information and the nearest neighbor frame point cloud information whose cumulative mileage is greater than a preset mileage threshold as candidate frame point cloud information for forming a loop.

[0106] In some optional embodiments, such as Fig.12 As shown, S210 performs a nearest neighbor search on the pose information corresponding to the key frame point cloud information collected by the laser odometer, and obtaining the search frame point cloud information and the nearest neighbor frame point cloud information may include: S211 determines the search frame point cloud information at every preset distance based on the pose information corresponding to the key frame point cloud information; and S212 performs a nearest neighbor search of a preset radius on the pose information corresponding to the search frame point cloud information, and uses other key frame point cloud information whose pose information is within the preset radius as the nearest neighbor frame point cloud information.

[0107] In some optional examples, the nearest neighbor search can use a kdtree nearest neighbor search. According to the pose information of the moving target in the global coordinate system corresponding to the key frame point cloud collected by the laser odometer, the search frame point cloud can be selected from the key frame point cloud collected by the laser odometer at intervals of 20m, and the pose information of the moving target in the global coordinate system corresponding to the search frame point cloud can be used to perform a kdtree nearest neighbor search within a certain radius R. For the search frame point cloud and the nearest neighbor frame point cloud obtained by the kdtree nearest neighbor search, the cumulative mileage between them needs to be calculated to avoid using the key frame point cloud near the search frame point cloud as the nearest neighbor frame point cloud, where the key frame point cloud near the search frame point cloud refers to the key frame point cloud that is close to the search frame point cloud in both position and time. This situation does not meet the requirements of loop detection. The essence of loop detection is to find the situation where the beginning and the end return to the same scene in a long-distance trajectory.

[0108] It should be noted that in addition to using kdtree for nearest neighbor search, the present application may also use other existing nearest neighbor search methods, and the implementation of the present application does not limit this. Among them, the value of the preset distance between the search frame point cloud information, the value of the preset radius of the nearest neighbor search, and the value of the preset mileage threshold of the accumulated mileage can be flexibly adjusted according to the application scenario, accuracy requirements, etc., and the implementation of the present application does not limit this. For example, the preset mileage threshold can be 3R, where R is the preset radius of the nearest neighbor search.

[0109] Fig.13 FIG. 1 shows a flow chart of loop closure matching through posture information according to an embodiment of the present application. Fig.13 As shown, step S300 matches the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information, and determining the loop frame point cloud information may include the following steps:

[0110] S310, determining relative pose information of the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information.

[0111] S320, matching the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information, and taking the candidate frame point cloud information whose matching result meets a preset threshold as the loop frame point cloud information.

[0112] In this embodiment, the relative pose information of the candidate frame point cloud information obtained from the pose information of the moving target in the global coordinate system is used as the initial value of the candidate frame point cloud information matching, which can ensure the accuracy of the candidate frame point cloud information matching and provide a basis for screening out higher quality loop frame point cloud information. Fig.14 As shown, S320 matches the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information, and uses the candidate frame point cloud information whose matching result meets a preset threshold as the loop frame point cloud information, which may include: S321 extracting the line and surface features of the candidate frame point cloud information; and S322 matching the line and surface features of the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information to obtain a matching score, and using the candidate frame point cloud information whose matching score meets a preset score threshold as the loop frame point cloud information.

[0113] In some optional examples, the line and surface features in the candidate frame point cloud can be extracted based on the GICP feature matching algorithm, and the matching of the candidate frame point cloud can be achieved based on the matching of the line and surface features in the point cloud. Compared with the ICP feature matching algorithm, the GICP feature matching algorithm has higher accuracy and speed. In the GICP feature matching algorithm, the matching score is obtained by the ratio of valid matching points. The fewer the valid matching points, the higher the score will be. Conversely, the more valid matching points, the lower the score will be. The matching results are quantitatively reflected by the matching scores of the candidate frame point clouds, and higher quality loop frame point clouds can be screened out to provide better loop detection results for downstream modules that use loop detection results.

[0114] Fig.15 FIG. 2 shows a flow chart of loop closure matching through posture information according to another embodiment of the present application. Fig.15 As shown, step S300 matches the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information, and determining the loop frame point cloud information may include the following steps:

[0115] S310, determining relative pose information of the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information.

[0116] S330 , concatenate the point cloud information of the candidate frame with the point cloud information of a preset number of frames before and / or after it to obtain a candidate sub-image.

[0117] S340, matching the candidate sub-graphs based on the relative pose information of the candidate frame point cloud information, and taking the candidate sub-graphs whose matching results meet a preset threshold as loop sub-graphs.

[0118] In order to obtain a larger common view area and improve the accuracy of the matching results, the present application can splice the candidate frame point cloud information with the point cloud information of the preset number of frames in front and / or behind it, and match the sub-images of the candidate frame. Compared with the matching between single-frame point cloud information and single-frame point cloud information, the matching between sub-images can obtain a larger common view area and more dense feature points, which can greatly improve the accuracy of loop detection, and is particularly suitable for loop detection in urban scenes with obstructed viaducts and wide roads. For example, the round-trip travel of a vehicle is in two lanes far apart in a four-lane road. During the round-trip process of the vehicle, the single-frame point cloud information collected by the laser odometer may not have a common view area or a small common view area. Matching with single-frame point cloud information may misjudge the loop frame point cloud information as non-loop frame point cloud information. For example, 5 frames of point cloud information can be taken in front and behind the candidate frame point cloud, and the adjacent 11 frames of point cloud information can be spliced ​​into sub-images for matching to obtain the matching relative posture information and matching result scores.

[0119] It should be noted that the numerical value of the preset number of frames of point cloud information selected before and / or after the candidate frame point cloud information to be spliced ​​into a sub-image can be determined according to the application scenario, accuracy requirements, etc., and the implementation methods of the present application are not limited to this. In addition to performing feature matching through the GICP feature matching algorithm, the present application can also perform feature matching through other existing feature matching algorithms, and the implementation methods of the present application are not limited to this. In addition to judging the matching results through matching scores, the present application can also judge the matching results through other existing matching result judgment methods, and the implementation methods of the present application are not limited to this.

[0120] Through a large amount of data verification, compared with using a single sensor, such as a laser odometer or a satellite positioning sensor alone, providing matching initial values ​​for loop detection will affect the quality of loop detection due to severe local drift of the laser odometer or occlusion of the satellite positioning sensor, such as Fig.16 As shown, Fig.16 It is a comparison diagram of the original trajectory provided by the laser odometer and the trajectory after eliminating the accumulated error of the odometer. Among them, the green trajectory is the trajectory of the vehicle posture trajectory provided by the laser odometer in the global coordinate system. Its Z value direction drift is serious, which will affect the accuracy of loop detection. The red trajectory is obtained by integrating the information collected by the laser odometer, IMU and satellite positioning sensor in the application to obtain the trajectory of the vehicle in the global coordinate system. It corrects the drift in the Z value direction of the laser odometer, and uses the red trajectory for loop search and matching. The result of the loop detection is more accurate.

[0121] The present application also provides a loop detection device 2000. Fig.17FIG. 2 is a block diagram of a loop detection device 2000 according to an embodiment of the present application. The loop detection device 2000 according to an embodiment of the present application can execute the loop detection method 1000 described above. Fig.17 As shown, the loop detection device 2000 may include:

[0122] The information fusion module 400 is configured to fuse the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor to obtain the position information of the moving target in the global coordinates.

[0123] The loop search module 500 is configured to determine candidate frame point cloud information that forms a loop based on a nearest neighbor search of the pose information.

[0124] The loop closure matching module 600 is configured to match the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information to determine the loop closure frame point cloud information.

[0125] In some optional implementations, the information fusion module 400 may include:

[0126] The information selection submodule is configured to select position information whose covariance is less than or equal to a preset covariance threshold from the position information collected by the satellite positioning sensor.

[0127] The noise adjustment submodule is configured to adjust the noise of the corresponding position information based on the covariance of the selected position information to optimize the selected position information.

[0128] The information fusion submodule is configured to fuse the optimized position information with the point cloud information collected by the laser odometer and the measurement information collected by the inertial measurement unit to obtain the posture information.

[0129] In some optional implementations, the noise adjustment submodule is further configured to:

[0130] Determine a covariance weight of the corresponding position information based on the covariance of the selected position information and a preset noise function;

[0131] Determining the noise of the corresponding position information based on the covariance and covariance weight of the selected position information; and

[0132] An optimization weight is determined based on the noise of the selected position information to weight the corresponding position information to obtain optimized position information.

[0133] In some optional implementations, the preset covariance threshold is 5.

[0134] In some optional implementations, the information fusion module 400 may include:

[0135] The coordinate conversion submodule is configured to convert the pose data of the point cloud information collected by the laser odometer into the coordinate system of the position information collected by the satellite positioning sensor to obtain initial pose information.

[0136] The constraint calculation submodule is configured to determine relative posture information based on the posture data of adjacent frame point cloud information collected by the laser odometer.

[0137] The information fusion submodule is configured to fuse the initial posture information and the relative posture information with the position information collected by the satellite positioning sensor and the measurement information collected by the inertial measurement unit to obtain the posture information.

[0138] In some optional implementations, the coordinate conversion submodule is further configured as follows:

[0139] Determine the translation vector and the rotation matrix based on the position coordinate data of the point cloud information collected by the laser odometer and the position coordinate data of the position information collected by the satellite positioning sensor;

[0140] constructing a transformation matrix based on the translation vector and the rotation matrix; and

[0141] The pose data of the point cloud information is converted into the coordinate system of the position information through the transformation matrix to obtain the initial pose information.

[0142] In some optional implementations, the coordinate conversion submodule is further configured as follows:

[0143] Based on the timestamp, determine the initial position information collected by the satellite positioning sensor corresponding to the first frame of point cloud information collected by the laser odometer, and determine the translation vector based on the position coordinate data of the first frame of point cloud information and the position coordinate data of the initial position information;

[0144] Based on the Euclidean distance, determine the farthest position information collected by the satellite positioning sensor that is farthest from the initial position information, and determine the first plane vector based on the position coordinate data of the initial position information and the position coordinate data of the farthest position information;

[0145] Based on the timestamp, determine the farthest frame point cloud information collected by the laser odometer corresponding to the farthest position information, and determine the second plane vector based on the position coordinate data of the first frame point cloud information and the position coordinate data of the farthest frame point cloud information; and

[0146] A rotation angle is determined based on the first plane vector and the second plane vector, and a rotation matrix is ​​determined based on the rotation angle.

[0147] In some optional implementations, the information fusion module 400 may include:

[0148] The information accumulation submodule is configured to accumulate measurement information collected by the inertial measurement unit for a preset number of times to form a group, wherein the preset number of times is determined based on the measurement frequency of the inertial measurement unit and the measurement frequency of the laser odometer.

[0149] The information fusion submodule is configured to fuse a set of measurement information with the point cloud information collected by the laser odometer and the position information collected by the satellite positioning sensor to obtain posture information.

[0150] In some optional implementations, the information accumulation submodule is further configured to:

[0151] The measurement information collected by the inertial measurement unit 10 times is accumulated to form a group, where the measurement frequency of the inertial measurement unit is 100 Hz and the measurement frequency of the laser odometer is 10 Hz.

[0152] In some optional implementations, the information fusion module 400 is further configured to:

[0153] Based on a preset global factor graph model, the point cloud information collected by the laser odometer, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor are integrated to obtain the posture information;

[0154] Among them, the measurement information collected by the inertial measurement unit is used as the pre-integration factor, the position information collected by the satellite positioning sensor is used as a unary factor, and the relative posture information determined by the adjacent frame point cloud information collected by the laser odometer is used as a binary factor;

[0155] The initial pose information obtained by converting the pose data of the point cloud information collected by the laser odometer into the coordinate system of the position information collected by the satellite positioning sensor is used as a variable.

[0156] In some optional implementations, the loop search module 500 may include:

[0157] The nearest neighbor search submodule is configured to perform a nearest neighbor search on the pose information corresponding to the key frame point cloud information collected by the laser odometer to obtain the search frame point cloud information and the nearest neighbor frame point cloud information.

[0158] The mileage calculation submodule is configured to determine the cumulative mileage between the nearest neighbor frame point cloud information and the search frame point cloud information, and use the search frame point cloud information and the nearest neighbor frame point cloud information whose cumulative mileage is greater than a preset mileage threshold as candidate frame point cloud information for forming a loop.

[0159] In some optional implementations, the nearest neighbor search submodule is further configured to:

[0160] Determining the search frame point cloud information at intervals of a preset distance based on the pose information corresponding to the key frame point cloud information; and

[0161] A nearest neighbor search within a preset radius is performed on the pose information corresponding to the search frame point cloud information, and other key frame point cloud information whose pose information is within the preset radius is used as the nearest neighbor frame point cloud information.

[0162] In some optional implementations, the loopback matching module 600 may include:

[0163] The posture registration submodule is configured to determine the relative posture information of the candidate frame point cloud information based on the posture information corresponding to the candidate frame point cloud information.

[0164] The loop closure matching submodule is configured to match the candidate frame point cloud information based on the relative pose information of the candidate frame point cloud information, and use the candidate frame point cloud information whose matching results meet a preset threshold as the loop closure frame point cloud information.

[0165] In some optional implementations, the loopback matching submodule is further configured as follows:

[0166] Extract line and surface features of candidate frame point cloud information;

[0167] Based on the relative pose information of the candidate frame point cloud information, the line and surface features of the candidate frame point cloud information are matched to obtain a matching score, and the candidate frame point cloud information whose matching score meets a preset score threshold is used as the loop frame point cloud information.

[0168] In some optional implementations, the loopback matching module 600 may further include:

[0169] The sub-image stitching sub-module is configured to stitch the point cloud information of the candidate frame with the point cloud information of a preset number of frames before and / or after it to obtain a candidate sub-image.

[0170] The loop closure matching submodule is configured to match the candidate sub-images based on the relative pose information of the candidate frame point cloud information, and use the candidate sub-images whose matching results meet a preset threshold as loop closure sub-images.

[0171] In addition, an embodiment of the present application also provides an autonomous driving vehicle, which includes at least one processor and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned loop detection method 1000.

[0172] An embodiment of the present application also provides a mobile robot, which includes at least one processor and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned loop detection method 1000.

[0173] The present application also provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, the above-mentioned loop detection method 1000 is implemented.

[0174] The present application also provides a computer program product, including a computer program, which implements the loop detection method 1000 when executed by a processor.

[0175] Fig.18 A block diagram of an example autonomous vehicle or mobile robot 3000 that can be used to implement embodiments of the present application is shown. Autonomous vehicle or mobile robot 3000 is intended to represent various forms of digital computers. Autonomous vehicle or mobile robot 3000 can also represent various forms of mobile devices that can run computing programs. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present application described and / or claimed herein.

[0176] like Fig.18 As shown, the autonomous driving vehicle or mobile robot 3000 includes a computing unit 710, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 720 or a computer program loaded from a storage unit 780 to a random access memory (RAM) 730. In RAM 730, various programs and data required for the operation of the autonomous driving vehicle or mobile robot 3000 can also be stored. The computing unit 710, ROM 720, and RAM 730 are connected to each other via a bus 740. An input / output (I / O) interface 750 is also connected to the bus 740.

[0177] A number of components in the autonomous vehicle or mobile robot 3000 are connected to the I / O interface 750, including: an input unit 760, such as a touch screen, etc.; an output unit 770, such as various types of displays, speakers, etc.; a storage unit 780, such as a disk, etc.; and a communication unit 790, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 790 allows the autonomous vehicle or mobile robot 3000 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0178] The computing unit 710 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 710 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 710 performs the various methods and processes described above, such as the loop detection method 1000. For example, in some embodiments, the loop detection method 1000 may be implemented as a computer software program, which is tangibly included in a machine-readable medium, such as a storage unit 780. In some embodiments, part or all of the computer program may be loaded and / or installed on the autonomous driving vehicle or mobile robot 3000 via the ROM 720 and / or the communication unit 790. When the computer program is loaded into the RAM 730 and executed by the computing unit 710, one or more steps of the loop detection method 1000 described above may be performed. Alternatively, in other embodiments, the computing unit 710 may be configured to execute the loop detection method 1000 in any other appropriate manner (eg, by means of firmware).

[0179] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0180] The program code for implementing the method of the present application can be written in any combination of one or more programming languages. These program codes can be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that the program code, when executed by the processor or controller, implements the functions / operations specified in the flow chart and / or block diagram. The program code can be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0181] In the context of the present application, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0182] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this disclosure can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solution disclosed in this application can be achieved, and this document is not limited here.

[0183] The above specific implementations do not constitute a limitation on the protection scope of this application. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of this application should be included in the protection scope of this application.

Claims

1. A loop detection method, characterized in that: include: The point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are integrated to obtain the position and posture information of the moving target in the global coordinates; Determine candidate frame point cloud information that forms a loop based on a nearest neighbor search of the pose information; as well as The candidate frame point cloud information is matched based on the pose information corresponding to the candidate frame point cloud information to determine the loop frame point cloud information.

2. The method according to claim 1, characterized in that The point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are integrated to obtain the position and posture information of the moving target in the global coordinates, including: Selecting, from the position information collected by the satellite positioning sensor, position information whose covariance is less than or equal to a preset covariance threshold; adjusting the noise of the corresponding position information based on the covariance of the selected position information to optimize the selected position information; and The optimized position information is fused with the point cloud information collected by the laser odometer and the measurement information collected by the inertial measurement unit to obtain the position and posture information.

3. The method according to claim 2, characterized in that The step of adjusting the noise of the corresponding position information based on the covariance of the selected position information to optimize the selected position information includes: Determine a covariance weight of the corresponding position information based on the covariance of the selected position information and a preset noise function; Determining the noise of the corresponding position information based on the covariance and covariance weight of the selected position information; and An optimization weight is determined based on the noise of the selected position information to weight the corresponding position information to obtain the optimized position information.

4. The method according to claim 3, characterized in that The preset covariance threshold is 5.

5. The method according to claim 1, characterized in that The point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit and the position information collected by the satellite positioning sensor are integrated to obtain the position and posture information of the moving target in the global coordinates, including: Converting the pose data of the point cloud information collected by the laser odometer into the coordinate system of the position information collected by the satellite positioning sensor to obtain initial pose information; Determining relative pose information based on the pose data of adjacent frame point cloud information collected by the laser odometer; and The initial posture information and the relative posture information are integrated with the position information collected by the satellite positioning sensor and the measurement information collected by the inertial measurement unit to obtain the posture information.

6. A loop detection device, characterized in that: include: An information fusion module is configured to fuse the point cloud information of the surrounding environment collected by the laser odometer on the moving target, the measurement information collected by the inertial measurement unit, and the position information collected by the satellite positioning sensor to obtain the position and posture information of the moving target in the global coordinates; A loop closure search module is configured to determine candidate frame point cloud information that forms a loop based on a nearest neighbor search of the pose information; as well as The loop closure matching module is configured to match the candidate frame point cloud information based on the pose information corresponding to the candidate frame point cloud information to determine the loop closure frame point cloud information.

7. An autonomous driving vehicle, characterized in that: include: at least one processor; as well as, A memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the loop detection method described in any one of claims 1 to 5.

8. A mobile robot, characterized in that: include: at least one processor; as well as, A memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the loop detection method described in any one of claims 1 to 5.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the loop detection method as described in any one of claims 1 to 5 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the loop detection method as described in any one of claims 1 to 5 is implemented.

Images