3D laser odometry positioning method and device with loop closure optimization

Abstract

The application relates to the technical field of unmanned positioning, and discloses a 3D laser odometry positioning method and equipment with loop optimization, which comprises the following steps: acquiring point clouds and IMU data, correcting the point clouds in combination with the angular velocity in the IMU data, converting to a world coordinate system to obtain source point clouds, matching and optimizing the source point clouds and a local map constructed based on key frames to obtain an initial transformation pose at the time when the error between the source point clouds and the local map is the smallest; performing key frame updating; if the key frame set does not change, the initial transformation pose is taken as a final correction pose; otherwise, loop detection is performed on the source point clouds; if the loop detection is performed, a loop frame and an error transformation matrix are acquired, and the final correction pose is calculated in combination with the error transformation matrix and the initial transformation pose. The application can solve the problem of precision decline caused by cumulative error, and improve the positioning efficiency while improving the positioning precision.

Description

Technical Field

[0001] This invention relates to the field of unmanned positioning technology, and in particular to a 3D laser odometry positioning method and device with loop closure optimization. Background Technology

[0002] 3D laser odometry is a device that uses laser technology to measure travel distance in three-dimensional space. 3D laser odometry can be used for positioning on many intelligent mobile platforms, such as outdoor positioning of mobile robots, positioning of autonomous vehicles, and positioning of drones.

[0003] Early 3D laser odometry directly used point clouds for pose matching and calculation. Later, it gradually began to extract feature points for this purpose. Currently, to further improve positioning accuracy and address positioning needs in complex scenarios, a fusion approach has emerged, combining 3D laser odometry and Inertial Measurement Unit (IMU) data. Examples include the fast_lio2 algorithm, which stores point cloud data using an ikd-tree data structure and fuses it with IMU data for forward and backward error propagation; and 3D laser odometry positioning methods based on fused IMU and line / surface feature point matching using the fast_lio algorithm. These methods can improve positioning accuracy and accelerate calculation speed in structured indoor environments. The fast_lio2 algorithm-based 3D laser odometry positioning method, in particular, shows good positioning performance in long corridor scenarios due to its use of IMU for forward and backward error propagation.

[0004] However, existing localization methods that combine IMU data and 3D laser odometry fail to achieve satisfactory results in large, unstructured outdoor environments. This is especially true when mobile robots navigate uneven terrain, where aggressive movements introduce significant localization errors. The primary reason for this is that while backpropagation can compensate for distortion at each point, its accuracy decreases due to accumulated errors over time. Furthermore, existing methods do not compensate for these accumulated errors; as the robot travels further outdoors, the accumulated errors increase, further impacting localization accuracy. Summary of the Invention

[0005] Therefore, the technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a 3D laser odometry positioning method and device with loop closure optimization, which can solve the problem of accuracy reduction caused by cumulative error and improve positioning efficiency while improving positioning accuracy.

[0006] To address the aforementioned technical problems, this invention provides a 3D laser odometry positioning method with loop closure optimization, comprising: The point cloud data obtained from 3D lidar scanning and the IMU data measured by IMU are acquired, and the point cloud data is distorted by combining the angular velocity in the IMU data to obtain a corrected point cloud. The corrected point cloud is transformed into the world coordinate system to obtain the source point cloud. The source point cloud is matched and optimized with the local map constructed based on keyframes to obtain the initial transformation pose with the smallest error between the source point cloud and the local map. Based on the source point cloud and the initial transformed pose, keyframes are updated, and it is determined whether the updated keyframe set has changed: if the updated keyframe set has not changed, the initial transformed pose is used as the final corrected pose; if the updated keyframe set has changed, loop closure detection is performed on the source point cloud. If loop closure detection is not performed, the local map is updated; if loop closure detection is performed, the loop closure frame is obtained, the error transformation matrix of the source point cloud relative to the loop closure frame is obtained, the final corrected pose is calculated by combining the error transformation matrix and the initial transformation pose, and the pose of the global keyframe is optimized. The local map is updated based on the optimized global keyframe. Output the final corrected pose.

