A fusion imu three-dimensional laser radar positioning and mapping method

Abstract

The application is a kind of fusion IMU three-dimensional laser radar positioning and mapping method. First, the high-frequency pose estimated by IMU is used to linearly interpolate the laser point cloud, thereby correcting the point cloud motion distortion; second, the height threshold is introduced in the ground segmentation part, the feature point cloud is quickly and accurately segmented, and then the point cloud clustering is used to segment the feature point cloud, and the dynamic clustering threshold is introduced for judgment and classification; third, the Scan-Context descriptor is introduced for loop detection; finally, the local map composed of key frames is used for registration with the current frame, the inter-frame matching error is calculated, and the joint optimization function is constructed by combining the IMU pre-integration error and the loop detection constraint to solve the laser odometry pose. With the KITTI dataset, the improved algorithm and the mainstream LeGO-LOAM scheme are evaluated in terms of trajectory and pose accuracy. The test results show that under the requirement of meeting the real-time of the odometer, the improved laser odometry has higher accuracy than the LeGO-LOAM scheme.

Description

Technical Field

[0001] This invention belongs to the field of robot perception and localization technology, specifically relating to a three-dimensional lidar localization and mapping method integrating IMU. Background Technology

[0002] The statements in this section are merely background information relating to this disclosure, and these statements may constitute prior art. In the process of developing this invention, the inventors discovered at least the following problems in the prior art.

[0003] Simultaneous Localization and Mapping (SLAM) is a crucial technology in mobile robotics. It primarily studies the mapping and localization problems of robots in unknown environments. Using onboard sensors, the robot estimates its pose and constructs a map of its environment, enabling autonomous operation in unfamiliar settings. The working environments of mobile robots are shifting from simple experimental testing scenarios to increasingly complex real-world applications, such as autonomous driving, service robots, and smart agriculture. These uncertainties present significant challenges to the autonomous localization and environmental perception of mobile robots.

[0004] Currently, there are three main SLAM methods: camera-based visual SLAM, radar-based laser SLAM, and multi-sensor fusion-based SLAM. Camera-based visual SLAM is susceptible to light interference, leading to erroneous information. Compared to visual cameras, laser radar offers advantages such as accurate target acquisition, strong anti-interference capabilities, wide detection range, and near-all-weather operation. Laser SLAM includes 2D and 3D laser SLAM. The latter can actively emit multiple laser beams for environmental detection and precise ranging, enabling real-time pose estimation, 3D mapping of the surrounding environment, and scene recognition, thus broadening its application scope. With the rapid development of laser SLAM technology, 3D laser SLAM has become one of the most advanced mobile robot SLAM technologies.

[0005] However, 3D laser SLAM exhibits degradation in open spaces and similar corridors. This degradation is due to the working principle of single-sensor lidar and the characteristics of the environment. In open spaces and similar corridor environments, lidar is susceptible to multipath interference, limitations of reflective surfaces, environmental noise, and power loss, leading to data degradation and performance decline.

[0006] Currently, most solutions employ multiple sensors to address these issues. For example, patent application number 202211616723.5, entitled "A Robot Localization Method, Device, Equipment and Storage Medium," corrects robot pose by fusing multiple sensors and adding semantic information to a known map, thereby solving the problem of easy loss of localization and incorrect matching in environments with similar and sparse features using a single sensor.

[0007] However, few solutions to the above problems are made using a single-sensor lidar. Summary of the Invention

[0008] In view of the above problems, the purpose of this invention is to solve some of the problems in the prior art, or at least alleviate these problems.

[0009] A three-dimensional lidar localization and mapping method integrating IMU includes the following steps:

[0010] Acquire the raw point cloud data collected by the lidar and the high-frequency signal collected by the inertial measurement unit (IMU);

[0011] The original point cloud data is preprocessed and segmented; the point cloud segmentation includes ground point cloud segmentation and point cloud clustering segmentation, so as to convert the original point cloud into a target point cloud containing only obstacles.

[0012] Point cloud distortion correction is performed on the target point cloud to obtain a feature point cloud with motion distortion compensation.

[0013] By calculating the curvature of the feature point cloud, line and surface features are extracted and added to the local feature map, and feature points at consecutive time points are registered.

[0014] Introducing the Scan-Context descriptor for loop closure detection;

[0015] Distortion of the point cloud is eliminated by IMU state prediction. The processed radar pose is used as the initial value for inter-frame registration. A joint optimization error function is constructed to accurately solve the radar pose and generate the final trajectory and a globally consistent map.

