A method for filtering out dynamic targets and noise in a real-time laser SLAM system

Abstract

The application discloses a kind of filtering of dynamic target and noise in real-time laser SLAM system, comprising: sensor data time hard synchronization;Initial map construction;Single frame point cloud motion rectification based on IMU;Single frame point cloud line feature and surface feature extraction;Construct the joint optimization objective function of line feature residual and surface feature residual, and solve optimization pose by nonlinear least square method;Variable size voxel map is constructed and the time statistics and position statistics of point cloud in voxel are maintained;Dynamic target time domain determination is carried out based on voxel time complexity, dynamic target spatial domain ghost detection is carried out based on the linearity index of covariance matrix eigenvalue, and dynamic target rejection of time-space double verification is realized.The application realizes comprehensive and accurate removal of dynamic target and multiple types of noise by fusing time characteristics, spatial geometric features and statistical analysis processing, and significantly improves the mapping accuracy and real-time performance of laser SLAM system in dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of laser point cloud data processing technology, and in particular to a method for filtering dynamic targets and noise in a real-time laser SLAM system. Background Technology

[0002] With the rapid development of autonomous mobile device technology, Simultaneous Localization and Mapping (SLAM) technology has become a core support for robots, autonomous vehicles, and other devices to achieve autonomous navigation. Laser SLAM, with its advantages of high precision and strong stability, is widely used in complex scenarios. However, it is susceptible to various interferences in dynamic environments, and the effectiveness of dynamic target and noise removal directly determines the accuracy of map construction and the reliability of positioning. Currently, existing laser SLAM dynamic target removal technologies mainly fall into three categories, each with significant limitations. Therefore, how to efficiently and accurately remove dynamic targets and various types of noise has become a key challenge for the practical application of laser SLAM technology.

[0003] (I) Technical Path Based on Multi-Sensor Collaborative Detection; This type of method is the core reliance of multi-sensor fusion solutions. The mainstream solution achieves tight coupling and fusion of LiDAR with IMU and vision devices (such as RGB cameras and depth cameras), and uses image semantic segmentation networks (such as Mask R-CNN and YOLO series) to identify dynamic targets (such as pedestrians and vehicles), and then removes the LiDAR point cloud corresponding to the segmented dynamic target area. The main drawbacks of this solution are: First, target judgment is easily affected by environmental factors such as changes in lighting, occlusion, and shadows. In low-light or drastic lighting change scenarios (such as indoor lighting switching or entering a tunnel outdoors), the segmentation accuracy drops sharply; Second, accurate time synchronization and intrinsic and extrinsic parameter calibration between multiple sensors are required. Calibration errors or synchronization deviations will directly lead to misjudgment of dynamic targets. For example, the time offset between vision and LiDAR may cause pedestrian point clouds to be incorrectly projected onto the background wall area; Third, the hardware cost, power consumption, and size of the multi-sensor system increase significantly, posing a substantial obstacle to edge deployment.

[0004] (II) Technical Approach Based on Motion and Geometric Feature Recognition; This type of method mainly determines whether the motion state of the point cloud conforms to the static scene assumption through inter-frame point cloud registration residual analysis and motion model projection verification. Specific implementations include: detection of occupancy probability changes based on continuous frame point clouds (e.g., Bayes filtering for occupancy grid updates), point-level motion estimation based on optical flow fields or scene flow (e.g., filtering moving points through ICP registration residual thresholds), and detection based on free space inconsistencies (identifying dynamic obstacles by comparing the visibility differences of ray projections at different times). The sensitivity of this scheme to motion feature recognition and its resistance to self-motion interference directly determine the dynamic point culling effect. Its main drawbacks include: First, insufficient accuracy in recognizing low-speed dynamic targets (such as people walking slowly or carts moving gradually), because the changes in point cloud due to low-speed motion may be comparable to the noise level of the sensor, leading to motion detection failure; Second, it is easy to misclassify isolated static points (such as thin rods or table legs) with large residual constants caused by registration errors as dynamic targets, resulting in the loss of static structural features; Third, when the device itself rotates rapidly or moves significantly, self-motion estimation errors will contaminate the inter-frame residual calculation, leading to large-area false rejection.

[0005] (III) Technical Approach Based on Semantic Models and Data Dependencies; This type of method relies on pre-trained point cloud semantic segmentation networks (such as PointNet++, RangeNet++, Cylinder3D, etc.) to directly predict semantic category labels point by point from the 3D point cloud, marking points belonging to movable categories such as pedestrians and vehicles as dynamic targets and removing them. The core reliance is on high-quality labeled datasets (such as SemanticKITTI, nuScenes) and sufficient computing power (usually requiring GPU acceleration). Its main drawbacks include: First, limited model generalization ability—it performs drastically on target types (such as new types of robots or special industrial equipment) or scene styles (such as differences in point cloud distribution caused by different sensor models) that are not present in the training data, and cannot effectively handle unknown dynamic target types in the open world; Second, a prominent contradiction between inference speed and real-time performance—the inference time of high-precision semantic segmentation models (such as point cloud segmentation networks based on the Transformer architecture) is usually on the order of hundreds of milliseconds per frame, which is difficult to meet the real-time requirements of SLAM on low-computing-power edge devices (such as embedded ARM platforms); Third, the dependence on high-quality labeled datasets leads to high deployment costs, and customized labeling for specific application scenarios requires a large amount of manual input.

[0006] In summary, the three existing technical approaches mentioned above face the following common problems in practical applications: First, the feature judgment dimension is singular and the robustness is insufficient—all methods have feature dependency limitations. Geometric methods rely only on motion and geometric information, while semantic methods rely on category labeling. Single-dimensional judgment is difficult to adapt to complex dynamic scenes. Second, noise coverage is incomplete and the processing process is cumbersome—existing methods mostly target single types of interference, lacking effective handling of isolated noise and planar deviation noise. Third, the contradiction between real-time performance and adaptability is prominent—semantic methods are time-consuming inference, multi-sensor fusion methods are greatly affected by the environment and have complex calibration, and pure geometric methods, although having excellent real-time performance, are easily affected by self-motion interference. Therefore, there is an urgent need for a laser SLAM dynamic target and noise filtering scheme that can overcome the feature dependency limitations of single methods, simultaneously cover multiple types of noise, and combine high real-time performance with strong robustness. Summary of the Invention

[0007] To address the problems of inaccurate dynamic target removal, incomplete noise type coverage, and significant contradictions between real-time performance and adaptability in existing laser SLAM technologies, this invention aims to provide a method for filtering dynamic targets and noise in laser SLAM systems. This method integrates map building, temporal features, spatial normal features, and statistical analysis to achieve precise removal of dynamic targets, isolated noise, and planar deviation noise, while ensuring the real-time performance and accuracy of map building. This effectively improves the localization and map building accuracy of SLAM systems in dynamic environments, and outputs clean and effective map point clouds.

[0008] To achieve the above objectives, the present invention provides a method for filtering dynamic targets and noise in a laser SLAM system, comprising the following steps:

[0009] S10. Perform hard time synchronization between the multi-line lidar and the IMU; calculate the gravity direction using IMU data collected during the stationary period of the equipment; convert the radar point cloud at the stationary moment to the gravity coordinate system and construct the initial map;

[0010] S20. Based on the motion state data collected in real time by IMU, perform motion distortion correction on the single frame point cloud of lidar; divide the corrected point cloud according to the line bundle ID, calculate the local curvature of each point, and extract line features and surface features according to the curvature threshold.