[0007] In one embodiment of the present invention, acquiring point cloud data obtained from 3D lidar scanning and IMU data measured by an IMU, and combining the angular velocity in the IMU data to distort the point cloud data to obtain a corrected point cloud, includes: The point cloud data obtained from 3D LiDAR scanning is acquired and preprocessed, including removing invalid points and performing voxel filtering downsampling, to obtain preprocessed point cloud data. ; Representing the lidar coordinate system L Below t Preprocessed point cloud data at each moment, include n There are 1 laser points, denoted as _ ... , Denotes the first in the lidar coordinate system L i One laser point, , n The total number of laser points within the scanning cycle; A continuous-time motion correction model is established by combining the angular velocity in the IMU data. The continuous-time motion correction model uses interpolation to fit the rotation angle of each laser point within the scanning cycle to remove distortion from the preprocessed point cloud data, thus obtaining the corrected point cloud.

[0008] In one embodiment of the present invention, the step of using interpolation to fit the rotation angle of each laser point within the scanning period to remove distortion from the preprocessed point cloud data and obtain a corrected point cloud includes: The angular velocity of the corresponding point cloud data in the IMU data at time t is obtained as follows: , ;in, This represents the actual measurement value of the IMU. It is the bias of angular velocity. It is white noise of angular velocity; The angular velocity of the laser point is obtained as follows , , ;in, This represents the rotation vector of the laser point in the X direction. This represents the rotation vector of the laser point in the Y direction. This represents the rotation vector of the laser point in the Z direction; The rotation compensation angle of the laser point is calculated as follows: , in, Indicates the first i laser points The rotation compensation angle, express t Moment The Middle i laser points The receiving time; The rotation compensation angle The rotation vector of the laser point in the X direction The rotation vector of the laser point in the Y direction The rotation vector of the laser point in the Z direction As a quaternion, it is transformed into a rotation matrix, and the rotation matrix is ​​used to... All laser points are corrected to obtain... t The point cloud after time correction is as follows: , in, for t The point cloud after time correction, , Let be the rotation matrix.

[0009] In one embodiment of the present invention, the modified point cloud is transformed to a world coordinate system to obtain a source point cloud, and the source point cloud is matched and optimized with a local map constructed based on keyframes to obtain an initial transformed pose with the minimum error between the source point cloud and the local map, including: The corrected point cloud is transferred from the lidar coordinate system L Down-conversion to world coordinate system W After obtaining the source point cloud, construct the matching cost function between the source point cloud and the local map: , in, This represents the matching cost function. Represents a partial map. Represents the source point cloud. Represents source point cloud and local maps Transformation of pose between; The transformation pose at which the matching cost function is minimized is taken as the initial transformation pose. .

[0010] In one embodiment of the present invention, keyframe updating based on the source point cloud and the initial transformed pose includes: judge t Is the set of keyframes for a given moment empty? If it is empty, then... t The source cloud of time and t The initial transformed pose of the source point cloud at time t. Add to the keyframe set to obtain the updated keyframe set ; like t The set of keyframes at a given time is not empty; obtain... t The source cloud of time The length value of each laser point is used, and based on preset rules, the median value of all length values ​​is used to select the keyframe update step size threshold, with a preset rotation angle threshold; if t Initial transformed pose corresponding to the source point cloud at time step Compared to t The initial transformed pose of the source point cloud at time -1 If the displacement is greater than the step size threshold or the relative selection angle is greater than the rotation angle threshold, then the current... t The source cloud of time and the corresponding initial transformation pose Add to the keyframe set to obtain the updated keyframe set Otherwise, abandon the keyframe update.

[0011] In one embodiment of the present invention, determining whether the updated set of keyframes has changed includes: Based on the distance between the initial transformation pose of the source point cloud at the current moment and the poses of all keyframes in the keyframe set at the current moment, select the m closest keyframes. Based on the distance between the initial transformation pose of the source point cloud at the previous moment and the poses of all keyframes in the keyframe set at the previous moment, select the m closest keyframes. If the index numbers of the m keyframes selected at the current moment are the same as those of the m keyframes selected at the previous moment, then it is determined that the updated keyframe set has not changed; otherwise, it is determined that the updated keyframe set has changed.