[0016] After acquiring the raw point cloud from the lidar and the high-frequency signal from the inertial measurement unit (IMU), the process further includes pre-integration calculation of the high-frequency signal, using the following formula:

[0017]

[0018]

[0019]

[0020] In the formula, Δp ij Δv ijq ij Let i represent the changes in position, velocity, and rotation from the previous time i to the current time j, respectively; t∈(i,j), with symbols... q represents the multiplication of quaternions. i q j Let i and j be the rotation amounts at the previous time i and the current time j, respectively. b is the measured value of angular velocity. gk b ak η represents the bias error of the gyroscope and accelerometer, respectively. gk η ak These represent the measurement noise of the gyroscope and accelerometer, respectively; ΔR ik The pre-integrated ideal value of the previous time i has bias error and noise compared to the measured value; Δv ik Δt represents the ideal velocity value at the previous moment i, and the error exists between it and the measured velocity value; Δt is the time interval. This represents the acceleration of the IMU in the world coordinate system.

[0021] Furthermore, the point cloud segmentation includes the following steps:

[0022] Ground point cloud segmentation: The original point cloud image after point cloud preprocessing is quickly segmented into ground point cloud and target point cloud using a ground segmentation algorithm, and the target point cloud is extracted;

[0023] Point cloud clustering and segmentation: Clustering and segmentation algorithms are used to cluster and segment the target point cloud after the ground point cloud segmentation and extraction.

[0024] The steps for segmenting the ground point cloud and the target point cloud are as follows:

[0025] Angle θ is determined using a ground segmentation algorithm: OK and OM are two adjacent laser lines emitted by the lidar, with corresponding depths R and R, respectively. r and R r-1 θ is the angle between KM and MN, β i β j Let θ be the angle between the lidar beam and the x-axis. The angle θ is calculated as follows:

[0026]

[0027] Introduce a height threshold h that varies based on the installation height of the lidar. pc， To filter out small obstacles close to the ground:

[0028] h pc =0.15h L

[0029] Among them, h L This represents the installation height of the radar;

[0030] Distinguish and segment the target point cloud from the ground point cloud: when the angle θ is less than 15 degrees and the height of the point cloud at points M and K is greater than the height threshold h. pc If the angle θ is greater than 15 degrees, it is determined to be a target point cloud; otherwise, it is a ground point cloud.

[0031] Furthermore, a dynamic clustering threshold is introduced in the point cloud clustering segmentation:

[0032]

[0033] Where, N p N represents the number of cluster points. l d represents the number of scanning laser beams in the vertical direction, and d represents the horizontal distance between the radar and the laser scanning point.

[0034] The target point cloud is corrected for point cloud distortion using high-frequency signals generated by the IMU to complete motion distortion compensation; including the following steps:

[0035] Simultaneously receive and store lidar data and high-frequency signals from the IMU;

[0036] Calculate the time for each sampling point based on the current point cloud timestamp and the LiDAR sampling frequency;

[0037] Based on the current timestamp, search for adjacent IMU attitude data in the storage queue, and estimate the current laser point attitude through spherical linear interpolation;

[0038] Transform the pose of all point clouds from the start to the end of the scan to the pose at the end of the scan.

[0039] Specifically, linear interpolation is performed on the pre-integrated pose of the IMU at the start and end times of point cloud scanning to calculate the pose at the corresponding time within a frame of point cloud; let L k-1 and L k+1 The translation vectors at time points are T. k-1 T k+1 The attitude vectors are P k-1 P k+1 L j-1 and L j+1 The translation vectors at time points are T. j-1 T j+1 The attitude vectors are P j-1 P j+1 ;

[0040] The position interpolation formula is as follows:

[0041]

[0042] The formula for spherical linear interpolation is as follows:

[0043] P k =a(t)P k-1 +b(t)P k+1

[0044]

[0045] In the formula, P k It is the attitude vector to be obtained, P k-1 and P k+1 IMU in L j-1 and L j+1 The pose at time t, a(t) and b(t) are two coefficient variables that change with respect to time t.

[0046] After correcting the point cloud distortion of the target point cloud, the process further includes filtering out non-feature point clouds from the feature point cloud that has undergone motion distortion compensation, and distinguishing between planar features and edge features to reduce the amount of data processing and improve computational efficiency; the specific steps are as follows:

[0047] Remove non-feature point clouds: feature points when the laser beam and the object surface are nearly parallel and feature points in the occluded parts are regarded as unreliable point clouds and are not included in the feature sequence;

[0048] Distinguishing between planar features and edge features: by analyzing point cloud roughness Setting thresholds to distinguish between planar features and edge features allows for the extraction of different types of structural information from point cloud data, facilitating better point cloud matching and calculation.