[0011] S30. Perform neighborhood search on the extracted line features and surface features in the existing map, and obtain the principal direction of the line features and the normal of the surface features through principal component analysis; construct the error equations of the line features and the error equations of the surface features respectively, and iteratively optimize them using the nonlinear least squares method to obtain the optimized pose of the current frame;

[0012] S40. Convert the optimized point cloud to the map coordinate system and use variable-size voxels for map management; accumulate time and position statistics for each voxel, and update features when the number of point clouds within the voxel reaches a preset condition; determine temporal dynamic targets based on temporal variance, and perform spatial shadow detection based on the linear index of covariance matrix eigenvalue decomposition; mark voxels that simultaneously satisfy low temporal variance and high linear index as dynamic targets and remove them; perform planarity judgment on non-dynamic voxels, remove planar deviation noise points for voxels that satisfy planarity constraints through planar fitting, and remove isolated noise points for voxels that do not satisfy planarity constraints through statistical outlier analysis.

[0013] S50: Update the selected voxels to the current map and release dormant voxels that have not been activated for a long time; repeat S20 to S40 until the map construction is completed and output the point cloud map after filtering out dynamic targets and noise.

[0014] Preferably, as one possible implementation, the single-frame point cloud motion compensation and correction in S20 further includes the following sub-steps:

[0015] S21. Establish the initial attitude reference using IMU data at the stationary moment;

[0016] S22. Integrate the IMU data to obtain complete attitude angle information including roll angle, pitch angle, and yaw angle;

[0017] S23. Using the pre-calibrated external parameter calibration relationship between the IMU and the lidar, the lidar point cloud data is transformed from the lidar body coordinate system to the IMU coordinate system;

[0018] S24. Based on the position and attitude obtained from IMU integration at each time step, convert the transformed point cloud data to the IMU initial time coordinate system.

[0019] S25. Using the gravity coordinate system transformation parameters calculated during the stationary period in S10, the point cloud is finally transformed to the gravity coordinate system at the stationary moment of the IMU, so as to eliminate the distortion caused by the device motion in the single frame point cloud.

[0020] Preferably, as one possible implementation, the extraction of line and surface features from the corrected point cloud in step S20 further includes the following sub-steps:

[0021] S26. Divide the single-frame lidar point cloud according to the line ID. The point cloud corresponding to each line constitutes an independent processing unit. The line affiliation is confirmed by the vertical installation angle of the lidar transmitter.

[0022] S27. Sort the points within each bundle by time. For the current point Pᵢ, search its n neighboring points before and after it in the time direction, and calculate the local curvature c according to the following formula:

[0023] ;

[0024] in, For the calculated curvature, For the current point, Points that are consecutive in time preceding the current time. Points that are consecutive in time after the current time;

[0025] S28, Set curvature threshold The curvature c is less than Points are determined to be line features, otherwise they are determined to be surface features, and the number of line features and surface features is filtered according to curvature sorting.

[0026] Preferably, as one possible implementation, the point cloud feature matching and pose optimization in S30 further includes the following sub-steps:

[0027] S31. Use the KDTREE search algorithm to perform a neighborhood search on the extracted line and area features in the existing map;

[0028] S32. Perform principal component analysis (PCA) on the searched neighborhood points, select the feature direction corresponding to the largest eigenvalue as the principal direction of the line feature, and select the feature direction corresponding to the smallest eigenvalue as the normal of the surface feature.

[0029] S33. Construct the line feature error equation based on the line characteristics and the main directions of its neighboring points:

[0030] ;

[0031] in, For the extracted line parameters, For the current frame pose that needs optimization, For the calibration parameters of the radar and IMU, For extracted line features;

[0032] S34. Construct the surface feature error equation based on the surface features and the normals of its neighboring points:

[0033] ;

[0034] in, For the extracted plane parameters, For the current frame pose that needs optimization, For the calibration parameters of the radar and IMU, For extracted surface features;

[0035] S35. Combine the line feature residuals and surface feature residuals into a unified residual function φ, and solve iteratively using the nonlinear least squares method to minimize φ to obtain the optimized pose. :

[0036] The residual set is expressed as:

[0037] ;

[0038] in Based on a predefined paradigm, by solving this function, the residuals of lines and surfaces are optimized, such that... The smaller the better, as this will ultimately yield an optimized pose. .

[0039] Preferably, as one possible implementation, the variable-size voxel map management and statistical accumulation in S40 further includes the following sub-steps:

[0040] S41. Construct variable-size voxels in the map coordinate system. The size of the voxels varies with the distance from the trajectory point: the closer to the trajectory point, the smaller the voxel size; the farther from the trajectory point, the larger the voxel size.

[0041] S42. When any point with timestamp t falls into a voxel, the basic statistics of that voxel are updated cumulatively:

[0042] The number of point clouds within a voxel, k, increases incrementally: k = k + 1;

[0043] Accumulated time statistics:

[0044] ;

[0045] ;

[0046] in, The total time represented by all points within a voxel; The sum of squares of time represents all points within a voxel;

[0047] Next, its location information is calculated:

[0048] ;

[0049] ;

[0050] in, The coordinates of the current point. This is the transpose of the current point's coordinates. This is the cumulative coordinate value of all points within the current voxel. This is the cumulative sum of the squares of the coordinates of the current voxel.

[0051] Preferably, as one possible implementation, the voxel feature update and dynamic target determination in S40 further includes the following sub-steps:

[0052] S43. A feature update is triggered once for every N1 new points added within a preset voxel. The update stops when the cumulative number of voxel updates reaches the upper limit of N2.

[0053] S44. Calculate the geometric centroid and covariance matrix:

[0054] ;

[0055] ;

[0056] Where Pc is the geometric centroid of the voxel. Let be the covariance matrix of the voxel point cloud, and k be the current number of points in the voxel;

[0057] S45, Regarding the covariance matrix Perform SVD eigenvalue decomposition to obtain eigenvalues ​​λ1, λ2, λ3 (λ1 ≤ λ2 ≤ λ3) and corresponding eigenvectors V1, V2, V3 in ascending order;

[0058] S46. Calculate the time complexity within a voxel:

[0059] Simultaneously perform in-voxel time complexity calculations:

[0060] ;

[0061] ;

[0062] in, The average time for the current voxel. The time variance of the current voxel;

[0063] S47. Time-domain determination based on time complexity: setting a time variance threshold. ,like < Then the voxel is marked as a dynamic target candidate, where It is generally set to 0.5;

[0064] S48. Spatial morphology determination based on eigenvalues, i.e., motion blur detection: calculating linear indices. :

[0065] = (λ3 - λ2) / λ3;

[0066] Set linear threshold ,like < If so, the voxel is marked as a dynamic target candidate;

[0067] S49. Comprehensive Judgment: If the current voxel simultaneously satisfies the time variance... Less than the threshold And linear index Less than the threshold If the result is positive, the voxel is determined to be a dynamic voxel, and all point cloud data within it is deleted.

[0068] Preferably, as one possible implementation; in S40, for conditions that do not meet the dynamic objective (i.e., time complexity) ≥ For static voxels that have been activated multiple times, planarity determination and planar deviation noise removal are performed, further including the following sub-steps:

[0069] S401. Use the RANSAC algorithm for coarse screening and planeness determination: Randomly select three non-collinear points within a voxel to estimate the initial plane equation Ax + By + Cz + D = 0, and calculate Pᵢ = ( for each point within the voxel) , , The algebraic distance to the initial plane: Given an interior point, determine the distance threshold from that point to the plane. Traverse all k points within the voxel and calculate the planarity score of the current plane:

[0070] ;

[0071] Where I is an indicator function, which is 1 when the condition is met and 0 otherwise; k is the number of point clouds within a voxel.