[0012] In one embodiment of the present invention, loop closure detection of the source point cloud includes: Loop closure detection is performed based on source point cloud and keyframe set to obtain loop closure candidate frames; The source point cloud is matched with the point cloud of the loop closure candidate frame. If the matching error value is less than the preset error threshold, it is determined that loop closure detection has been performed; otherwise, it is determined that loop closure detection has not been performed. The step of matching the source point cloud with the point cloud of the loop closure candidate frame, and determining that loop closure detection has been performed if the matching error value is less than a preset error threshold, includes: Construct the transformation cost function between the source point cloud and the loop closure candidate frame: , in, Represents the transformation cost function. This refers to the source point cloud. This represents the i-th loop closure candidate frame. This represents the transformation matrix of the source point cloud relative to the loop closure candidate frame; If the cost function If the minimum error value is less than the preset error threshold, it is determined that loop closure detection has been performed.

[0013] In one embodiment of the present invention, the step of obtaining the loop closure frame of the loop closure detection, obtaining the error transformation matrix of the source point cloud relative to the loop closure frame, and calculating the final corrected pose by combining the error transformation matrix and the initial transformed pose includes: The candidate frames for loop closure when loop closure detection is confirmed are taken as loop closure frames, and the corresponding transformation matrix is ​​the error transformation matrix. Based on the transformation matrix and the initial transformed pose, the final corrected pose is calculated as follows: , in, for t The final pose after time-lapse correction. Let be the error transformation matrix of the source point cloud relative to the loopback frame.

[0014] In one embodiment of the present invention, the pose optimization of global keyframes includes: The residual equation for the odometer constraint factor is constructed as follows: , in, It is the residual equation of the odometer constraint factor. It is the pose of the (k+1)th keyframe in the keyframe set at this moment. It is the pose of the k-th keyframe in the keyframe set at this moment. It is the pose observation of the (k+1)th keyframe in the keyframe set at this time relative to the kth keyframe; The residual equation for constructing the closure constraint factor is as follows: , in, It is the residual equation of the loop constraint factor. It represents the pose of the keyframes in the current keyframe set. It is the pose of the historical keyframe corresponding to the loopback frame. It is the pose observation of the keyframe in the keyframe set at this time relative to the historical keyframe corresponding to the loopback frame; Combining the odometry constraint factor and the laparoscopy constraint factor, the optimization cost function is constructed as follows: , in, It is an optimization cost function; The poses of all keyframes at the minimum value of the optimized cost function are used to update the poses of the corresponding keyframes in the keyframe set at this time.

[0015] The present invention also provides a 3D laser odometry positioning device with loop closure optimization, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the 3D laser odometry positioning method with loop closure optimization.

[0016] The technical solution of the present invention has the following advantages compared with the prior art: This invention combines angular velocity data from IMU data to distort point cloud data, and then uses loop closure detection to correct the final output pose, thereby solving the problem of decreased robot accuracy caused by accumulated errors and improving positioning accuracy. At the same time, this invention uses single-frame point cloud and local map matching when calculating pose, which can reduce the amount of computation, thereby improving positioning efficiency while improving positioning accuracy. Attached Figure Description

[0017] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings, wherein: Figure 1 This is a flowchart of the method of the present invention.

[0018] Figure 2 This is a flowchart illustrating the steps of the method of the present invention. Detailed Implementation

[0019] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0020] Example 1 Reference Figure 1 and Figure 2 As shown, this invention discloses a 3D laser odometry positioning method with loop closure optimization, comprising the following steps: S1: Acquire point cloud data obtained from 3D LiDAR scanning and IMU data measured by IMU, and combine the angular velocity in the IMU data to distort the point cloud data to obtain a corrected point cloud.

[0021] S1-1: Acquire dense point cloud data obtained from 3D LiDAR scanning and preprocess it. Preprocessing includes removing invalid points and performing voxel filtering downsampling to obtain preprocessed point cloud data. , Representing the lidar coordinate system L Below t The preprocessed point cloud data at each time step. In this embodiment, invalid points can be removed using radius filtering or mean filtering. Voxel filtering can reduce the number of point cloud samples in dense point cloud data, optimize the point cloud structure, and improve subsequent processing efficiency. include n There are 1 laser points, denoted as _ ... , Denotes the first in the lidar coordinate system L i One laser point, , n This represents the total number of laser points within the scanning cycle.

