LiDAR-IMU odometer method and device suitable for degraded scene
By adopting pre-integration processing and adaptive distance threshold methods in the LiDAR-IMU odometer system, the problem of poor point cloud matching in the degraded scenario is solved, and higher positioning accuracy and robustness are achieved.
Patent Information
- Application Number
- CN202510358176.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-25
- Publication Date
- 2025-06-27
Smart Images
Figure CN120213018A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of data fusion between lidar and inertial measurement unit, and particularly relates to a LiDAR-IMU odometry method and device adapted to degraded scenarios. Background Technique
[0002] As a core sensor for high-precision positioning and environmental perception, LiDAR is widely used in fields such as unmanned driving, mobile robots, and drones. With its excellent spatial resolution and accurate ranging ability, LiDAR can provide rich environmental information. However, in some geometrically degraded scenarios (such as long straight corridors, flat open areas, or repetitive texture environments), the point cloud data of LiDAR may have deficiencies or be too single in terms of feature distribution, resulting in poor point cloud matching effects and further affecting the positioning accuracy of the system. In addition, problems such as noise and occlusion in the environment will further weaken the performance of the lidar system.
[0003] To alleviate these problems, several solutions have been proposed by researchers:
[0004] (1) Enhancement strategy based on filtering methods: This strategy reduces invalid information by performing noise filtering or downsampling on the lidar point cloud data, thereby improving the point cloud registration accuracy. Such methods mainly optimize the matching process by removing irrelevant noise points and retaining effective environmental information.
[0005] (2) Multi-sensor fusion: To improve the robustness of the system in complex environments, many solutions use other sensors such as IMU and GNSS (Global Navigation Satellite System) to fuse with lidar data. By combining multi-source information, the deficiencies of a single sensor can be compensated, and the adaptability and positioning accuracy of the system to different scenarios can be improved.
[0006] (3) Optimization algorithm improvement: Optimization algorithms, especially point cloud registration methods introducing robust kernel functions, improve the performance of the system in degraded scenarios by optimizing the error of the point cloud during the matching process. Optimization algorithms help reduce the impact of false matches and improve the stability of the system.
[0007] Although these methods have improved the performance of the LiDAR system to a certain extent, they still have some limitations, especially in terms of insufficient performance in degraded scenarios:
[0008] (1) Loss of feature information due to noise filtering: Although noise filtering can reduce invalid point cloud data, excessive noise filtering may lead to the loss of useful feature information, especially in low-feature environments, which will affect the accuracy of point cloud registration.
[0009] (2) High cost and complexity of high-precision sensor fusion: Sensor fusion methods usually require high-precision calibration and additional hardware support, increasing the complexity of the system and the hardware cost. In addition, the inconsistency or error between sensors will affect the fusion result, and thus affect the final positioning accuracy.
[0010] (3) Initial pose dependence of optimization algorithms: Many optimization algorithms are highly dependent on the initial pose and are prone to falling into local optimal solutions. Especially in complex environments, this will affect the robustness and accuracy of the system.
[0011] (4) Limitations of the fixed threshold method: In the existing technology, many methods use a fixed threshold to eliminate the mismatched points in the point cloud. However, the fixed threshold ignores the influence of the distance and pose change from the point to the LiDAR on the matching error. In the case of long-distance points, the fixed threshold may wrongly eliminate valid matching points; while in the case of short-distance points, it may miss eliminating invalid matching points, thus reducing the accuracy of point cloud matching.
[0012] (5) Limitations of the optimization objective function: The optimization objective functions in the existing technology usually only rely on the geometric features of the point cloud or simply combine the IMU and point cloud data, but fail to fully balance the weights of the two. In a degraded scenario, the geometric features of the point cloud may be sparse, and at this time, the optimization objective that solely relies on point cloud information is likely to fail. In addition, the optimization objective function with fixed weights cannot adapt to the uncertainty changes of sensor data and lacks the ability to adapt to dynamic environments.
[0013] In summary, although the existing technology has made contributions to improving the robustness and accuracy of lidar systems, there are still many limitations in complex or degraded environments. Therefore, it is urgent to propose a LiDAR-IMU odometry method and device adapted to degraded scenarios. Summary of the Invention
[0014] To solve the above technical problems, the present invention proposes a LiDAR-IMU odometry method and device adapted to degraded scenarios, which improves the robustness of point cloud matching in degraded scenarios and reduces the influence of mismatches on pose estimation.
[0015] On the one hand, to achieve the above object, the present invention provides a LiDAR-IMU odometry method adapted to degraded scenarios, including:
[0016] Obtain the original point cloud data and the acceleration and angular velocity values;
[0017] Perform pre-integration processing on the acceleration and angular velocity values to obtain a pre-integration result;
[0018] Combine the pre-integration result with the original point cloud data for point cloud preprocessing to obtain a sparse point cloud;
[0019] Calculate an initial estimate of the current frame pose based on the pre-integration result;
[0020] According to the initial estimate of the current frame pose, perform point cloud registration on the sparse point cloud to obtain the optimal estimated value of the current frame pose;
[0021] Use the optimal estimate of the current frame pose to transform the sparse point cloud into the global coordinate system, add the transformed sparse point cloud to the global map, and update the global map.
[0022] Optionally, the pre-integration result is combined with the original point cloud data for point cloud preprocessing to obtain a sparse point cloud, including:
[0023] Convert the reference coordinate system of the original point cloud to the same coordinate system according to the IMU pre-integration result;
[0024] Adopt voxel filtering to downsample the point cloud data converted to the same coordinate system to obtain a sparse point cloud.
[0025] Optionally, converting the reference coordinate system of the original point cloud to the same coordinate system according to the IMU pre-integration result includes:
[0026]
[0027] Wherein, is the coordinate of the point cloud at time t in the LiDAR coordinate system, i is the external parameter matrix between LiDAR and IMU, is the result of IMU pre-integration at time t is t i is the result of IMU pre-integration at time t
[0028] Optionally, calculating an initial estimate of the current frame pose based on the pre-integration result includes:
[0029] Calculate the initial estimate of the current frame pose by superimposing the pre-integration result and the optimal estimated value of the previous frame pose.
[0030] Optionally, the method for calculating the initial estimate of the current frame pose includes:
[0031]
[0032] Wherein, is the pre-integration result of the previous frame pose; is the optimal estimated value of the previous frame pose; is the initial estimate of the current frame pose.
[0033] Optionally, according to the initial estimate of the current frame pose, performing point cloud registration on the sparse point cloud to obtain the optimal estimated value of the current frame pose includes:
[0034] Using the kd-tree algorithm to obtain the nearest neighbor points of the point cloud of the current frame in the local point cloud map, and using an adaptive distance threshold to eliminate invalid matching points;
[0035] Setting an optimization objective function to optimize the matching result to obtain the optimal estimated value of the current frame pose.
[0036] Optionally, using the optimal estimate of the current frame pose to transform the sparse point cloud into the global coordinate system and adding the transformed sparse point cloud to the global map includes:
[0037] Using the optimal estimated value of the current frame pose to transform the sparse point cloud into the global coordinate system;
[0038] Fusing the transformed point cloud with the global point cloud map to obtain an updated global point cloud map.
[0039] On the other hand, to achieve the above object, the present invention also provides a LiDAR-IMU odometer device adapted to a degraded scenario, including: a data acquisition module, a pre-integration processing module, a point cloud preprocessing module, an initial pose estimation module, a point cloud registration module, and a global map update module;
[0040] The data acquisition module is used to acquire the original point cloud data and the acceleration and angular velocity values;
[0041] The pre-integration processing module is used to perform pre-integration processing on the acceleration and angular velocity values to obtain a pre-integration result;
[0042] The point cloud preprocessing module is used to perform point cloud preprocessing on the pre-integration result in combination with the original point cloud data to obtain a sparse point cloud;
[0043] The initial pose estimation module is used to calculate the initial estimate of the current frame pose based on the pre-integration result;
[0044] The point cloud registration module is used to perform point cloud registration on the sparse point cloud according to the initial estimate of the current frame pose to obtain the optimal estimated value of the current frame pose;
[0045] The global map update module is used to use the optimal estimate of the current frame pose to transform the sparse point cloud into the global coordinate system, add the transformed sparse point cloud to the global map, and update the global map.
[0046] The technical effects of the present invention:
[0047] (1) High adaptability to geometric degradation scenarios: The strategy of dynamically removing invalid matching points with distance thresholds proposed in the present invention adaptively calculates the removal conditions by combining the distance from points in the point cloud to the LiDAR and the pose change, effectively filtering out the matching errors caused by the amplified relative displacement in the long-distance point cloud. This strategy can significantly improve the accuracy of point cloud matching. Especially in degraded scenarios with sparse geometric features such as long straight corridors or open fields, the robustness of the system is better than that of traditional fixed threshold methods.
[0048] (2) Enhancement of the robustness and adaptability of the optimization objective function: The present invention designs an optimization objective function that balances the IMU constraint and the point cloud geometric constraint. By dynamically adjusting the influence of the two through an adaptive weight matrix, the optimization process takes into account both the dynamic information provided by the IMU and the geometric constraints of the point cloud features. The introduction of a robust kernel function to suppress outliers further improves the robustness of the optimization and reduces the impact of mismatches on pose estimation. This design is particularly suitable for environments with high sensor noise or single point cloud features.
[0049] (3) Improvement of positioning accuracy and mapping integrity: The combination of the adaptive matching removal strategy and the robust optimization objective function significantly improves the accuracy of pose estimation in degraded scenarios, avoiding the positioning drift problem caused by mismatches or noise accumulation. During the global point cloud map update process, the optimized pose is used to efficiently and accurately construct the map, thus ensuring the integrity and consistency of the map in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0050] The drawings forming a part of this application are used to provide a further understanding of this application. The schematic embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation of this application. In the drawings:
[0051] Figure 1 is a schematic flow chart of a LiDAR-IMU odometry method adapted to degraded scenarios according to an embodiment of the present invention;
[0052] Figure 2 is a schematic structural diagram of a LiDAR-IMU odometry device adapted to degraded scenarios according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS
[0053] It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The following will refer to the drawings and combine the embodiments to detail this application.
[0054] It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. And although the logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order than here.
[0055] As Figure 1 shown, in this embodiment, a LiDAR-IMU odometry method adapted to a degraded scenario is provided, including:
[0056] Obtain the original point cloud data and the acceleration and angular velocity values;
[0057] Perform pre-integration processing on the acceleration and angular velocity values to obtain a pre-integration result;
[0058] Combine the pre-integration result with the original point cloud data for point cloud preprocessing to obtain a sparse point cloud;
[0059] Calculate an initial estimate of the current frame pose based on the pre-integration result;
[0060] According to the initial estimate of the current frame pose, perform point cloud registration on the sparse point cloud to obtain the optimal estimated value of the current frame pose;
[0061] Use the optimal estimate of the current frame pose to transform the sparse point cloud into the global coordinate system, add the transformed sparse point cloud to the global map, and update the global map.
[0062] Further, combining the pre-integration result with the original point cloud data for point cloud preprocessing to obtain a sparse point cloud includes:
[0063] Convert the reference coordinate system of the original point cloud to the same coordinate system according to the IMU pre-integration result;
[0064] Adopt voxel filtering to perform downsampling processing on the original point cloud data converted to the same coordinate system to obtain a sparse point cloud.
[0065] Specifically, point cloud preprocessing includes point cloud de-distortion and point cloud downsampling. Point cloud de-distortion uses the result of IMU pre-integration to convert the reference coordinate system of the original point cloud to the same coordinate system. Point cloud downsampling adopts the method of voxel filtering (voxel filtering is a commonly used downsampling technique, which reduces the point cloud density to reduce the computational complexity by dividing the point cloud space into individual volume units and only retaining one representative point in each volume unit).
[0066] Further, converting the reference coordinate system of the original point cloud to the same coordinate system according to the IMU pre-integration result includes:
[0067]
[0068] Among them, is the coordinate of the point cloud at time t in the LiDAR coordinate system, i and is the external parameter matrix between the LiDAR and the IMU, is i the result of IMU pre-integration at time t.
[0069] Furthermore, calculating the initial estimate of the current frame pose based on the pre-integration result includes:
[0070] Calculating the initial estimate of the current frame pose by superimposing the pre-integration result and the optimal estimated value of the previous frame pose.
[0071] Furthermore, the method for calculating the initial estimate of the current frame pose includes:
[0072]
[0073] Among them, is the pre-integration result of the previous frame pose; is the optimal estimated value of the previous frame pose; is the initial estimate of the current frame pose.
[0074] Among them, the optimal estimated value of the previous frame pose and the pose increment obtained by IMU pre-integration are represented in matrix form:
[0075]
[0076] Furthermore, according to the initial estimate of the current frame pose, performing point cloud registration on the sparse point cloud to obtain the optimal estimated value of the current frame pose includes:
[0077] Using the kd-tree algorithm to process the point cloud of the current frame, obtaining the nearest neighbor points in the local point cloud map, and removing invalid matching points using an adaptive distance threshold;
[0078] Setting an optimization objective function to optimize the matching result to obtain the optimal estimated value of the current frame pose.
[0079] Specifically, point cloud registration is achieved through the loop of point cloud matching and point cloud registration to obtain the optimal estimated value of the current frame pose. Point cloud matching is realized by using the kd-tree algorithm to obtain the nearest neighbor points in the local point cloud map. To address the problem of false matches during point cloud matching, an adaptive distance threshold is used to eliminate invalid matching points, improving the accuracy of point cloud matching. Point cloud registration is achieved by solving an optimization problem that includes a pose error term and a point cloud matching error term. The pose error term is the difference between the initial estimated value of the current frame pose and the pose estimated value of the optimization problem.
[0080] Register the sparse point cloud, perform point cloud matching and point cloud registration iteratively, and obtain the optimal estimated value of the current frame pose. During point cloud matching, use the kd-tree algorithm to process the point cloud of the current frame to find the nearest neighbor points in the local point cloud map To address the problem of false matches during point cloud matching, an adaptive distance threshold is used to eliminate invalid matching points, improving the precision of point cloud matching. The condition for judging an invalid matching point is:
[0081]
[0082] where ∈ i is the dynamic distance threshold, which is calculated by the following formula:
[0083]
[0084] where is the logarithmic operation that maps the rotation matrix to the skew-symmetric matrix, mapping the skew-symmetric matrix to the attitude angle. Point cloud registration is achieved by solving the following optimization problem:
[0085]
[0086] where the optimization objective function includes a pose error term and a point cloud matching error term, which are respectively:
[0087]
[0088] The weight matrix W is expressed as:
[0089]
[0090] where is the covariance matrix of the pose obtained by IMU pre-integration, P rThe residual covariance matrix obtained from the point cloud geometric constraint. By fusing the two, it is ensured that the IMU prior information and the point cloud geometric constraint are considered in a balanced manner. ρ(·) is a robust kernel function, and the Huber kernel function can be used. The above optimization problem is solved by an iterative method until one of the iterative exit conditions is met: reaching the maximum number of iterations (considering the real-time requirement of LiDAR-IMU odometry, the maximum number of iterations is generally set to 20); the norm of the state quantity update value is less than the set threshold (setting it to 0.05 can meet the convergence requirement). Solving the optimization problem gives the optimal estimated value T of the current frame pose ★ 。
[0091] Further, based on the optimal estimated value of the current frame pose and combined with the vehicle pose, updating the global map includes:
[0092] Using the optimal estimated value of the current frame pose to transform the sparse point cloud into the global coordinate system;
[0093] Fusing the transformed point cloud with the global point cloud map to obtain an updated global point cloud map.
[0094] As Figure 2 shown, in this embodiment, a LiDAR-IMU odometry device adapted to a degraded scenario is also provided, including: a data acquisition module, a pre-integration processing module, a point cloud preprocessing module, an initial pose estimation module, a point cloud registration module, and a global map update module;
[0095] The data acquisition module is used to acquire the original point cloud data and the acceleration and angular velocity values;
[0096] The pre-integration processing module is used to perform pre-integration processing on the acceleration and angular velocity values to obtain a pre-integration result;
[0097] The point cloud preprocessing module is used to perform point cloud preprocessing on the pre-integration result combined with the original point cloud data to obtain a sparse point cloud;
[0098] The initial pose estimation module is used to calculate the initial estimate of the current frame pose based on the pre-integration result;
[0099] The point cloud registration module is used to perform point cloud registration on the sparse point cloud according to the initial estimate of the current frame pose to obtain the optimal estimated value of the current frame pose;
[0100] The global map update module is used to update the global map according to the optimal estimate of the current frame pose combined with the vehicle pose.
[0101] The above are only the preferred specific embodiments of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.
Claims
1. A LiDAR-IMU odometer method adapted to degraded scenarios, characterized in that: include: Get the original point cloud data as well as the acceleration and angular velocity values; Performing pre-integration processing on the acceleration and the angular velocity value to obtain a pre-integration result; The pre-integration result is combined with the original point cloud data to perform point cloud preprocessing to obtain a sparse point cloud; Calculating an initial estimate of the current frame pose based on the pre-integration result; Performing point cloud registration on the sparse point cloud according to an initial estimate of the current frame pose to obtain an optimal estimate of the current frame pose; The sparse point cloud is transformed into a global coordinate system using an optimal estimate of the current frame pose, the transformed sparse point cloud is added to the global map, and the global map is updated.
2. The LiDAR-IMU odometer method adapted to degraded scenarios according to claim 1, characterized in that: The pre-integration result is combined with the original point cloud data to perform point cloud preprocessing to obtain a sparse point cloud, which includes: The reference coordinate system of the original point cloud is converted to the same coordinate system according to the IMU pre-integration result; Voxel filtering is used to downsample the point cloud data converted to the same coordinate system to obtain a sparse point cloud.
3. The LiDAR-IMU odometer method adapted to degraded scenarios according to claim 2, characterized in that: Converting the reference coordinate system of the original point cloud to the same coordinate system according to the IMU pre-integration result includes: in, t i The coordinates of the point cloud at the moment in the LiDAR coordinate system, is the external parameter matrix between LiDAR and IMU, t i The result of IMU pre-integration at this moment.
4. The LiDAR-IMU odometer method adapted to degraded scenarios according to claim 1, characterized in that: Calculating an initial estimate of the current frame pose based on the pre-integration result includes: The initial estimate of the current frame pose is calculated by superimposing the pre-integration result and the optimal estimate of the previous frame pose.
5. The LiDAR-IMU mileage calculation method adapted to degraded scenarios according to claim 1, characterized in that: The method for calculating the initial estimate of the current frame pose includes: in, is the pre-integration result of the previous frame pose; is the optimal estimated value of the pose of the previous frame; is the initial estimate of the pose of the current frame.
6. The LiDAR-IMU odometer method adapted to degraded scenarios according to claim 1, characterized in that: According to the initial estimation of the current frame pose, the sparse point cloud is registered to obtain the optimal estimation value of the current frame pose, including: The kd-tree algorithm is used to obtain the nearest neighbor points of the point cloud of the current frame in the local point cloud map, and the adaptive distance threshold is used to eliminate invalid matching points; Set the optimization objective function, optimize the matching results, and obtain the optimal estimate of the current frame pose.
7. The LiDAR-IMU odometer method adapted to degraded scenarios according to claim 1, characterized in that: The sparse point cloud is transformed into a global coordinate system using an optimal estimate of the current frame pose, and the transformed sparse point cloud is added to the global map, comprising: Converting the sparse point cloud to a global coordinate system using the best estimate of the current frame pose; The converted point cloud is fused with the global point cloud map to obtain an updated global point cloud map.
8. A device for the LiDAR-IMU odometer method adapted to degraded scenarios according to any one of claims 1 to 7, characterized in that: include: Data acquisition module, pre-integration processing module, point cloud pre-processing module, initial pose estimation module, point cloud registration module and global map update module; The data acquisition module is used to acquire original point cloud data and acceleration and angular velocity values; The pre-integration processing module is used to perform pre-integration processing on the acceleration and the angular velocity value to obtain a pre-integration result; The point cloud preprocessing module is used to perform point cloud preprocessing on the pre-integration result in combination with the original point cloud data to obtain a sparse point cloud; The initial pose estimation module is used to calculate an initial estimate of the current frame pose based on the pre-integration result; The point cloud registration module is used to perform point cloud registration on the sparse point cloud according to the initial estimation of the current frame pose to obtain the optimal estimation value of the current frame pose; The global map updating module is used to transform the sparse point cloud into a global coordinate system using the optimal estimate of the current frame pose, add the transformed sparse point cloud to the global map, and update the global map.