[0072] S402. If the score is greater than the preset score threshold St, the voxel is determined to have strong planar geometric features and enters the precise planar fitting stage after coarse screening.

[0073] S403. Use the least squares method to perform exact plane fitting, and optimize the plane equation parameters A, B, C, and D to satisfy:

[0074] = 0;

[0075] S404. Using the optimal plane parameters, calculate the Euclidean distance from each point within the voxel to the optimal plane:

[0076] E = | |;

[0077] Set error rejection threshold E th If E >Eth If the point is found to be a plane deviation noise point, it will be removed.

[0078] Preferably, as one possible implementation, step S40, which involves removing isolated noise points from non-planar voxels that do not satisfy the planarity constraint, further includes the following sub-steps:

[0079] S405. Suppose there are n points within the current voxel, and the three-dimensional coordinates of each point are represented by a column vector Pⱼ = ( , , ), calculate the mean coordinates of the point cloud within the voxel:

[0080] ;

[0081] S406, Calculate the distance from each point to the geometric center. absolute Euclidean distance :

[0082] ;

[0083] S407, Calculate the average distance of point clouds within voxels. :

[0084] ;

[0085] Calculate the sample standard deviation of the distance from the point cloud to the center point. :

[0086] ;

[0087] S408. Outlier detection based on the statistical 3σ law: If the distance dj from a point to the center exceeds the interval (dc - 3σd, dc + 3σd), then the point is determined to be an isolated noise point and is removed (i.e., because the distance error from a point within a voxel to the center point approximately follows a normal distribution). According to statistics... According to the law, the distance from the normal measurement point to the center should be highly concentrated in ( ). - , + Within the interval. If the distance from a point to the center is... If the distance is significantly greater than the average distance and exceeds 3 times the standard deviation threshold, it is considered an isolated flypoint and is therefore removed.

[0088] Preferably, as one possible implementation, S50 further includes the following sub-steps:

[0089] S51. Update the voxels after dynamic target removal, planar deviation noise removal and isolated noise removal to the current map for matching and optimization in subsequent frames.

[0090] S52. Periodically filter voxels in the map: determine whether a voxel has been silenced by judging the distance from the center of the voxel to the center of the current matching frame and the time when the voxel was last activated.

[0091] S53. If a voxel is not activated for more than a preset time threshold and its distance from the current frame exceeds a preset distance threshold, then the voxel is determined to be silent, its point cloud data is output to the disk and the corresponding memory is released.

[0092] Preferably, as one possible implementation; in S53, the preset time threshold is 5 minutes and the preset distance threshold is 30 meters; that is, if a voxel is not activated for more than 5 minutes and the distance from the current frame is more than 30 meters, it is determined to be a silent voxel.

[0093] Preferably, as one possible implementation, the determination of dynamic targets in S40 adopts a dual verification mechanism of temporal and spatial characteristics: the temporal dimension characterizes the dispersion of the target's temporal distribution based on the time variance of the point cloud within the voxel; the lower the time variance, the more concentrated the laser point timestamps are, and the more likely they correspond to a dynamic target that passes through briefly; the spatial dimension characterizes the linearity of the point cloud spatial morphology based on the ratio of eigenvalues ​​of the covariance matrix; the lower the linearity index, the closer the point cloud is to a one-dimensional linear distribution, and the more likely it is to correspond to the motion trail of a dynamic target; only when a voxel simultaneously meets the two conditions of low time variance and high linearity index is it determined to be a dynamic voxel and the deletion operation is performed.

[0094] Preferably, as one possible implementation, in S43, a voxel feature update is triggered once for every N1=5 newly added points, and the upper limit of the cumulative number of voxel updates N2 corresponds to the upper limit of the number of point clouds of 20 points. After exceeding this upper limit, the voxel will no longer be updated, so as to control the computational overhead.

[0095] Preferably, as one possible implementation, the sensor data preprocessing relies only on two sensors: multi-line lidar and IMU, without the need to introduce additional vision equipment, thus avoiding the complexity of external parameter calibration in multi-sensor fusion and the interference of environmental factors such as illumination and occlusion on the judgment of dynamic targets.

[0096] Preferably, as an implementation scheme, S40 simultaneously completes the filtering of three types of interference in a single traversal through the same voxel map management framework: (1) dynamic target removal based on dual verification of time variance and linear index; (2) plane deviation noise removal based on RANSAC coarse screening and least squares fine fitting; (3) isolated noise removal based on 3σ statistical outlier analysis; the three types of filtering share the accumulated statistics within the voxel, realizing the integrated removal of dynamic targets and multiple types of noise.

[0097] Preferably, as one possible implementation, the process of cyclically executing S20 to S40 in S50 is an online continuous optimization: after motion compensation (S20) and pose optimization (S30) of each newly arrived LiDAR point cloud, the map is updated through the multi-dimensional feature filtering mechanism of the voxel map (S40), and silent voxels are periodically recycled, so that the map is continuously purified and optimized as the sensor continues to collect data, and finally a clean and effective global point cloud map is output.

[0098] Compared with the prior art, the present invention has the following beneficial effects:

[0099] This invention provides a method for filtering dynamic targets and noise in a real-time laser SLAM system. It integrates a complete workflow including hard synchronization, static gravity direction calculation, initial map construction, single-frame point cloud correction, line and surface feature extraction and matching, attitude optimization, variable-size voxel map maintenance (including spatiotemporal feature statistical analysis and dynamic / noise judgment), map updating, and iterative optimization. It utilizes multi-dimensional feature fusion (temporal features, normal features, spatial morphological features, and statistical analysis) to replace single-feature judgment and employs an integrated processing flow to simultaneously incorporate dynamic target removal and noise filtering into the SLAM mapping process, rather than the traditional staged post-processing mode. By utilizing multiple feature information to form a complementary verification mechanism, it solves technical problems such as insufficient accuracy of single geometric methods for low-speed dynamic target recognition, the reliance on computing power and data for semantic methods, and the complexity and susceptibility to environmental interference in multi-sensor fusion methods. This achieves a comprehensive improvement in the localization and mapping accuracy of the laser SLAM system in dynamic environments, while ensuring real-time performance and significantly enhancing the system's robustness and applicability in complex dynamic scenarios.

[0100] The present invention provides a method for filtering dynamic targets and noise in a real-time laser SLAM system. This method employs a variable-size voxel map construction method, which significantly reduces the computational load in distant areas while ensuring local mapping accuracy. It sets frequency and quantity thresholds for voxel updates to avoid meaningless repeated updates and reduce time complexity. Combined with voxel silencing judgment rules (if the voxel is not activated within a timeout period and the distance from the current frame exceeds a spatial threshold, memory is released), achieving efficient memory utilization. This method avoids the problems of long reasoning time and high computing power requirements of semantic methods, while retaining the real-time advantages of pure geometric methods, and is suitable for low-computing-power edge devices.

[0101] This invention provides a complete map maintenance process and a method for continuous feature map updates. A three-stage filtering pipeline—dynamic target identification, planar deviation noise removal, and isolated noise detection—ensures the up-to-date state and high accuracy of the map data. Simultaneously, a pose optimization method with dynamic damping factor adjustment and an unstable frame contribution reduction method ensure the continuous stability of mapping and localization in dynamic environments. Vertical expansion and correction schemes for each sub-step (such as neighbor point cross-plane detection, multi-plane hypothesis generation and verification, and adaptive damping factor adjustment) further improve the method's processing accuracy and robustness in special boundary scenarios such as corners, multi-plane intersections, and low-speed targets. Attached Figure Description