[0022] S1-2: A continuous-time motion correction model is established by combining the angular velocity in the IMU data. The continuous-time motion correction model uses interpolation to accurately fit the rotation angle of each laser point in one scanning cycle of the 3D LiDAR to remove distortion of the laser points in the preprocessed point cloud data of each frame, thus obtaining the corrected point cloud.

[0023] S1-2-1: Obtain the angular velocity of the corresponding point cloud data in the IMU data. , This represents the angular velocity of the point cloud in the IMU data at time t. ;in, This represents the actual measurement value of the IMU. It is the bias of angular velocity. It is white noise of angular velocity. and Obtained through online calibration using a preset algorithm.

[0024] S1-2-2: Obtaining the laser point The angular velocity is , , , Let be the angular velocity of the corresponding point cloud data in the IMU data at time t-1; where, This represents the rotation vector of the laser point in the X direction. This represents the rotation vector of the laser point in the Y direction. This represents the rotation vector of the laser point in the Z direction.

[0025] S1-2-3: The rotation compensation angle of the laser point is calculated as follows: , in, Indicates the first i Rotation compensation angle of each laser point express t Moment The Middle i laser points The receiving time.

[0026] S1-2-4: Adjust the rotation compensation angle The rotation vector of the laser point in the X direction The rotation vector of the laser point in the Y direction The rotation vector of the laser point in the Z direction Transformation into a rotation matrix as a quaternion Using rotation matrix pairs All laser points are corrected to obtain... t The corrected point cloud at time (i.e., one frame) is as follows: , in, for t The point cloud after time correction, .

[0027] S2: Transform the corrected point cloud into the world coordinate system to obtain the source point cloud, and match and optimize the source point cloud with the local map constructed based on keyframes to obtain the initial transformation pose with the smallest error between the source point cloud and the local map.

[0028] S2-1: Transfer the corrected point cloud from the lidar coordinate system L Down-conversion to world coordinate system W Source cloud , , It is the coordinate system of the lidar L Transform to world coordinate system W The transformation matrix.

[0029] S2-2: Construct the matching cost function between the source point cloud and the local map using the GICP matching method: , in, This represents the matching cost function. Represents a partial map. Represents the source point cloud. Represents source point cloud and local maps The pose changes between positions.

[0030] S2-3: The transformation pose at which the matching cost function is minimized is taken as the initial transformation pose. ,Right now .

[0031] S3: Update keyframes based on the source point cloud and initial transformed pose.

[0032] S3-1: Determine the current situation t Check if the set of keyframes for a given moment is empty. If it is empty, execute S3-2; otherwise, execute S3-3.

[0033] S3-2: If the current t If the keyframe set for a given moment is empty, meaning the current moment is the initial moment and the corresponding keyframes are empty, then... t The source cloud of time and the corresponding initial transformation pose Add to the keyframe set to obtain the updated keyframe set .

[0034] S3-3: If the current t If the set of keyframes for a given moment is not empty, obtain the current keyframe. t The source cloud of time The length value (i.e., distance value) of each laser point is used, and a keyframe update step size threshold is selected based on the median value of all length values ​​according to preset rules. A preset rotation angle threshold is also used. The step size threshold and rotation angle threshold are determined according to the actual situation. In this embodiment, they are set as follows: Step size threshold is The rotation angle threshold is thr =35°.

[0035] Get the previous moment, i.e. t Source cloud at time -1 The source point cloud of the previous time step is calculated using the method in S3. Corresponding initial transformation pose Specifically, it involves using the GICP matching method to construct... The matching cost function of the local map is used, and the transformed pose at the minimum value of the matching cost function is used as the source point cloud of the previous time step. Corresponding initial transformation pose .

[0036] like t Initial transformed pose corresponding to the source point cloud at time step Compared to t The initial transformed pose of the source point cloud at time -1 If the displacement is greater than the step size threshold or the relative selection angle is greater than the rotation angle threshold, then the current... t The source cloud of time and the corresponding initial transformation pose Add to the keyframe set to obtain the updated keyframe set Otherwise, abandon the keyframe update.

[0037] S4: Determine if the updated keyframe set has changed: If the updated keyframe set has not changed, proceed to S5; if the updated keyframe set has changed, proceed to S6.