[0049] Point cloud roughness The formula is:

[0050]

[0051] in, Let k be the set of adjacent points on the same scan line at time k; The coordinates of the point cloud in the current frame of the lidar reference system. To and Point clouds that are adjacent on the same scan line; when the point cloud roughness When the point cloud roughness exceeds a threshold, it is classified as an edge feature; when the point cloud roughness... If the value is less than the threshold, it is classified as a planar feature.

[0052] The Scan-Context descriptor is introduced for loop closure detection, including the following steps:

[0053] The Scan-Context descriptor is introduced for loop closure detection, and the average of the three adjacent maximum height values ​​in each region is selected as the value of the feature matrix.

[0054] If the distance between a scan context pair meets the threshold condition, the next step is to perform a verification by finding a matching loop closure frame based on Euclidean distance.

[0055] If the query point cloud and the candidate point cloud successfully match the loop closure based on Euclidean distance, then the loop closure constraint is added to the nonlinear optimization function to obtain the minimum error.

[0056] Constructing a joint optimization error function to accurately solve for radar pose includes the following steps:

[0057] Optimize inter-frame matching error: Construct a point cloud registration error between two frames to improve the real-time performance of the algorithm by incorporating the features of key frames into the optimization function;

[0058] Optimize IMU error: Construct an IMU error function to improve positioning accuracy;

[0059] Optimize loop closure error; construct loop closure error to improve positioning accuracy;

[0060] A nonlinear cost function is constructed for lidar constraints, IMU measurement constraints, and loop closure constraints. The optimal state that minimizes the error function is then solved using the Gauss-Newton nonlinear optimization algorithm.

[0061] A computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the 3D LiDAR localization and mapping method with fused IMU.

[0062] The present invention has the following beneficial effects:

[0063] 1. To address the degradation issue that occurs with single-sensor LiDAR in open areas and similar corridors, this invention utilizes a loosely coupled laser odometry method combining laser and IMU data in SLAM. The IMU provides motion constraints, and IMU pre-integration and loop closure detection work together to minimize inter-frame matching errors. A joint optimization function is then constructed by combining IMU pre-integration error and loop closure detection constraints to solve for the laser odometry pose, ultimately improving positioning accuracy. Simulation results show that the laser odometry accuracy of this invention is 42% higher than the LeGO-LOAM scheme and more closely approximates the actual trajectory.

[0064] 2. Considering positioning accuracy, this invention takes two specific steps: First, it uses the high-frequency pose estimated by the IMU to perform linear interpolation on the laser point cloud, thereby correcting the motion distortion of the point cloud; Second, in the loop closure detection part, it uses the distance information generated by the Scan-Context descriptor sub-loop closure detection module. If it meets the judgment criteria, the next step is to find the matching loop closure frame based on Euclidean distance. After successful verification, loop closure constraints are added, effectively avoiding the influence of accumulated errors.

[0065] 3. To reduce computational load, this invention introduces pre-integration calculation for the high-frequency signal of the IMU and introduces a height threshold in the ground segmentation part. In addition, a dynamic clustering threshold is introduced in the point cloud clustering segmentation to determine the classification, thereby converting the original point cloud into a target point cloud containing only obstacles, which can be used in subsequent feature extraction and matching. Furthermore, by filtering out non-feature point clouds from the feature point cloud that completes motion distortion compensation, the amount of data computation can be greatly reduced and the computational efficiency can be improved. Attached Figure Description

[0066] Figure 1 This is a flowchart of the present invention;

[0067] Figure 2 This is a schematic diagram of the ground segmentation algorithm.

[0068] Figure 3 This is a schematic diagram of IMU linear interpolation;

[0069] Figure 4 This is a comparison diagram of the trajectories of the present invention and LeGO-LOAM in the KITTI 05 sequence. Detailed Implementation

[0070] The present invention will be further described below with reference to the accompanying drawings. The embodiments of the present invention are only used to illustrate the present invention and not to limit the present invention. Various substitutions and modifications made based on ordinary technical knowledge and common practices in the art without departing from the technical concept of the present invention should be included within the scope of the present invention.

[0071] This invention addresses the limitation of single-sensor 3D LiDAR, which exhibits degradation in open spaces and similar corridors. It proposes a 3D LiDAR localization and mapping method based on a single sensor and fused IMU, by applying a loosely coupled laser odometry method to SLAM.

[0072] like Figure 1 As shown, a three-dimensional lidar localization and mapping method integrating IMU includes the following steps:

[0073] Acquire the raw point cloud data collected by the lidar and the high-frequency signal collected by the inertial measurement unit (IMU);

[0074] The original point cloud data is preprocessed and segmented; the point cloud segmentation includes ground point cloud segmentation and point cloud clustering segmentation, so as to convert the original point cloud into a target point cloud containing only obstacles.

[0075] Point cloud distortion correction is performed on the target point cloud to obtain a feature point cloud with motion distortion compensation.

[0076] By calculating the curvature of the feature point cloud, line and surface features are extracted and added to the local feature map, and feature points at consecutive time points are registered.

[0077] Introducing the Scan-Context descriptor for loop closure detection;

[0078] Distortion of the point cloud is eliminated by IMU state prediction. The processed radar pose is used as the initial value for inter-frame registration. A joint optimization error function is constructed to accurately solve the radar pose and generate the final trajectory and a globally consistent map.

[0079] The high-frequency signals acquired by the IMU include acceleration data, angular velocity data, etc.

[0080] Preprocessing the raw point cloud data, including projecting the raw point cloud to obtain a raw point cloud image with depth information, and removing invalid points from the raw point cloud, aims to compensate for motion distortion in the point cloud and generate high-quality point cloud frames.

[0081] Invalid points are those that may arise from measurement interference in the laser point cloud, with coordinates such as NAN / Inf. Because they severely interfere with some geometric features, a preprocessing step of invalid point detection and removal is incorporated to avoid affecting subsequent point cloud data. This processing method is existing technology.

[0082] To avoid calculating the changes in position, velocity, and rotation at time j during iterative calculations and thus reduce computational load, after acquiring the original point cloud from the lidar and the high-frequency signal from the inertial measurement unit (IMU), a pre-integration calculation is also included for the high-frequency signal. The calculation formula is as follows:

[0083]

[0084]

[0085]

[0086] In the formula, Δp ij Δv ij q ij Let i represent the changes in position, velocity, and rotation from the previous time i to the current time j, respectively; t∈(i,j), with symbols... q represents the multiplication of quaternions.i q j Let i and j be the rotation amounts at the previous time i and the current time j, respectively. b is the measured value of angular velocity. gk b ak η represents the bias error of the gyroscope and accelerometer, respectively. gk η ak These represent the measurement noise of the gyroscope and accelerometer, respectively; ΔR ik The pre-integrated ideal value of the previous time i has bias error and noise compared to the measured value; Δv ik Δt represents the ideal velocity value at the previous moment i, and the error exists between it and the measured velocity value; Δt is the time interval. This represents the acceleration of the IMU in the world coordinate system.

[0087] The point cloud segmentation includes the following steps:

[0088] Ground point cloud segmentation: The original point cloud image after preprocessing is quickly segmented into ground point cloud and target point cloud using a ground segmentation algorithm, and the target point cloud is extracted. Feature extraction and matching are performed only on the target point cloud, which significantly reduces computation and improves algorithm efficiency and real-time performance.

[0089] Point cloud clustering and segmentation: Clustering and segmentation algorithms are used to cluster and segment the target point cloud after the ground point cloud segmentation and extraction.

[0090] By segmenting and clustering ground point clouds, the original point cloud can be converted into a target point cloud containing only obstacles. The system only extracts and matches features from the target point cloud, which can significantly reduce the amount of computation.

[0091] like Figure 2 As shown, the steps for segmenting the ground point cloud and the target point cloud are as follows:

[0092] Angle θ is determined using a ground segmentation algorithm: OK and OM are two adjacent laser lines emitted by the lidar, with corresponding depths R and R, respectively. r and R r-1 θ is the angle between KM and MN, β i β j Let θ be the angle between the lidar beam and the x-axis. The angle θ is calculated as follows:

[0093]

[0094] Introduce a height threshold h that varies based on the installation height of the lidar. pc To filter out small obstacles close to the ground:

[0095] h pc =0.15h L

[0096] Among them, h L This represents the installation height of the radar;

[0097] Distinguish and segment the target point cloud from the ground point cloud: when the angle θ is less than 15 degrees and the height of the point cloud at points M and K is greater than the height threshold h. pc If the angle θ is greater than 15 degrees, it is determined to be a target point cloud; otherwise, it is a ground point cloud.

[0098] Research shows that the number of laser points and laser beams detected differs at a distance of approximately 50 meters from the lidar. The farther the object is from the lidar, the fewer laser points and laser beams are detected. To accurately segment point clouds of targets at different distances in a large scene, a dynamic clustering threshold is introduced into the point cloud clustering segmentation process.

[0099]