[0102] Figure 1 This is a flowchart of a method for filtering dynamic targets and noise in a real-time laser SLAM system according to the present invention;

[0103] Figure 2 This is a schematic diagram of a specific process in a method for filtering dynamic targets and noise in a real-time laser SLAM system according to the present invention.

[0104] Figure 3 This is another specific flowchart illustrating a method for filtering dynamic targets and noise in a real-time laser SLAM system according to the present invention.

[0105] Figure 4 This is another detailed flowchart illustrating a method for filtering dynamic targets and noise in a real-time laser SLAM system according to the present invention. Detailed Implementation

[0106] The technical solutions of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0107] Example 1

[0108] like Figure 1 As shown in the figure, a method for filtering dynamic targets and noise in a real-time laser SLAM system in this embodiment may specifically include:

[0109] S10. Perform hard time synchronization between the multi-line lidar and the IMU; calculate the gravity direction using IMU data collected during the stationary period of the equipment; convert the radar point cloud at the stationary moment to the gravity coordinate system and construct the initial map;

[0110] S20. Based on the motion state data collected in real time by IMU, perform motion distortion correction on the single frame point cloud of lidar; divide the corrected point cloud according to the line bundle ID, calculate the local curvature of each point, and extract line features and surface features according to the curvature threshold.

[0111] S30. Perform neighborhood search on the extracted line features and surface features in the existing map, and obtain the principal direction of the line features and the normal of the surface features through principal component analysis; construct the error equations of the line features and the error equations of the surface features respectively, and iteratively optimize them using the nonlinear least squares method to obtain the optimized pose of the current frame;

[0112] S40. Convert the optimized point cloud to the map coordinate system and use variable-size voxels for map management; accumulate time and position statistics for each voxel, and update features when the number of point clouds within the voxel reaches a preset condition; determine temporal dynamic targets based on time variance, and perform spatial shadow detection based on the linear index of covariance matrix eigenvalue decomposition; mark voxels that simultaneously satisfy low time variance and high linear index as dynamic targets and remove them; perform planarity judgment on non-dynamic voxels, remove planar deviation noise points for voxels that satisfy planarity constraints through planar fitting, and remove isolated noise points for voxels that do not satisfy planarity constraints through statistical outlier analysis.

[0113] S50: Update the selected voxels to the current map, and release dormant voxels that have not been activated for a long time; repeat steps S20 to S40 until map construction is complete, and output a point cloud map after filtering out dynamic targets and noise. It should be noted that in the above process, "update the selected voxels to the current map", the current map is actually continuously accumulating and updating the map that was initially initialized. Each optimized voxel is used to update the map.

[0114] The present invention provides a method for filtering dynamic targets and noise in a real-time laser SLAM system that does not rely on additional sensors such as vision devices, nor does it require pre-trained semantic segmentation networks or high-quality labeled datasets. It achieves the entire processing process solely through a conventional sensor combination of LiDAR and IMU, combined with the fusion analysis of pure geometric and temporal features. This avoids the precise calibration requirements and environmental interference issues of multi-sensor fusion methods, and eliminates the computational and data dependencies of semantic methods, enhancing adaptability and anti-interference capabilities in complex dynamic scenes.

[0115] The above-described scheme describes a method for filtering dynamic targets and noise in a real-time laser SLAM system. It integrates the entire process from S10 to S50, including hard synchronization of time, static gravity direction calculation, initial map construction, single-frame point cloud correction, line and surface feature extraction and matching, attitude optimization, variable-size voxel map maintenance (including spatiotemporal feature statistical analysis and dynamic / noise judgment), map updating, and iterative optimization. It replaces single-feature judgment with multi-dimensional feature fusion (temporal features, normal features, spatial morphological features, and statistical analysis), and adopts an integrated processing flow to simultaneously incorporate dynamic target removal and noise filtering into the SLAM mapping process, rather than the traditional staged post-processing mode. It utilizes a complementary verification mechanism formed by multiple feature information, solving technical problems such as insufficient accuracy of single geometric methods for low-speed dynamic target recognition, reliance on computational power and data for semantic methods, and complex calibration and susceptibility to environmental interference in multi-sensor fusion methods. This achieves a comprehensive improvement in the localization and mapping accuracy of the laser SLAM system in dynamic environments, while ensuring real-time performance and significantly enhancing the system's robustness and applicability in complex dynamic scenarios.

[0116] The present invention provides a method for filtering dynamic targets and noise in a real-time laser SLAM system. This method employs a variable-size voxel map construction method, which significantly reduces the computational load in distant areas while ensuring local mapping accuracy. It sets frequency and quantity thresholds for voxel updates to avoid meaningless repeated updates and reduce time complexity. Combined with voxel silencing judgment rules (if the voxel is not activated within a timeout period and the distance from the current frame exceeds a spatial threshold, memory is released), achieving efficient memory utilization. This method avoids the problems of long reasoning time and high computing power requirements of semantic methods, while retaining the real-time advantages of pure geometric methods, and is suitable for low-computing-power edge devices.

[0117] This invention provides a complete map maintenance process and a method for continuous feature map updates. A three-stage filtering pipeline—dynamic target identification, planar deviation noise removal, and isolated noise detection—ensures the up-to-date state and high accuracy of the map data. Simultaneously, a pose optimization method with dynamic damping factor adjustment and an unstable frame contribution reduction method ensure the continuous stability of mapping and localization in dynamic environments. Vertical expansion and correction schemes for each sub-step (such as neighbor point cross-plane detection, multi-plane hypothesis generation and verification, and adaptive damping factor adjustment) further improve the method's processing accuracy and robustness in special boundary scenarios such as corners, multi-plane intersections, and low-speed targets.

[0118] like Figure 2 As shown, preferably, as one possible implementation scheme, the single-frame point cloud motion compensation and correction in S20 further includes the following sub-steps:

[0119] S21. Establish the initial attitude reference using IMU data at the stationary moment;

[0120] S22. Integrate the IMU data to obtain complete attitude angle information including roll angle, pitch angle, and yaw angle;

[0121] S23. Using the pre-calibrated external parameter calibration relationship between the IMU and the lidar, the lidar point cloud data is transformed from the lidar body coordinate system to the IMU coordinate system;

[0122] S24. Based on the position and attitude obtained from IMU integration at each time step, convert the transformed point cloud data to the IMU initial time coordinate system.

[0123] S25. Using the gravity coordinate system transformation parameters calculated during the stationary period in S10, the point cloud is finally transformed to the gravity coordinate system at the stationary moment of the IMU, so as to eliminate the distortion caused by the device motion in the single frame point cloud.

[0124] Analysis reveals that the above scheme describes the specific implementation of the single-frame point cloud correction step. It utilizes IMU data at a stationary moment for initial attitude, obtains complete attitude angle information (roll angle, pitch angle, yaw angle) through IMU integration, and uses the pre-calibrated relative position and attitude relationship between the IMU and the lidar to sequentially transform the point cloud data from the lidar coordinate system to the IMU coordinate system, the IMU initial moment coordinate system, and the gravity coordinate system. Continuous motion information from the inertial measurement unit is used to retrospectively compensate for the precise pose of each measurement point of the lidar. This technical scheme utilizes the principle of point-by-point motion distortion correction of the lidar point cloud using high-frequency IMU motion data, solving the problem of point cloud distortion caused by different measurement poses at different times within a single frame of point cloud during equipment movement. This eliminates interference errors from equipment self-motion on the point cloud data, providing an accurate and effective point cloud data foundation for subsequent feature extraction, feature matching, and dynamic target judgment, effectively improving the mapping accuracy of the SLAM system in motion.