[0038] The method to determine if the updated set of keyframes has changed is: based on the source point cloud at the current moment. Corresponding initial transformation pose With keyframe set Select the m closest keyframes based on the pose distances between all keyframes, and then use the source point cloud from the previous time step. Corresponding initial transformation pose Keyframe set from the previous time step The m closest keyframes are selected based on the distances between the poses of all keyframes. If the index numbers of the m keyframes selected at the current time step are the same as those of the m keyframes selected at the previous time step, then the updated keyframe set is considered unchanged compared to the previous keyframe set; if the index numbers of the m keyframes selected at the current time step are different from those of the m keyframes selected at the previous time step, then the updated keyframe set is considered changed compared to the previous keyframe set.

[0039] S5: If the updated keyframe set remains unchanged, it indicates that the aggressive outdoor motion has not produced excessive errors. In this case, the initial pose transformation is... As the final pose, execute S8.

[0040] S6: If the updated set of keyframes changes, then perform loop closure detection on the source point cloud.

[0041] S6-1: Perform loop closure detection based on source point cloud and keyframe set to obtain loop closure candidate frames.

[0042] Using different 3D point cloud matching methods, the updated keyframe set is successively used. Select the source point cloud. The matched keyframes are used as loop closure candidate frames. In this embodiment, the contour_context algorithm is first used to select from the updated keyframe set. The source point cloud was selected from the data. Similar keyframes are used as the first screening to obtain loop closure candidate frames. Then, the GICP algorithm is used to further filter out the loop closure candidate frames obtained in the first screening and the source point cloud. Frames that are more similar are selected as candidate frames for loop closure.

[0043] When determining whether keyframes are similar, the contour_context algorithm sets a distance threshold and considers the source point cloud. Compared to the updated keyframe set Keyframes whose distance to other keyframes is less than a certain distance threshold are used as candidate frames for loop closure in the first screening; the GICP algorithm sets an error threshold and selects the source point cloud as the source point cloud. Keyframes whose matching scores with keyframes in the loopback candidate frames are less than the error threshold are selected as loopback candidate frames. The values ​​of the distance threshold and the error threshold are determined based on the actual situation; in this embodiment, the error threshold is set to 0.2.

[0044] S6-2: Match the source point cloud with the point cloud of the loop closure candidate frame. If the matching error value is less than the preset error threshold, it is determined that loop closure detection has been performed, and S7 is executed; otherwise, it is determined that loop closure detection has not been performed, and the local map is updated.

[0045] S6-2-1: Constructing the transformation cost function between the source point cloud and loop closure candidate frames: , in, Represents the transformation cost function. This refers to the source point cloud. This represents the i-th loop closure candidate frame. This represents the transformation matrix of the source point cloud relative to the loop closure candidate frame; S6-2-2: If the cost function If the minimum error value is less than the preset error threshold, it is determined that loop closure detection has been performed, and step S7 is executed; otherwise, it is determined that loop closure detection has not been performed, and the local map is updated.

[0046] S7: If loop closure detection is performed, obtain the loop closure frame of the loop closure detection, obtain the error transformation matrix of the source point cloud relative to the loop closure frame, calculate the final corrected pose by combining the error transformation matrix and the initial transformation pose, optimize the pose of the global keyframe, and update the local map based on the optimized global keyframe.

[0047] S7-1: The candidate frame for loop closure when loop closure detection is confirmed is taken as the loop closure frame, and the corresponding transformation matrix is ​​the error transformation matrix. Based on the transformation matrix and the initial transformed pose, the final corrected pose is calculated as follows: , in, for t The final pose after time-lapse correction. Let be the error transformation matrix of the source point cloud relative to the loopback frame.

[0048] S7-2: Optimize the pose of global keyframes and update the local map based on the optimized global keyframes.

[0049] S7-2-1: Combine odometry constraint factor, loop closure constraint factor, and loop closure frame to optimize the pose of global keyframes.

[0050] S7-2-1-1: The odometry constraint factor uses a binary factor to constrain the relative pose between two keyframes. The residual equation for the odometry constraint factor is as follows: , in, It is the residual equation of the odometer constraint factor. It is the pose of the (k+1)th keyframe. It is the pose of the k-th keyframe. It is the pose observation of the (k+1)th keyframe relative to the kth keyframe.