[0100] Where, N p N represents the number of cluster points. l Let d represent the number of scanning laser beams in the vertical direction, and d represent the horizontal distance between the radar and the laser scanning point. When the distance d is greater than 50 meters, a point cloud cluster is segmented into a single target point cloud if the number of clusters is greater than 30 and the number of scanning laser beams in the vertical direction is greater than 3. When the distance d is less than 50 meters, the condition for segmenting the target point cloud is that the number of clusters is greater than 50 and the number of scanning laser beams in the vertical direction is greater than 5. This facilitates subsequent feature extraction and registration of the target point cloud, reducing computational load.

[0101] This invention improves the registration accuracy of point clouds by providing motion constraints through an IMU.

[0102] The target point cloud is corrected for point cloud distortion using high-frequency signals generated by the IMU to complete motion distortion compensation; including the following steps:

[0103] Simultaneously receive and store lidar data and high-frequency signals from the IMU;

[0104] Calculate the time for each sampling point based on the current point cloud timestamp and the LiDAR sampling frequency;

[0105] Based on the current timestamp, search for adjacent IMU attitude data in the storage queue, and estimate the current laser point attitude through spherical linear interpolation;

[0106] Transform the pose of all point clouds from the start to the end of the scan to the pose at the end of the scan.

[0107] Specifically, linear interpolation is performed on the pre-integrated pose of the IMU at the start and end times of point cloud scanning to calculate the pose at the corresponding time within a frame of point cloud; let L k-1and L k+1 The translation vectors at time points are T. k-1 T k+1 The attitude vectors are P k-1 P k+1 L j-1 and L j+1 The translation vectors at time points are T. j-1 T j+1 The attitude vectors are P j-1 P j+1 ;

[0108] The position interpolation formula is as follows:

[0109]

[0110] The formula for spherical linear interpolation is as follows:

[0111] P k =a(t)P k-1 +b(t)P k+1

[0112]

[0113] In the formula, P k It is the attitude vector to be obtained, P k-1 and P k+1 IMU in L j-1 and L j+1 The pose at time t, a(t) and b(t) are two coefficient variables that change with respect to time t.

[0114] Receiving LiDAR data and IMU high-frequency signals can also be completed in the same step as "acquiring the original point cloud collected by LiDAR and the high-frequency signals collected by the inertial measurement unit (IMU). The high-frequency signals from the IMU include triaxial acceleration data, triaxial angular velocity data, etc."

[0115] To achieve rapid and accurate point cloud registration, after correcting the point cloud distortion of the target point cloud, the process further includes filtering out non-feature point clouds from the feature point cloud that has undergone motion distortion compensation, and distinguishing between planar features and edge features to reduce data computation and improve computational efficiency. The specific steps are as follows:

[0116] Remove non-feature point clouds: feature points when the laser beam and the object surface are nearly parallel and feature points in the occluded parts are regarded as unreliable point clouds and are not included in the feature sequence;

[0117] Distinguishing between planar features and edge features: by analyzing the point cloud roughness c l kSetting thresholds to distinguish between planar features and edge features allows for the extraction of different types of structural information from point cloud data, facilitating better point cloud matching and calculation.

[0118] Point cloud roughness The formula is:

[0119]

[0120] in, Let k be the set of adjacent points on the same scan line at time k; The coordinates of the point cloud in the current frame of the lidar reference system. To and Point clouds that are adjacent on the same scan line; when the point cloud roughness When the point cloud roughness exceeds a threshold, it is classified as an edge feature; when the point cloud roughness... If the value is less than the threshold, it is classified as a planar feature.

[0121] The purpose of classifying the information within a point cloud frame into planar features and edge features is to extract different types of structural information from the point cloud data, such as sets of edge feature points and sets of planar feature points, and to match them between the point clouds of the previous frame and the current frame, thereby improving point cloud matching calculations and significantly enhancing subsequent system performance.

[0122] The Scan-Context descriptor is introduced for loop closure detection, including the following steps:

[0123] The Scan-Context descriptor is introduced for loop closure detection, and the average of the three adjacent maximum height values ​​in each region is selected as the value of the feature matrix.

[0124] If the distance between a scan context pair meets the threshold condition, the next step is to perform a verification by finding a matching loop closure frame based on Euclidean distance.

[0125] If the query point cloud and the candidate point cloud successfully match the loop closure based on Euclidean distance, then the loop closure constraint is added to the nonlinear optimization function to obtain the minimum error.

[0126] By iteratively optimizing the nonlinear error function, an accurate state estimate is obtained, leading to accurate trajectory estimation and map update results. The construction of a jointly optimized error function to accurately solve for radar pose includes the following steps:

[0127] Optimize inter-frame matching error: Construct a point cloud registration error between two frames. To improve the real-time performance of the algorithm, keyframe features are incorporated into the optimization function:

[0128] E (Lk,N) =d(l,v) +d (p,u)

[0129] In the formula, d (l,v) Let d be the distance from the point to the line. (p,u) The distance from the point to the plane is used as the observed value of the keyframe registration error; N is the sum of the number of feature points extracted from the point cloud of a certain keyframe.

[0130] Optimizing IMU Error: Constructing the IMU Error Function To improve positioning accuracy. IMU error comes from pre-integration pose error, gyroscope and accelerometer bias error.

[0131]

[0132] In the formula, It is the IMU pose error in the radar coordinate system from time k-1 to time k; b is the pose of the IMU at time k; gk b ak η represents the bias error of the gyroscope and accelerometer, respectively; gk η ak These represent the measurement noise of the gyroscope and accelerometer, respectively.

[0133] Optimize loop closure error; construct loop closure error to improve positioning accuracy. Loop closure error arises from inaccurate matching of feature extraction and descriptors, as well as feature point occlusion or position changes caused by environmental variations.

[0134] To improve positioning accuracy, a nonlinear cost function is constructed based on the aforementioned errors, including lidar constraints, IMU measurement constraints, and loop closure constraints. The Gauss-Newton nonlinear optimization algorithm is then used to solve for the optimal state that minimizes the error function.

[0135] To better understand the above technical solutions, the following will provide a detailed explanation of the technical solutions in conjunction with the accompanying drawings and specific implementation methods.

[0136] Figure 4This image shows a trajectory comparison between the present invention and LeGO-LOAM on the KITTI dataset. The trajectory and pose accuracy of the improved algorithm are evaluated against the mainstream LeGO-LOAM scheme using the KITTI dataset. LeGO-LOAM is a lightweight, ground-optimized LiDAR SLAM algorithm. The KITTI dataset, jointly created by the Karlsruhe Institute of Technology in Germany and Toyota Research Institute of America, is currently the largest international dataset for evaluating computer vision algorithms in autonomous driving scenarios. Simulation results show that, while meeting the real-time requirements of odometry, the improved laser odometry accuracy is 42% higher than the LeGO-LOAM scheme, thus proving that the present invention approximates the real trajectory more closely than the LeGO-LOAM algorithm.

[0137] A computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the 3D LiDAR localization and mapping method fused with an IMU. It includes a front-end data processing module and a back-end optimization module.

[0138] The front-end data processing flow includes:

[0139] (1) Data preprocessing module. After receiving the raw point cloud generated by the lidar and the high-frequency signal output by the IMU, including angular velocity and acceleration information, the system projects the raw point cloud to generate an image with depth information.

[0140] (2) Point Cloud Segmentation Module. This module comprises three parts: point cloud preprocessing, ground point cloud segmentation, and point cloud clustering segmentation. The point cloud segmentation module quickly extracts non-ground point clouds and uses a clustering segmentation algorithm to remove invalid points from the target point cloud. To filter out small obstacles close to the ground, this invention introduces a height threshold that varies based on the radar installation height, such as... Figure 2 As shown.

[0141] (3) Point Cloud Distortion Correction Module. High-frequency signals acquired by the IMU are used to remove point cloud distortion, completing motion distortion compensation. The motion distortion correction steps of the fused IMU and LiDAR are as follows: First, simultaneously receive and save the high-frequency attitude angle data from both the LiDAR and IMU. Second, based on the current point cloud timestamp and the LiDAR sampling frequency, the time of each sampling point can be calculated. Finally, based on the current timestamp, search for adjacent IMU attitude data in the storage queue, estimate the current LiDAR point attitude using spherical linear interpolation, and ultimately transform the attitude of all point clouds from the start to the end of the scan to the state at the end of the scan. The interpolation method is as follows: Figure 3 As shown.

[0142] (4) Feature Point Extraction Module. By calculating the curvature of the point cloud, line and surface features are extracted and added to the local feature map. Feature points at consecutive time points are registered to estimate the relative motion information of the LiDAR, providing information for the next feature point calibration. Before feature extraction from the distortion-free LiDAR point cloud, feature points when the laser beam and the object surface are nearly parallel and feature points in occluded parts are considered unreliable point clouds and are not included in the feature sequence.

[0143] The backend optimization module includes:

[0144] (1) Loop Closure Detection Module. Calculating the feature similarity of a depth image frame involves two steps: first, calculating the Euclidean distance score, and then calculating the spatial distance score of the Scan Context feature descriptor. Combining these two scores with the time interval, a threshold is used to determine whether a loop closure has occurred. If the query point cloud and candidate point cloud successfully match a loop closure based on Euclidean distance, the loop closure constraint is added to the nonlinear optimization function to obtain the minimum error.

[0145] (2) Backend Optimization Module. The system receives the current feature frame, pose estimation, and other information to perform global consistency optimization, outputting the optimized pose and generating the final trajectory and a globally consistent map. Three error functions need to be jointly optimized: inter-frame matching error, IMU error, and loop closure error. Inter-frame matching error is the error generated during the matching process of feature points in consecutive keyframes; IMU error comes from pose error due to pre-integration, and bias errors from the gyroscope and accelerometer; loop closure error comes from inaccurate matching of feature extraction and descriptors, as well as feature point occlusion or position changes caused by environmental changes. Since high-precision IMU data can compensate for point cloud registration errors, an IMU sensor is introduced. The IMU radar pose is used as the initial value for inter-frame registration, and a jointly optimized error function is constructed to accurately solve for the radar pose.

[0146] This invention presents a 3D LiDAR localization and mapping method integrating IMU (Integrated Measurement Unit). First, the high-frequency pose estimated by the IMU is used to linearly interpolate the laser point cloud, thereby correcting motion distortion. Second, a height threshold is introduced in the ground segmentation section to quickly and accurately segment the feature point cloud. Then, point cloud clustering is used to further segment the feature point cloud, and a dynamic clustering threshold is introduced for classification. Third, a Scan-Context descriptor is introduced for loop closure detection. Finally, a local map composed of keyframes is registered with the current frame to obtain the inter-frame matching error. A joint optimization function is then constructed by combining the IMU pre-integration error and loop closure detection constraints to solve for the laser odometry pose. The improved algorithm and the mainstream LeGO-LOAM scheme are compared using the KITTI dataset to evaluate trajectory and pose accuracy. Experimental results show that, while meeting the real-time requirements of odometry, the improved laser odometry accuracy is 42% higher than the LeGO-LOAM scheme and more closely approximates the actual trajectory.

[0147] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0148] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A three-dimensional lidar localization and mapping method integrating IMU, characterized in that, Includes the following steps: Acquire the raw point cloud data collected by the lidar and the high-frequency signal collected by the inertial measurement unit (IMU); The original point cloud data is preprocessed and segmented; the point cloud segmentation includes ground point cloud segmentation and point cloud clustering segmentation, so as to convert the original point cloud into a target point cloud containing only obstacles. Performing point cloud distortion correction on the target point cloud to obtain a feature point cloud with motion distortion compensation includes the following steps: Simultaneously receive and store lidar data and high-frequency signals from the IMU; Calculate the time for each sampling point based on the current point cloud timestamp and the LiDAR sampling frequency; Based on the current timestamp, search for adjacent IMU attitude data in the storage queue, and estimate the current laser point attitude through spherical linear interpolation; Transform the pose of all point clouds from the start to the end of the scan to the pose at the end of the scan. Specifically, linear interpolation is performed on the pre-integrated pose of the IMU at the start and end times of point cloud scanning to calculate the pose at the corresponding time within a frame of point cloud; assuming and The translation vectors at different times are respectively , The attitude vectors are respectively , ; and The translation vectors at different times are respectively , The attitude vectors are respectively , ; The position interpolation formula is as follows: The formula for spherical linear interpolation is as follows: In the formula, It is the attitude vector that needs to be obtained. and IMU in and Position at any given moment and These are two coefficient variables that change according to time t; By calculating the curvature of the feature point cloud, line and surface features are extracted and added to the local feature map, and feature points at consecutive time points are registered. Non-feature point clouds are filtered out from the feature point cloud after motion distortion compensation, and planar features and edge features are distinguished to reduce the amount of data processing and improve computational efficiency; the specific steps are as follows: Remove non-feature point clouds: feature points when the laser beam and the object surface are nearly parallel and feature points in the occluded parts are regarded as unreliable point clouds and are not included in the feature sequence; Distinguishing between planar features and edge features: by analyzing point cloud roughness Setting thresholds to distinguish between planar features and edge features allows for the extraction of different types of structural information from point cloud data, facilitating better point cloud matching and calculation. Point cloud roughness The formula is: in, Let k be the set of adjacent points on the same scan line at time k; The coordinates of the point cloud in the current frame of the lidar reference system. To and Point clouds that are adjacent on the same scan line; when the point cloud roughness When the point cloud roughness exceeds a threshold, it is classified as an edge feature; when the point cloud roughness... When the value is less than the threshold, it is classified as a planar feature; Introducing the Scan-Context descriptor for loop closure detection; Distortion removal of the point cloud is performed using IMU state prediction. The processed radar pose is used as the initial value for inter-frame registration. A jointly optimized error function is constructed to accurately solve the radar pose, and the final trajectory and globally consistent map are generated. The construction of the jointly optimized error function to accurately solve the radar pose includes the following steps: Optimize inter-frame matching error: Construct a point cloud registration error between two frames. To improve the real-time performance of the algorithm, the features of key frames are incorporated into the optimization function; In the formula, Let be the distance from the point to the line. The distance from the point to the plane. The sum of the number of feature points extracted from the point cloud of a certain keyframe; Optimizing IMU Error: Constructing the IMU Error Function To improve positioning accuracy; In the formula, yes Time's up IMU pose error in the radar coordinate system at any given time; for The pose of the IMU at any given moment; , These represent the bias errors of the gyroscope and accelerometer, respectively. , These represent the measurement noise of the gyroscope and accelerometer, respectively. Optimize loop closure error; construct loop closure error to improve positioning accuracy; A nonlinear cost function is constructed for lidar constraints, IMU measurement constraints, and loop closure constraints. The optimal state that minimizes the error function is then solved using the Gauss-Newton nonlinear optimization algorithm.