[0125] like Figure 3 As shown, preferably, as one possible implementation, the extraction of line and surface features from the corrected point cloud in step S20 further includes the following sub-steps:

[0126] S26. Divide the single-frame lidar point cloud according to the line ID. The point cloud corresponding to each line constitutes an independent processing unit. The line affiliation is confirmed by the vertical installation angle of the lidar transmitter.

[0127] S27. Sort the points within each bundle by time. For the current point Pᵢ, search its n neighboring points before and after it in the time direction, and calculate the local curvature c according to the following formula:

[0128] ;

[0129] in, For the calculated curvature, For the current point, Points that are consecutive in time preceding the current time. Points that are consecutive in time after the current time;

[0130] S28, Set curvature threshold The curvature c is less than Points are determined to be line features, otherwise they are determined to be surface features, and the number of line features and surface features is filtered according to curvature sorting.

[0131] Analysis of the above scheme reveals that it describes the specific implementation method of single-frame point cloud feature extraction. It divides the corrected single-frame LiDAR point cloud according to beam IDs, making the point cloud corresponding to each beam an independent processing unit. For each beam's point cloud, it calculates the local curvature of each point and uses a curvature threshold to distinguish between line features (high curvature) and surface features (low curvature), then sorts and selects the number of feature points based on curvature. This scheme utilizes the inherent physical structure of LiDAR beam scanning (the temporal and spatial continuity of adjacent points on the same beam) to calculate local curvature, thereby efficiently extracting edges and planar structures in the environment. It solves the problem of quickly extracting geometrically representative feature points from massive amounts of raw point clouds, providing high-quality, low-data-volume feature input for subsequent line-surface feature-based scanning matching and pose optimization, effectively reducing matching computation and improving pose estimation accuracy.

[0132] Preferably, as one possible implementation, the point cloud feature matching and pose optimization in S30 further includes the following sub-steps:

[0133] S31. Use the KDTREE search algorithm to perform a neighborhood search on the extracted line and area features in the existing map;

[0134] S32. Perform principal component analysis (PCA) on the searched neighborhood points, select the feature direction corresponding to the largest eigenvalue as the principal direction of the line feature, and select the feature direction corresponding to the smallest eigenvalue as the normal of the surface feature.

[0135] S33. Construct the line feature error equation based on the line characteristics and the main directions of its neighboring points:

[0136] ;

[0137] in, For the extracted line parameters, For the current frame pose that needs optimization, For the calibration parameters of the radar and IMU, For extracted line features;

[0138] S34. Construct the surface feature error equation based on the surface features and the normals of its neighboring points:

[0139] ;

[0140] in, For the extracted plane parameters, For the current frame pose that needs optimization, For the calibration parameters of the radar and IMU, For extracted surface features;

[0141] S35. Combine the line feature residuals and surface feature residuals into a unified residual function φ, and solve iteratively using the nonlinear least squares method to minimize φ to obtain the optimized pose. :

[0142] The residual set is expressed as:

[0143] ;

[0144] in Based on a predefined paradigm, by solving this function, the residuals of lines and surfaces are optimized, such that... The smaller the better, as this will ultimately yield an optimized pose. .

[0145] The aforementioned scheme describes a specific formula for calculating the local curvature of a point cloud. After sorting the points on each line bundle by time, it takes n consecutive neighboring points before and after the current point and quantitatively characterizes the geometric curvature of the local point cloud by accumulating the cosine values ​​of the angles between the vectors. Utilizing the principle that the magnitude of the curvature value reflects the directional consistency between the current point and its neighboring points, it achieves quantitative differentiation between corner points (edge ​​points) and planar points in the laser point cloud. This formula, through comprehensive calculation of multi-point neighborhood information, effectively suppresses the interference of single-point noise on curvature judgment, making the differentiation between line features (high curvature) and surface features (low curvature) more stable and reliable. This provides accurate data support for subsequent feature extraction, thereby indirectly improving the accuracy of pose optimization in the SLAM system.

[0146] Preferably, as one possible implementation, the variable-size voxel map management and statistical accumulation in S40 further includes the following sub-steps:

[0147] S41. Construct variable-size voxels in the map coordinate system. The size of the voxels varies with the distance from the trajectory point: the closer to the trajectory point, the smaller the voxel size; the farther from the trajectory point, the larger the voxel size.

[0148] S42. When any point with timestamp t falls into a voxel, the basic statistics of that voxel are updated cumulatively:

[0149] The number of point clouds within a voxel, k, increases incrementally: k = k + 1;

[0150] Accumulated time statistics:

[0151] ;

[0152] ;

[0153] in, The total time represented by all points within a voxel; The sum of squares of time represents all points within a voxel;

[0154] Next, its location information is calculated:

[0155] ;

[0156] ;

[0157] in, The coordinates of the current point. This is the transpose of the current point's coordinates. This is the cumulative coordinate value of all points within the current voxel. This is the cumulative sum of the squares of the coordinates of the current voxel.

[0158] The above scheme records the mathematical expressions of the line feature error equation, the surface feature error equation, and the joint residual equation in the single-frame point cloud pose optimization. It constructs the line feature residual based on the extracted line features and the main direction of the neighboring points, and constructs the surface feature residual based on the surface features and the normal direction of the neighboring points. The sum of the L2 norms of the two types of residuals is used as the joint optimization objective function, and the objective function is minimized by iterative solution using the nonlinear least squares method.

[0159] Therefore, this technical solution utilizes stable linear structures (such as corners and tree trunks) and planar structures (such as the ground and walls) in the environment as geometric constraints. By jointly optimizing the line and planar residuals, it accurately estimates the pose of the current frame. This solves the problem of insufficient pose estimation accuracy of single feature constraints in geometrically degraded environments, and achieves high-precision frame-map registration. This lays a solid foundation for the accuracy of subsequent voxel map maintenance and spatiotemporal feature analysis of dynamic targets, thereby improving the positioning reliability of the entire SLAM system in dynamic environments.

[0160] Preferably, as one possible implementation, the voxel feature update and dynamic target determination in S40 further includes the following sub-steps:

[0161] S43. A feature update is triggered once for every N1 new points added within a preset voxel. The update stops when the cumulative number of voxel updates reaches the upper limit of N2.

[0162] S44. Calculate the geometric centroid and covariance matrix:

[0163] ;

[0164] ;

[0165] Where Pc is the geometric centroid of the voxel. Let be the covariance matrix of the voxel point cloud, and k be the current number of points in the voxel;

[0166] S45, Regarding the covariance matrix Perform SVD eigenvalue decomposition to obtain eigenvalues ​​λ1, λ2, λ3 (λ1 ≤ λ2 ≤ λ3) and corresponding eigenvectors V1, V2, V3 in ascending order;

[0167] S46. Calculate the time complexity within a voxel:

[0168] Simultaneously perform in-voxel time complexity calculations:

[0169] ;

[0170] ;

[0171] in, The average time for the current voxel. The time variance of the current voxel;

[0172] S47. Time-domain determination based on time complexity: setting a time variance threshold. ,like < Then the voxel is marked as a dynamic target candidate, where It is generally set to 0.5;

[0173] S48. Spatial morphology determination based on eigenvalues, i.e., motion blur detection: calculating linear indices. :

[0174] = (λ3 - λ2) / λ3;

[0175] Set linear threshold ,like < If so, the voxel is marked as a dynamic target candidate;

[0176] S49. Comprehensive Judgment: If the current voxel simultaneously satisfies the time variance... Less than the threshold And linear index Less than the threshold If the result is positive, the voxel is determined to be a dynamic voxel, and all point cloud data within it is deleted.