[0051] The loop closure constraint factor uses a binary factor to constrain the relative pose between the current keyframe and the historical keyframe where the loop closure occurred. The residual equation for the loop closure constraint factor is as follows: , in, It is the residual equation of the loop constraint factor. It is the pose of the current keyframe. It is the pose of the historical keyframe corresponding to the loopback frame. It is the pose observation of the current keyframe relative to the historical keyframe corresponding to the loopback frame.

[0052] S7-2-1-2: Combining the odometry constraint factor and the laparoscopy constraint factor, the optimization cost function is constructed as follows: , in, It is an optimization cost function.

[0053] S7-2-1-3: Optimize the optimization cost function using the GTSAM algorithm, and update the pose of the corresponding keyframe in the keyframe set at this time using the pose of all global keyframes when the optimization cost function reaches its minimum value.

[0054] S7-2-2: Update the local map based on the optimized global keyframes.

[0055] S8: The odometer outputs the final corrected pose.

[0056] Example 2 The present invention also discloses a 3D laser odometry positioning system with loop closure optimization, including a point cloud correction module, an initial transformation pose calculation module, a loop closure detection module, a final pose calculation module, a local map update module, and a final pose output module.

[0057] The point cloud correction module is used to acquire point cloud data obtained from 3D lidar scanning and IMU data measured by IMU, and combine the angular velocity in the IMU data to distort the point cloud data to obtain a corrected point cloud.

[0058] The initial transformation pose calculation module is used to transform the corrected point cloud into the world coordinate system to obtain the source point cloud, and to match and optimize the source point cloud with the local map constructed based on keyframes to obtain the initial transformation pose with the smallest error between the source point cloud and the local map.

[0059] The loop closure detection module is used to update keyframes based on the source point cloud and the initial transformed pose, and to determine whether the updated keyframe set has changed. If the updated keyframe set has changed, loop closure detection is performed on the source point cloud. The final pose calculation module is used to use the initial transformed pose as the final corrected pose when the updated keyframe set has not changed. When loop closure detection is performed, the loop closure frame is obtained, the error transformation matrix of the source point cloud relative to the loop closure frame is obtained, and the final corrected pose is calculated by combining the error transformation matrix and the initial transformed pose.

[0060] The local map update module is used to update the local map when loop closure detection is not performed, and to optimize the pose of global keyframes when loop closure detection is performed, and update the local map based on the optimized global keyframes.

[0061] The final pose output module is used to output the final corrected pose.

[0062] Example 3 The present invention also discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the 3D laser odometry positioning method with loop closure optimization in Embodiment 1.

[0063] Example 4 The present invention also discloses a 3D laser odometry positioning device with loop closure optimization, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the 3D laser odometry positioning method with loop closure optimization.

[0064] The advantages of this invention compared to the prior art are: 1. This invention uses the angular velocity in IMU data and the angular acceleration motion model to construct a continuous time trajectory to quickly distort the point cloud. This can solve the problem of inaccurate positioning caused by the aggressive movement of robots in outdoor unstructured environments, thereby improving positioning accuracy.

[0065] 2. This invention addresses the problem of decreased accuracy caused by accumulated errors when a robot travels outdoors. By using loop closure detection to correct the final output pose, it solves the problem that the odometer positioning error increases with the distance traveled, thereby further improving positioning accuracy.

[0066] 3. This invention improves the positioning accuracy of long tunnel scenes by setting step size threshold and rotation angle threshold and using an adaptive keyframe update method to select keyframes to construct a local map, which can ensure point cloud density while improving the positioning accuracy of long tunnel scenes.

[0067] 4. In existing technologies, point cloud matching for laser mileage calculation consists of two parts. The first part calculates the initial pose value by matching point clouds within a single frame, and then uses this initial pose value to calculate the final pose by matching point clouds within a single frame with a local map in the second part. This invention, however, uses point clouds within a single frame and a local map for pose calculation, which reduces the computational load and solves the problem of high computational complexity in laser mileage calculation.