2. The three-dimensional lidar positioning and mapping method with fused IMU according to claim 1, characterized in that, After acquiring the raw point cloud from the lidar and the high-frequency signal from the inertial measurement unit (IMU), the process further includes pre-integration calculation of the high-frequency signal, using the following formula: In the formula, , , Each at the previous moment up to the current moment Position, velocity, and rotational changes; ,symbol This represents the multiplication of quaternions. , Each is the previous moment and the current moment The amount of rotation, This is the measured value of angular velocity. , These represent the bias errors of the gyroscope and accelerometer, respectively. , These represent the measurement noise of the gyroscope and accelerometer, respectively. For the previous moment The pre-integrated ideal value has bias error and noise compared to the measured value; For the previous moment There is an error between the ideal speed value and the measured speed value. For time intervals; This represents the acceleration of the IMU in the world coordinate system.

3. The three-dimensional lidar positioning and mapping method with fused IMU according to claim 1, characterized in that, The point cloud segmentation includes the following steps: Ground point cloud segmentation: The original point cloud image after point cloud preprocessing is quickly segmented into ground point cloud and target point cloud using a ground segmentation algorithm, and the target point cloud is extracted; Point cloud clustering and segmentation: Clustering and segmentation algorithms are used to cluster and segment the target point cloud after the ground point cloud segmentation and extraction.

4. The three-dimensional lidar positioning and mapping method with fused IMU according to claim 3, characterized in that, The steps for segmenting the ground point cloud and the target point cloud are as follows: Determining angles using ground segmentation algorithms OK and OM are two adjacent laser lines emitted by the lidar, corresponding to depths of respectively. and ; It is the angle between KM and MN. , The angle between the lidar beam and the x-axis is [angle]. The calculation method is as follows: Introduce a height threshold that varies based on the installation height of the lidar. To filter out small obstacles close to the ground: in, This represents the installation height of the radar; Distinguish and segment the target point cloud and the ground point cloud: when the angle The angle is less than 15 degrees and the point cloud heights at points M and K are greater than the height threshold. or angle If the angle is greater than 15 degrees, it is determined to be a target point cloud; otherwise, it is a ground point cloud.

5. The three-dimensional lidar positioning and mapping method with fused IMU according to claim 3, characterized in that, A dynamic clustering threshold is introduced in the point cloud clustering segmentation: in, For the number of cluster points, d represents the number of scanning laser beams in the vertical direction, and d represents the horizontal distance between the radar and the laser scanning point.

6. The three-dimensional lidar positioning and mapping method with fused IMU according to claim 1, characterized in that, The Scan-Context descriptor is introduced for loop closure detection, including the following steps: The Scan-Context descriptor is introduced for loop closure detection, and the average of the three adjacent maximum height values ​​in each region is selected as the value of the feature matrix. If the distance between a scan context pair meets the threshold condition, the next step is to perform a verification by finding a matching loop closure frame based on Euclidean distance. If the query point cloud and the candidate point cloud successfully match the loop closure based on Euclidean distance, then the loop closure constraint is added to the nonlinear optimization function to obtain the minimum error.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the three-dimensional lidar positioning and mapping method with fused IMU as described in any one of claims 1 to 6.

Images