[0177] Research has found that this scheme sets different voxel sizes for different regions during map construction—smaller voxels closer to the trajectory point and larger voxels farther away—and stores the time and location information of each point after determining the voxel range it falls into. This scheme leverages the spatial characteristic that nearby regions have a greater impact on mapping accuracy than distant regions, employing non-uniform voxel partitioning to balance accuracy and computational load. This solves the technical problems of uniform voxel maps generating excessive redundant calculations in distant regions and insufficient accuracy in nearby regions. It achieves the technical effect of significantly reducing computational load and time complexity in distant regions while maintaining local mapping accuracy, making the algorithm more friendly to low-computing-power devices at the edge and enhancing the system's real-time performance.

[0178] The above scheme describes a dynamic target determination method based on spatial morphology (momentation detection), which calculates the linearity index L_n = (λ3-λ2) / λ3 by analyzing the eigenvalues ​​of the covariance matrix. Less than the set threshold The system considers linearity to be significant and combines it with temporal variance for dual judgment: if the temporal variance is less than a specified threshold and the linearity is less than a set threshold, it is judged as a dynamic voxel and all points inside it are deleted. It utilizes the geometric principle that dynamic targets, due to the time difference in continuous radar scanning during movement, produce a trailing effect in the direction of motion, causing the spatial morphology of the point cloud within the voxel to degenerate into a linear distribution (λ2 and λ3 are similar and much smaller than the direction corresponding to λ1). This solves the problem of misjudgment that may occur in complex scenarios with a single time dimension judgment (such as the special case of static but concentrated timestamps). Through a spatiotemporal dual verification mechanism, it significantly improves the accuracy of dynamic target recognition, reduces the risk of erroneously deleting static map points, and ensures the integrity and reliability of map construction.

[0179] The above scheme also describes the specific update method after the voxels reach the feature update conditions. It obtains eigenvalues ​​λ1, λ2, and λ3 and their eigenvectors through geometric centroid calculation, covariance matrix calculation, and SVD eigenvalue decomposition, and then updates them using time variance... This method quantifies the discreteness of timestamps in voxel point clouds. It utilizes SVD decomposition to decompose the spatial distribution of voxel point clouds into variance components in three orthogonal directions. By employing the principle of calculating temporal variance using first and second moments, it transforms the raw spatiotemporal data of voxel point clouds into eigenvalues ​​and temporal statistics with clear geometric and temporal physical meaning. This achieves a structured mathematical expression of the spatiotemporal distribution of voxel point clouds, providing a foundation for subsequent temporal domain determination (temporal variance threshold) and spatial domain determination (linearity index) of dynamic targets. It provides a unified quantitative analysis basis for noise judgment (planarity, isolation).

[0180] Preferably, as one possible implementation; in S40, for conditions that do not meet the dynamic objective (i.e., time complexity) ≥ For static voxels that have been activated multiple times, planarity determination and planar deviation noise removal are performed, further including the following sub-steps:

[0181] S401. Use the RANSAC algorithm for coarse screening and planeness determination: Randomly select three non-collinear points within a voxel to estimate the initial plane equation Ax + By + Cz + D = 0, and calculate Pᵢ = ( for each point within the voxel) , , The algebraic distance to the initial plane: Given an interior point, determine the distance threshold from that point to the plane. Traverse all k points within the voxel and calculate the planarity score of the current plane:

[0182] ;

[0183] Where I is an indicator function, which is 1 when the condition is met and 0 otherwise; k is the number of point clouds within a voxel.

[0184] S402. If the score is greater than the preset score threshold St, the voxel is determined to have strong planar geometric features and enters the precise planar fitting stage after coarse screening.

[0185] S403. Use the least squares method to perform exact plane fitting, and optimize the plane equation parameters A, B, C, and D to satisfy:

[0186] = 0;

[0187] S404. Using the optimal plane parameters, calculate the Euclidean distance from each point within the voxel to the optimal plane:

[0188] E = | |;

[0189] Set error rejection threshold E th If E >E th If the point is found to be a plane deviation noise point, it will be removed.

[0190] The above scheme describes the specific methods for accurate plane fitting and point-to-plane error elimination. It utilizes the least squares method to perform accurate plane fitting on the voxel point cloud that has passed the initial screening, optimizing the plane equation parameters A, B, C, and D to make Σ( ) = 0, and then use the fitted optimal plane to calculate the absolute Euclidean distance E for each point within the voxel. = | | Discard items with a distance greater than the error threshold E th Points that deviate from the target plane are identified as noise and removed. This method utilizes high-quality intra-points coarsely filtered by RANSAC to perform least-squares precise fitting to obtain optimal plane parameters. Then, it uses this precise plane to evaluate the point-by-point distance of all points within a voxel. This solves the technical problem of limited accuracy and inability to distinguish between small-amplitude deviation noise points and normal points when relying solely on RANSAC for coarse removal. It achieves high-precision point-by-point screening and removal of plane deviation noise points within a voxel, further improving the purity of the feature map and providing a cleaner and more reliable reference map for subsequent frame-map matching.

[0191] Preferably, as one possible implementation, step S40, which involves removing isolated noise points from non-planar voxels that do not satisfy the planarity constraint, further includes the following sub-steps:

[0192] S405. Suppose there are n points within the current voxel, and the three-dimensional coordinates of each point are represented by a column vector Pⱼ = ( , , ), calculate the mean coordinates of the point cloud within the voxel:

[0193] ;

[0194] S406, Calculate the distance from each point to the geometric center. absolute Euclidean distance :

[0195] ;

[0196] S407, Calculate the average distance of point clouds within voxels. :

[0197] ;

[0198] Calculate the sample standard deviation of the distance from the point cloud to the center point. :

[0199] ;

[0200] S408. Outlier detection based on the statistical 3σ law: If the distance dj from a point to the center exceeds the interval (dc - 3σd, dc + 3σd), then the point is determined to be an isolated noise point and is removed (i.e., because the distance error from a point within a voxel to the center point approximately follows a normal distribution). According to statistics... According to the law, the distance from the normal measurement point to the center should be highly concentrated in ( ). - , + Within the interval. If the distance from a point to the center is... If the distance is significantly greater than the average distance and exceeds 3 times the standard deviation threshold, it is considered an isolated flypoint and is therefore removed.

[0201] The above scheme describes a method for judging and removing isolated noise points. For voxel points that do not satisfy the planarity constraint, the mean value of the point cloud coordinates within the voxel is calculated. Euclidean distance from each point to the geometric center Average distance and sample standard deviation Using the statistical 3σ law: if the distance from a point to the center is... Points significantly larger than the average distance and exceeding three times the standard deviation are identified as outliers and removed. This method utilizes the statistical principle that over 99.7% of samples in a normal distribution should fall within the mean ± 3σ range. It approximates the distance error from a point within a voxel to its geometric center as following a normal distribution, identifying isolated noise points by statistically assessing the outlier degree of the distance. This solves the problem of not being able to identify noise points in non-planar structured voxels (such as vegetation or cluttered areas) through planar fitting, achieving effective detection and removal of isolated outliers scattered in space. It compensates for the blind spots of planar methods in unstructured areas, forming a comprehensive coverage capability for various types of noise.

[0202] like Figure 4 As shown, preferably, as one possible implementation, S50 further includes the following sub-steps:

[0203] S51. Update the voxels after dynamic target removal, planar deviation noise removal and isolated noise removal to the current map for matching and optimization in subsequent frames.

[0204] S52. Periodically filter voxels in the map: determine whether a voxel has been silenced by judging the distance from the center of the voxel to the center of the current matching frame and the time when the voxel was last activated.