[0068] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0069] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0070] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0071] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0072] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A 3D laser odometry positioning method with loop closure optimization, characterized in that, include: The point cloud data obtained from 3D lidar scanning and the IMU data measured by IMU are acquired, and the point cloud data is distorted by combining the angular velocity in the IMU data to obtain a corrected point cloud. The corrected point cloud is transformed into the world coordinate system to obtain the source point cloud. The source point cloud is matched and optimized with the local map constructed based on keyframes to obtain the initial transformation pose with the smallest error between the source point cloud and the local map. Based on the source point cloud and the initial transformed pose, keyframes are updated, and it is determined whether the updated keyframe set has changed: if the updated keyframe set has not changed, the initial transformed pose is used as the final corrected pose; if the updated keyframe set has changed, loop closure detection is performed on the source point cloud. If no loop closure detection is performed, update the local map; If loop closure detection is performed, the loop closure frame is obtained, the error transformation matrix of the source point cloud relative to the loop closure frame is obtained, the final corrected pose is calculated by combining the error transformation matrix and the initial transformation pose, and the pose of the global keyframe is optimized. The local map is updated based on the optimized global keyframe. Output the final corrected pose; The process involves transforming the corrected point cloud to the world coordinate system to obtain the source point cloud, and then matching and optimizing the source point cloud with a local map constructed based on keyframes to obtain the initial transformed pose with the minimum error between the source point cloud and the local map. This includes: The corrected point cloud is transferred from the lidar coordinate system L Down-conversion to world coordinate system W After obtaining the source point cloud, construct the matching cost function between the source point cloud and the local map: ， in, This represents the matching cost function. Represents a partial map. Represents the source point cloud. Represents source point cloud and local maps Transformation of pose between; The transformation pose at which the matching cost function is minimized is taken as the initial transformation pose. .

2. The 3D laser odometry positioning method with loop closure optimization according to claim 1, characterized in that: Acquiring point cloud data from 3D LiDAR scanning and IMU data from IMU measurements, and combining the angular velocity data from the IMU data to perform distortion correction on the point cloud data to obtain a corrected point cloud, including: The point cloud data obtained from 3D LiDAR scanning is acquired and preprocessed, including removing invalid points and performing voxel filtering downsampling, to obtain preprocessed point cloud data. ; Representing the lidar coordinate system L Below t Preprocessed point cloud data at each moment, include n There are 1 laser points, denoted as _ ... , Denotes the first in the lidar coordinate system L i One laser point, , n The total number of laser points within the scanning cycle; A continuous-time motion correction model is established by combining the angular velocity in the IMU data. The continuous-time motion correction model uses interpolation to fit the rotation angle of each laser point within the scanning cycle to remove distortion from the preprocessed point cloud data, thus obtaining the corrected point cloud.

3. The 3D laser odometry positioning method with loop closure optimization according to claim 2, characterized in that: The step of using interpolation to fit the rotation angle of each laser point within the scanning period to remove distortion from the preprocessed point cloud data, resulting in a corrected point cloud, includes: The angular velocity of the corresponding point cloud data in the IMU data at time t is obtained as follows: , ;in, This represents the actual measurement value of the IMU. It is the bias of angular velocity. It is white noise of angular velocity; The angular velocity of the laser point is obtained as follows , , ;in, This represents the rotation vector of the laser point in the X direction. This represents the rotation vector of the laser point in the Y direction. This represents the rotation vector of the laser point in the Z direction; The rotation compensation angle of the laser point is calculated as follows: ， in, Indicates the first i laser points The rotation compensation angle, express t Moment Middle i laser points The receiving time; The rotation compensation angle The rotation vector of the laser point in the X direction The rotation vector of the laser point in the Y direction The rotation vector of the laser point in the Z direction As a quaternion, it is transformed into a rotation matrix, and the rotation matrix is ​​used to... All laser points are corrected to obtain... t The point cloud after time correction is as follows: ， in, for t The point cloud after time correction, , Let be the rotation matrix.