[0205] S53. If a voxel is not activated for more than a preset time threshold and its distance from the current frame exceeds a preset distance threshold, then the voxel is determined to be silent, its point cloud data is output to the disk and the corresponding memory is released.

[0206] Preferably, as one possible implementation; in S53, the preset time threshold is 5 minutes and the preset distance threshold is 30 meters; that is, if a voxel is not activated for more than 5 minutes and the distance from the current frame is more than 30 meters, it is determined to be a silent voxel.

[0207] Preferably, as one possible implementation, the determination of dynamic targets in S40 adopts a dual verification mechanism of temporal and spatial characteristics: the temporal dimension characterizes the dispersion of the target's temporal distribution based on the time variance of the point cloud within the voxel; the lower the time variance, the more concentrated the laser point timestamps are, and the more likely they correspond to a dynamic target that passes through briefly; the spatial dimension characterizes the linearity of the point cloud spatial morphology based on the ratio of eigenvalues ​​of the covariance matrix; the lower the linearity index, the closer the point cloud is to a one-dimensional linear distribution, and the more likely it is to correspond to the motion trail of a dynamic target; only when a voxel simultaneously meets the two conditions of low time variance and high linearity index is it determined to be a dynamic voxel and the deletion operation is performed.

[0208] Preferably, as one possible implementation, in S43, a voxel feature update is triggered once for every N1=5 newly added points, and the upper limit of the cumulative number of voxel updates N2 corresponds to the upper limit of the number of point clouds of 20 points. After exceeding this upper limit, the voxel will no longer be updated, so as to control the computational overhead.

[0209] Preferably, as one possible implementation, the sensor data preprocessing relies only on two sensors: multi-line lidar and IMU, without the need to introduce additional vision equipment, thus avoiding the complexity of external parameter calibration in multi-sensor fusion and the interference of environmental factors such as illumination and occlusion on the judgment of dynamic targets.

[0210] Preferably, as an implementation scheme, S40 simultaneously completes the filtering of three types of interference in a single traversal through the same voxel map management framework: (1) dynamic target removal based on dual verification of time variance and linear index; (2) plane deviation noise removal based on RANSAC coarse screening and least squares fine fitting; (3) isolated noise removal based on 3σ statistical outlier analysis; the three types of filtering share the accumulated statistics within the voxel, realizing the integrated removal of dynamic targets and multiple types of noise.

[0211] Preferably, as one possible implementation, the process of cyclically executing S20 to S40 in S50 is an online continuous optimization: after motion compensation (S20) and pose optimization (S30) of each newly arrived LiDAR point cloud, the map is updated through the multi-dimensional feature filtering mechanism of the voxel map (S40), and silent voxels are periodically recycled, so that the map is continuously purified and optimized as the sensor continues to collect data, and finally a clean and effective global point cloud map is output.

[0212] Example 2

[0213] On the other hand, based on the method for filtering dynamic targets and noise in a real-time laser SLAM system provided in Embodiment 1, this second embodiment also provides a storage medium, including a processor and a memory. The memory stores a computer program, and when the processor executes the computer program, it implements the steps of the method for filtering dynamic targets and noise in a real-time laser SLAM system as described in Embodiment 1.

[0214] In summary, the method for filtering dynamic targets and noise in a real-time laser SLAM system provided by this invention has the following advantages compared with the prior art: This invention provides a dynamic target and noise removal method that combines time features, normal features, and statistical analysis. By jointly determining time complexity and spatial linear features, it achieves high-precision identification of dynamic targets. At the same time, through multi-plane RANSAC fitting and normal distribution statistics, it covers two types of interference: plane deviation noise and isolated noise, solving the problem of incomplete noise coverage in the prior art, and significantly improving the mapping quality and positioning accuracy of SLAM in dynamic environments.

[0215] The present invention provides a method for filtering dynamic targets and noise in a real-time laser SLAM system. This method employs a variable-size voxel map construction method, which significantly reduces the computational load in distant areas while ensuring local mapping accuracy. It sets frequency and quantity thresholds for voxel updates to avoid meaningless repeated updates and reduce time complexity. Combined with voxel silencing judgment rules (if the voxel is not activated within a timeout period and the distance from the current frame exceeds a spatial threshold, memory is released), achieving efficient memory utilization. This method avoids the problems of long reasoning time and high computing power requirements of semantic methods, while retaining the real-time advantages of pure geometric methods, and is suitable for low-computing-power edge devices.

[0216] The present invention provides a method for filtering dynamic targets and noise in a real-time laser SLAM system that does not rely on additional sensors such as vision devices, nor does it require pre-trained semantic segmentation networks or high-quality labeled datasets. It achieves the entire processing process solely through a conventional sensor combination of LiDAR and IMU, combined with the fusion analysis of pure geometric and temporal features. This avoids the precise calibration requirements and environmental interference issues of multi-sensor fusion methods, and eliminates the computational and data dependencies of semantic methods, enhancing adaptability and anti-interference capabilities in complex dynamic scenes.

[0217] This invention provides a complete map maintenance process and a method for continuous feature map updates. A three-stage filtering pipeline—dynamic target identification, planar deviation noise removal, and isolated noise detection—ensures the up-to-date state and high accuracy of the map data. Simultaneously, a pose optimization method with dynamic damping factor adjustment and an unstable frame contribution reduction method ensure the continuous stability of mapping and localization in dynamic environments. Vertical expansion and correction schemes for each sub-step (such as neighbor point cross-plane detection, multi-plane hypothesis generation and verification, and adaptive damping factor adjustment) further improve the method's processing accuracy and robustness in special boundary scenarios such as corners, multi-plane intersections, and low-speed targets.

[0218] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; those skilled in the art can modify the technical solutions described in the foregoing embodiments, or make equivalent substitutions for some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

[0219] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A method for filtering dynamic targets and noise in a real-time laser SLAM system, characterized in that, Includes the following steps: S10. Perform hard time synchronization between the multi-line lidar and the IMU; The direction of gravity is calculated using IMU data collected during periods when the device is stationary; Transform the radar point cloud at a stationary moment into a gravity coordinate system to construct an initial map; S20. Based on the motion state data collected in real time by the IMU, perform motion distortion correction on the single-frame point cloud of the lidar. The corrected point cloud is divided according to the line bundle ID, the local curvature of each point is calculated, and line features and surface features are extracted based on the curvature threshold. S30. Perform neighborhood search on the extracted line and surface features in the existing map, and obtain the principal direction of the line features and the normal of the surface features through principal component analysis; Line feature error equations and surface feature error equations are constructed respectively, and the optimized pose of the current frame is obtained by iterative optimization using nonlinear least squares method. S40. Convert the point cloud after pose optimization to the map coordinate system and use variable-size voxels for map management. For each voxel, the cumulative time and position statistics are updated when the number of point clouds within the voxel reaches a preset condition. Temporal dynamic targets are determined based on temporal variance, and spatial trailing is detected based on the linear index of the covariance matrix eigenvalue decomposition. Voxels that simultaneously satisfy low temporal variance and high linear index are marked as dynamic targets and removed. Planarity is determined for non-dynamic voxels. For voxels that satisfy the planarity constraint, planar deviation noise is removed by plane fitting. For voxels that do not satisfy the planarity constraint, isolated noise is removed by statistical outlier analysis. S50: Update the selected voxels to the current map and release dormant voxels that have not been activated for a long time; repeat S20 to S40 until the map construction is completed and output the point cloud map after filtering out dynamic targets and noise.

2. The method according to claim 1, characterized in that, The single-frame point cloud motion compensation and correction in S20 further includes the following sub-steps: S21. Establish the initial attitude reference using IMU data at the stationary moment; S22. Integrate the IMU data to obtain complete attitude angle information including roll angle, pitch angle, and yaw angle; S23. Using the pre-calibrated external parameter calibration relationship between the IMU and the lidar, the lidar point cloud data is transformed from the lidar body coordinate system to the IMU coordinate system; S24. Based on the position and attitude obtained from IMU integration at each time step, convert the transformed point cloud data to the IMU initial time coordinate system. S25. Using the gravity coordinate system transformation parameters calculated during the stationary period in S10, the point cloud is finally transformed to the gravity coordinate system at the stationary moment of the IMU, so as to eliminate the distortion caused by the device motion in the single frame point cloud.

3. The method according to claim 1, characterized in that, The step S20, which involves extracting line and surface features from the corrected point cloud, further includes the following sub-steps: S26. Divide the single-frame lidar point cloud according to the line ID. The point cloud corresponding to each line constitutes an independent processing unit. The line affiliation is confirmed by the vertical installation angle of the lidar transmitter. S27. Sort the points within each bundle by time. For the current point Pᵢ, search its n neighboring points before and after it in the time direction, and calculate the local curvature c according to the following formula: ； in, For the curvature to be calculated, For the current point, Points that are consecutive in time preceding the current time. Points that are consecutive in time after the current time; S28, Set curvature threshold The curvature c is less than Points are determined to be line features, otherwise they are determined to be surface features, and the number of line features and surface features is filtered according to curvature sorting.

4. The method according to claim 1, characterized in that, The point cloud feature matching and pose optimization in S30 further includes the following sub-steps: S31. Use the KDTREE search algorithm to perform a neighborhood search on the extracted line and area features in the existing map; S32. Perform principal component analysis (PCA) on the searched neighborhood points, select the feature direction corresponding to the largest eigenvalue as the principal direction of the line feature, and select the feature direction corresponding to the smallest eigenvalue as the normal of the surface feature. S33. Construct the line feature error equation based on the line characteristics and the main directions of its neighboring points: ； in, For the extracted line parameters, For the current frame pose that needs optimization, For the calibration parameters of the radar and IMU, For extracted line features; S34. Construct the surface feature error equation based on the surface features and the normals of its neighboring points: ； in, For the extracted plane parameters, For the current frame pose that needs optimization, For the calibration parameters of the radar and IMU, For extracted surface features; S35. Combine the line feature residuals and surface feature residuals into a unified residual function φ, and solve iteratively using the nonlinear least squares method to minimize φ to obtain the optimized pose. : The residual set is expressed as: ； in Based on a predefined paradigm, by solving this function, the residuals of lines and surfaces are optimized, such that... The smaller the better, as this will ultimately result in an optimized pose. .

5. The method according to claim 1, characterized in that, The variable-size voxel map management and statistical accumulation in S40 further includes the following sub-steps: S41. Construct variable-size voxels in the map coordinate system. The size of the voxels varies with the distance from the trajectory point: the closer to the trajectory point, the smaller the voxel size; the farther from the trajectory point, the larger the voxel size. S42. When any point with timestamp t falls into a voxel, the basic statistics of that voxel are updated cumulatively: The number of point clouds within a voxel, k, increases incrementally: k = k + 1; Accumulated time statistics: ； ； in, The sum of time for all points within a voxel; The sum of squares of time represents all points within a voxel; Next, its location information is calculated: ； ； in, The coordinates of the current point. This is the transpose of the current point's coordinates. This is the cumulative coordinate value of all points within the current voxel. This is the cumulative sum of the squares of the coordinates of the current voxel.

6. The method according to claim 5, characterized in that, The voxel feature update and dynamic target determination in S40 further include the following sub-steps: S43. A feature update is triggered once for every N1 new points added within a preset voxel. The update stops when the cumulative number of voxel updates reaches the upper limit of N2. S44. Calculate the geometric centroid and covariance matrix: ； ； Where Pc is the geometric centroid of the voxel. Let be the covariance matrix of the voxel point cloud, and k be the current number of points in the voxel; S45, Regarding the covariance matrix Perform SVD eigenvalue decomposition to obtain eigenvalues ​​λ1, λ2, λ3 and corresponding eigenvectors V1, V2, V3 in ascending order; S46. Calculate the time complexity within a voxel: Simultaneously perform in-voxel time complexity calculations: ； ； in, The average time for the current voxel. The time variance of the current voxel; S47. Time-domain determination based on time complexity: setting a time variance threshold. ,like < Then the voxel is marked as a dynamic target candidate, where It is generally set to 0.5; S48. Spatial morphology determination based on eigenvalues, i.e., motion blur detection: calculating linear indices. : = (λ3 - λ2) / λ3; Set linear threshold ,like < If so, the voxel is marked as a dynamic target candidate; S49. Comprehensive Judgment: If the current voxel simultaneously satisfies the time variance... Less than the threshold And linear index Less than the threshold If the result is positive, the voxel is determined to be a dynamic voxel, and all point cloud data within it is deleted.

7. The method according to claim 6, characterized in that, In step S40, the planarity determination and planar deviation noise removal for static voxels that do not meet the dynamic target conditions are further included in the following sub-steps: S401. Use the RANSAC algorithm for coarse screening and planeness determination: Randomly select three non-collinear points within a voxel to estimate the initial plane equation Ax + By + Cz + D = 0, and calculate Pᵢ = ( for each point within the voxel) , , The algebraic distance to the initial plane: Given an interior point, determine the distance threshold from that point to the plane. Traverse all k points within the voxel and calculate the planeness score of the current plane: ； Where I is an indicator function, which is 1 when the condition is met and 0 otherwise; k is the number of point clouds within a voxel. S402. If the score is greater than the preset score threshold St, the voxel is determined to have strong planar geometric features and enters the precise planar fitting stage after coarse screening. S403. Use the least squares method to perform exact plane fitting, and optimize the plane equation parameters A, B, C, and D to satisfy: = 0； S404. Using the optimal plane parameters, calculate the Euclidean distance from each point within the voxel to the optimal plane: E = | |； Set error rejection threshold E th If E >E th If the point is found to be a plane deviation noise point, it will be removed.

8. The method according to claim 7, characterized in that, In step S40, the isolated noise removal of non-planar voxels that do not satisfy the planarity constraint further includes the following sub-steps: S405. Suppose there are n points within the current voxel, and the three-dimensional coordinates of each point are represented by a column vector Pⱼ = ( , , ), calculate the mean coordinates of the point cloud within the voxel: ； S406, Calculate the distance from each point to the geometric center. absolute Euclidean distance : ； S407, Calculate the average distance of point clouds within voxels. : ； Calculate the sample standard deviation of the distance from the point cloud to the center point. : ； S408. Outlier determination based on the statistical 3σ law: If the distance dj from a point to the center exceeds the interval (dc - 3σd, dc + 3σd), then the point is determined to be an isolated noise point and is removed.

9. The method according to claim 1, characterized in that, S50 further includes the following sub-steps: S51. Update the voxels after dynamic target removal, planar deviation noise removal and isolated noise removal to the current map for matching and optimization in subsequent frames. S52. Periodically filter voxels in the map: determine whether a voxel has been silenced by judging the distance from the center of the voxel to the center of the current matching frame and the time when the voxel was last activated. S53. If a voxel is not activated for more than a preset time threshold and its distance from the current frame exceeds a preset distance threshold, then the voxel is determined to be silent, its point cloud data is output to the disk and the corresponding memory is released.

Images