4. The 3D laser odometry positioning method with loop closure optimization according to claim 1, characterized in that: Keyframe updates based on the source point cloud and initial transformed pose include: judge t Is the set of keyframes for a given moment empty? If it is empty, then... t The source cloud of time and t The initial transformed pose of the source point cloud at time t. Add to the keyframe set to obtain the updated keyframe set ; like t The set of keyframes at a given time is not empty; obtain... t The source cloud of time The length value of each laser point is used, and based on preset rules, the median value of all length values ​​is used to select the keyframe update step size threshold, with a preset rotation angle threshold; if t Initial transformed pose corresponding to the source point cloud at time step Compared to t The initial transformed pose of the source point cloud at time -1 If the displacement is greater than the step size threshold or the relative selection angle is greater than the rotation angle threshold, then the current... t The source cloud of time and the corresponding initial transformation pose Add to the keyframe set to obtain the updated keyframe set Otherwise, abandon the keyframe update.

5. The 3D laser odometry positioning method with loop closure optimization according to claim 1, characterized in that: Determine whether the updated set of keyframes has changed, including: Based on the distance between the initial transformation pose of the source point cloud at the current moment and the poses of all keyframes in the keyframe set at the current moment, select the m closest keyframes. Based on the distance between the initial transformation pose of the source point cloud at the previous moment and the poses of all keyframes in the keyframe set at the previous moment, select the m closest keyframes. If the index numbers of the m keyframes selected at the current moment are the same as those of the m keyframes selected at the previous moment, then it is determined that the updated keyframe set has not changed; otherwise, it is determined that the updated keyframe set has changed.

6. The 3D laser odometry positioning method with loop closure optimization according to claim 1, characterized in that, Loop closure detection of the source point cloud includes: Loop closure detection is performed based on source point cloud and keyframe set to obtain loop closure candidate frames; The source point cloud is matched with the point cloud of the loop closure candidate frame. If the matching error value is less than the preset error threshold, it is determined that loop closure detection has been performed; otherwise, it is determined that loop closure detection has not been performed. The step of matching the source point cloud with the point cloud of the loop closure candidate frame, and determining that loop closure detection has been performed if the matching error value is less than a preset error threshold, includes: Construct the transformation cost function between the source point cloud and the loop closure candidate frame: ， in, Represents the transformation cost function. This refers to the source point cloud. This represents the i-th loop closure candidate frame. This represents the transformation matrix of the source point cloud relative to the loop closure candidate frame; If the cost function If the minimum error value is less than the preset error threshold, it is determined that loop closure detection has been performed.

7. The 3D laser odometry positioning method with loop closure optimization according to claim 6, characterized in that, The process of acquiring the loop closure frame detected by loop closure, acquiring the error transformation matrix of the source point cloud relative to the loop closure frame, and calculating the final corrected pose by combining the error transformation matrix and the initial transformed pose includes: The candidate frames for loop closure when loop closure detection is confirmed are taken as loop closure frames, and the corresponding transformation matrix is ​​the error transformation matrix. Based on the transformation matrix and the initial transformed pose, the final corrected pose is calculated as follows: ， in, for t The final pose after time-lapse correction. Let be the error transformation matrix of the source point cloud relative to the loopback frame.

8. The 3D laser odometry positioning method with loop closure optimization according to claim 1, characterized in that: The pose optimization of global keyframes includes: The residual equation for the odometer constraint factor is constructed as follows: ， in, It is the residual equation of the odometer constraint factor. It is the pose of the (k+1)th keyframe in the keyframe set at this moment. It is the pose of the k-th keyframe in the keyframe set at this moment. It is the pose observation of the (k+1)th keyframe in the keyframe set at this time relative to the kth keyframe; The residual equation for constructing the closure constraint factor is as follows: ， in, It is the residual equation of the loop constraint factor. It represents the pose of the keyframes in the current keyframe set. It is the pose of the historical keyframe corresponding to the loopback frame. It is the pose observation of the keyframe in the keyframe set at this time relative to the historical keyframe corresponding to the loopback frame; Combining the odometry constraint factor and the laparoscopy constraint factor, the optimization cost function is constructed as follows: ， in, It is an optimization cost function; The poses of all keyframes at the minimum value of the optimized cost function are used to update the poses of the corresponding keyframes in the keyframe set at this time.

9. A 3D laser odometry positioning device with loop closure optimization, characterized in that: It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the 3D laser odometry positioning method with loop closure optimization as described in any one of claims 1-8.

Images