Laser SLAM system and method for removing dynamic point cloud based on motion detection

Abstract

The invention discloses a laser SLAM system and method for removing a dynamic point cloud based on motion detection, the laser SLAM system comprises an input module, an IMU pre-integration module, a dynamic removal module, a feature extraction module and a matching optimization module, and the laser SLAM method comprises the steps: carrying out the pre-integration of an IMU measurement value, and obtaining a prior pose; the dynamic point cloud is identified through motion detection, error detection suppression is completed by using clustering and near-earth detection methods, and dynamic objects in the laser point cloud are removed preferentially; using a vertical clustering method to improve the extraction of edge features of the residual static background to obtain stable geometric feature points; the feature points are transmitted to a pose optimization module for registration and factor graph optimization to obtain an optimal pose; according to the method, the dynamic point cloud can be effectively filtered in a dynamic environment, and the positioning precision of a radar odometer and the accuracy of a point cloud map are improved.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving, and particularly relates to a laser SLAM system and method for removing dynamic point clouds based on motion detection. Background Art

[0002] Simultaneous localization and mapping is a core technology for autonomous driving. Especially in the case of GNSS failure or unknown environments, autonomous localization and environmental modeling can be achieved through sensors. The SLAM technology based on lidar has become the mainstream solution for high-precision localization and mapping due to its unique advantages. The lidar emits high-frequency laser pulses and receives the reflected signals to generate high-density three-dimensional point cloud data, which can accurately capture the geometric structure and spatial distribution of objects in the environment. Compared with visual SLAM, the lidar is insensitive to light changes and can still stably output high-resolution environmental information in low-light, backlight, or nighttime scenarios.

[0003] However, most current laser SLAMs are mainly developed under the premise of static environments without fully considering dynamic objects in space. In most practical applications, the scenarios are highly dynamic environments with many moving objects such as vehicles and pedestrians. In these dynamic environments, due to the instantaneous nature of the data collected by the lidar, while acquiring static scene data, it will also capture dynamic objects such as cars and pedestrians, resulting in a significant reduction in the accuracy of lidar positioning and map construction. Therefore, how to real-time determine and remove dynamic objects in the point cloud data to achieve more accurate pose estimation and map construction has become an urgent problem to be solved in autonomous driving applications. Summary of the Invention

[0004] In order to solve the above problems existing in the prior art, the present invention provides a laser SLAM system and method for removing dynamic point clouds based on motion detection. The technical solution of the present invention is as follows:

[0005] A laser SLAM system for removing dynamic point clouds based on motion detection, characterized by comprising:

[0006] An input module for inputting time-aligned point cloud data and IMU data;

[0007] An IMU pre-integration module for inferring the system motion using the pre-integration of the IMU to obtain the initial state estimate of the SLAM system and generating an IMU odometer to provide constraints for the backend factor graph optimization;

[0008] A dynamic removal module for performing pose transformation on continuous radar point clouds to convert them to the current frame perspective to construct a background model, performing motion occlusion detection on the pre-segmented non-ground points and the background model to capture dynamic targets, and then introducing clustering and near-ground detection algorithms in false detection suppression to further clarify dynamic objects, thereby realizing the priority removal of dynamic objects in the laser point cloud;

[0009] A feature extraction module, which is used to calculate the curvature of the remaining static background points through cloud computing and improve the extraction of edge features by combining the vertical clustering method, ensuring that the extracted edge features are stable and have distinct geometric characteristics;

[0010] A matching optimization module, which is used to match the extracted static feature points with the local sub-map and estimate the pose transformation of the radar movement. If the current frame is detected as a key frame, the radar odometer and the IMU odometer are input into the factor graph for optimization to obtain the optimal pose, and the static point cloud map is updated.

[0011] A laser SLAM method for removing dynamic point clouds based on motion detection, characterized in that it includes the following steps:

[0012] Step S1: Obtain time-synchronized point cloud data and IMU data according to the lidar and the inertial measurement unit, convert the point cloud data into the image two-dimensional coordinates, and complete the depth image projection;

[0013] Step S2: Use the original IMU measurements for IMU pre-integration to obtain the initial state estimation of the laser SLAM system;

[0014] Step S3: Perform ground segmentation on the current frame point cloud, mark the ground points and non-ground points, and regard the ground points as static points;

[0015] Step S4: Transform the depth images of the previous several frames into the current frame coordinate system through coordinate transformation, perform motion occlusion detection on the non-ground points of the current frame, and preliminarily screen out the dynamic points;

[0016] Step S5: Perform false detection suppression on the dynamic points screened out in step S4. Based on the Euclidean distance as the judgment criterion, cluster with the detected dynamic points as the center, and perform ground contact detection to determine and filter out the dynamic objects;

[0017] Step S6: Calculate the curvature of the static points and extract features by combining vertical clustering;

[0018] Step S7: Perform scan matching on the extracted feature points and the local point cloud map, estimate the pose transformation of the radar movement. If the current frame is detected as a key frame, the radar odometer and the IMU odometer are input into the factor graph for optimization to obtain the optimal pose, and the static point cloud map is updated.

[0019] Further, in step S3, the specific process of performing ground segmentation on the current frame point cloud is as follows:

[0020] Step S31: Set the ground range area: Under normal circumstances, the distance between the lidar and the ground points remains within a certain range, and potential ground points are preliminarily screened according to the preset height threshold of the radar installation position;

[0021] Step S32: Considering the planar characteristics of a local ground area, use the principal component analysis method to calculate the local normal vector of each point in the ground area, and compare it with the ideal upward direction to remove the point cloud with too large normal vector deviation;

[0022] Step S33: In the depth image, start from the bottom of each column to find the first non-ground point p (i,j) , and use the first non-ground point and the previous ground point in this column as the vector to be detected V i , and set the initial reference vector V R_init , the formula is as follows:

[0023] V i = p (i,j) - p (i-1,j)

[0024] V R_init = [cosθ j , sinθ j , 0]

[0025] θ j = (j - 1)·r h

[0026] where θ j is the azimuth angle corresponding to the j-th column, and r h , r v represent the horizontal and vertical pixel sizes respectively;

[0027] Step S34: Calculate the angle between the vector to be detected and the reference vector. If the angle is less than the threshold, mark p (i,j) as a ground point and iterate the reference vector. The iteration process is as follows:

[0028]

[0029]

[0030] where α i is the angle between two adjacent vectors, and V Ri-1 is the iterative reference vector;

[0031] Step S35: Through the above process, segment the ordered LIDAR frames into ground points and non-ground points and add labels.

[0032] Furthermore, in step S4, the specific process of performing motion occlusion detection on the non-ground points of the current frame is as follows:

[0033] Step S41: Define the transformation function Γ(·) as follows:

[0034]

[0035] In the formula respectively represent the rotation matrix and translation vector from the robot coordinate system to the world coordinate system;

[0036] Step S42: In order to be able to segment moving objects in real time, convert the past N consecutive radar sequences to the current coordinate system. At this time, only moving objects will change their positions in the depth image. Convert the viewpoint of the point cloud through the transformation relationship between frames. The formula is as follows:

[0037]

[0038] where P i represents a point on the b i coordinate system, is the transformed point from b i to b j ;

[0039] Step S43: When the depth of the point cloud is less than the minimum depth stored in the current pixel and its adjacent pixels, the point is regarded as an occluded point in the depth image. When the depth of the point cloud is greater than the maximum depth stored in the current pixel and its adjacent pixels, it is considered that the point is occluded by a near point in the depth image;

[0040] Step S44: Observe the relationship between the moving object and the radar scan. The movement of the object can be divided into two categories: movement along the scan line and movement cutting the scan line. Execute two independent motion detection modules on the current non-ground point cloud. When any one of the judgment results is true, mark the current point as a dynamic point;

[0041] Step S45: Detection of an object moving by cutting the scan line. Check the occlusion situation between the point cloud and the past N images. If more than M1 depth image points are occluded by the current point, mark the point cloud as a dynamic point. The formula is as follows:

[0042]

[0043] In the formula, (u, v) represents the projection position of the point cloud p i in the depth image, represents the depth value corresponding to the point cloud p i in the k-th frame, represents the maximum depth value recorded in the corresponding pixel positions of the past depth images;

[0044] Step S46: Detection of an object moving along the scan line direction. When the moving object gradually approaches the radar along the scan line, if the points of N depth images are occluded by the current point, mark the point cloud as a dynamic point; when the moving object gradually moves away from the radar, check the occlusion situation between the point cloud and the past N images. If N depth images all occlude the current point, mark the point cloud as a dynamic point. The formula is as follows:

[0045]

[0046] where represents the minimum depth value recorded at the corresponding pixel position of the past depth image, and the recognized dynamic point set is

[0047] Further, in step 5, the specific process of suppressing false detections based on the dynamic points screened out in step S4 is as follows:

[0048] Step S51: Using the Euclidean distance as the judgment criterion, perform expansion through nearest neighbor search with the detected dynamic point as the center. If the distance from the detection center is less than the set threshold, put it into the clustering cluster C i until the points in C i no longer increase, and the clustering ends;

[0049] Step S52: Count the number of point clouds and the number of dynamic points contained in each clustering cluster and the number of dynamic points Regard the clustering with fewer point clouds as noise and remove it;

[0050] Step S53: According to the core premise that moving objects in the driving scene are in contact with the ground, perform ground contact detection on all dynamic clusters and make the following judgments:

[0051]

[0052] where th D represents the rejection coefficient, used to judge the proportion of the number of dynamic points contained in the laser cluster in the entire laser cluster, and low(*) represents the lower bound of the height range of the laser cluster;

[0053] Step S54: The clustering clusters that meet the conditions are regarded as dynamic objects, mark all points in the cluster as dynamic points and remove them, otherwise reject the dynamic points in the cluster.

[0054] Further, in step 6, the specific process of calculating the curvature of static points and extracting features in combination with vertical clustering is as follows:

[0055] Step S61: Calculate the curvature of each non-ground point in the laser scan frame, downsample the points with smaller curvature and ground points and extract them as plane features for subsequent point-to-plane matching, and regard the remaining points as candidate edge points;

[0056] Step S62: Propose a method based on vertical clustering to filter out invalid feature points. Similar to Euclidean clustering, this method sets a larger tolerance in the vertical direction, and the formula is as follows:

[0057]

[0058] In the formula represents the pitch angle of the sensor, and r th represents the clustering radius, and ρ xy and ρ z are coefficients from different axes;

[0059] Step S63: Perform vertical clustering on the candidate edge points. Select a point p, and use the KD-Tree to find the k points closest to the point p. If the relationship described in Step S62 is satisfied with the point p, add them to the clustering cluster until all candidate edge points are traversed to complete the clustering, and extract the longer clustering points as the final edge feature points.

[0060] Due to the application of the above technical solution, the advantages and beneficial effects of the present invention are as follows:

[0061] 1. The present invention uses the IMU pre-integration method that is not affected by the dynamic environment to predict the pose of the current point cloud, and combines the dynamic removal module to preferentially remove the dynamic point cloud. Through this method, the laser SLAM system can have higher robustness and positioning accuracy in a high-dynamic environment.

[0062] 2. The present invention uses the ground segmentation algorithm to quickly separate the ground points, reduce the interference of the static ground during motion detection, reduce the calculation time for identifying dynamic points, and increase the calculation efficiency; when removing dynamic points, the present invention introduces clustering and ground contact detection to suppress false detections, more accurately identify dynamic objects, can effectively filter dynamic point clouds in real time in a dynamic environment, and construct a more pure point cloud map. Description of the Drawings

[0063] Figure 1 is the overall block diagram of the laser SLAM system for removing dynamic point clouds based on motion detection of the present invention.

[0064] Figure 2 is the flowchart of the laser SLAM method of the present invention.

[0065] Figure 3 is the schematic diagram of the motion relationship between the radar and the moving object of the present invention.

[0066] Figure 4 is the trajectory comparison diagram between the method proposed in the present invention and the mainstream laser SLAM on the KITTI05 dataset in the embodiment.

[0067] Figure 5 is the trajectory comparison diagram between the method proposed in the present invention and the mainstream laser SLAM on the KITTI07 dataset in the embodiment.

[0068] Figure 6 is the dynamic point cloud recognition effect diagram of the method proposed in the present invention at the crossroads in the embodiment.

[0069] Figures 7 - 8 It is a partial comparison diagram of the method proposed in the present invention and the point cloud map constructed by LIO-SAM in the embodiment.

[0070] Figure 9 It is the global point cloud map of the KITTI07 dataset constructed by the method proposed in the present invention in the embodiment. Specific implementation manners

[0071] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0072] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific implementation manners.

[0073] The present invention discloses a laser SLAM system for removing dynamic point clouds based on motion detection, as Figure 1 shown, including:

[0074] An input module, which inputs point cloud data and IMU data with time alignment;

[0075] An IMU pre-integration module, which uses the pre-integration of the IMU to infer the system motion to obtain the initial state estimation of the laser SLAM system and generate an IMU odometer, providing constraints for the backend factor graph optimization;

[0076] A dynamic removal module, which uses continuous radar point clouds to perform pose transformation and convert them to the current frame perspective to construct a background model, performs motion occlusion detection on the pre-segmented non-ground points and the background model to capture dynamic targets, and then introduces clustering and near-ground detection algorithms in false detection suppression to further clarify dynamic objects, thereby realizing the priority removal of dynamic objects in the laser point cloud;

[0077] A feature extraction module, which calculates the curvature of the remaining static background point cloud and improves the extraction of edge features in combination with the vertical clustering method to ensure that the extracted edge features are stable and have distinct geometric characteristics;

[0078] A matching and optimization module, which matches the extracted static feature points with the local subgraph to estimate the pose transformation of the radar motion, detects whether the current frame is a key frame, inputs the radar odometer and the IMU odometer into the factor graph for optimization to obtain the optimal pose, and updates the static point cloud map.

[0079] A laser SLAM method based on the above system, as Figure 2 shown, includes the following processes:

[0080] Step S1: First, obtain time-synchronized point cloud data and IMU data based on the lidar and inertial measurement unit, convert the point cloud data into the two-dimensional image coordinates, and complete the depth image projection.

[0081] Step S2: Use the raw IMU measurements to perform IMU pre-integration to obtain the initial state estimation of the SLAM system. The specific process is as follows:

[0082] Step S21: According to the acceleration and angular velocity included in the IMU measurement, construct the IMU measurement model:

[0083]

[0084] In the formula and represent the raw measurements of the IMU in the robot coordinate system, b g and b a represent the zero biases of the gyroscope and accelerometer, η g and η a represent the noises of the gyroscope and accelerometer, represents the rotation matrix from the robot coordinate system to the world coordinate system, g w represents the gravitational acceleration;

[0085] Step S22: Denote the number of IMU measurements received between two LiDAR measurements as m, and use the IMU measurements to estimate the rotation speed and translation The calculation formulas are:

[0086]

[0087] Step S3: Mark the ground points and non-ground points through ground segmentation, and regard the ground points as static point clouds. The specific steps are as follows:

[0088] Step S31: Set the ground range area: Normally, the distance between the lidar and the ground points remains within a certain range. According to the installation position of the radar, preset the height threshold to initially screen out potential ground points;

[0089] Step S32: Considering the planar characteristics of the local ground area, use the principal component analysis method to calculate the local normal vector of each point in the ground area, and compare it with the ideal upward direction to remove the point clouds with too large normal vector deviations;

[0090] Step S33: In the depth image, start from the bottom of each column to find the first non-ground point p (i,j) , and use the first non-ground point and the previous ground point in this column as the vector to be detected Vi , and set the initial reference vector V R_init , the formula is as follows:

[0091] V i = p (i,k) - p (i-1,j) (7)

[0092]

[0093] θ j = (j - 1)·r h (9)

[0094] where θ j is the azimuth angle corresponding to the j-th column, and r h , r v represent the horizontal and vertical pixel sizes (i.e., resolution) respectively;

[0095] Step S34, calculate the angle between the vector to be detected and the reference vector. If the angle is less than the threshold, mark p (i,j) as a ground point and iterate the reference vector. The iteration process is as follows:

[0096]

[0097] where α i is the angle between two adjacent vectors, is the iterated reference vector;

[0098] Step S4, transform the depth images of the previous several frames to the current frame coordinate system according to the coordinate transformation, perform motion detection on the non-ground points of the current frame, and initially screen out the dynamic points. The specific process is as follows;

[0099] Step S41, define the transformation function Γ(·) as follows:

[0100]

[0101] Step S42, in order to be able to segment moving objects in real time, convert the past N consecutive radar sequences to the current coordinate system. At this time, only the moving objects will change their positions in the depth image. Convert the point cloud to a different viewing point through the transformation relationship between frames. The formula is as follows:

[0102]

[0103] where P i represents the point on the b i coordinate system, is the transformed point from b i to b j ;

[0104] Step S43: When the point cloud depth is less than the minimum depth saved in the current pixel and its adjacent pixels, this point is regarded as an occluded point in the depth image. When the point cloud depth is greater than the maximum depth saved in the current pixel and its adjacent pixels, it is considered that this point is occluded by a near point in the depth image.

[0105] Step S44: Observe the relationship between the moving object and the radar scan. The object motion can be divided into two categories: moving along the scan line and cutting the scan line. As Figure 3 shown, two independent motion detection modules are executed on the current non-ground point cloud. When the judgment result of any one of them is true, the current point is marked as a dynamic point.

[0106] Step S45: Detection of objects moving by cutting the scan line. At this time, the moving object will occlude the previously observed background object. Check the occlusion situation between the point cloud and the past N images. If more than M1 depth images (M1 ≤ N) have points occluded by the current point, the point cloud is marked as a dynamic point. The formula is as follows:

[0107]

[0108] In the formula, (u, v) represents the projection position of the point cloud p i in the depth image, represents the depth value corresponding to the point cloud p i in the k-th frame, represents the maximum depth value recorded in the corresponding pixel positions of the past depth images;

[0109] Step S46: Detection of objects moving along the scan line direction. When the moving object gradually approaches the radar along the scan line, the current dynamic point cloud will surely repeatedly occlude the point cloud that was visible to itself in the past. The detection formula is the same as (9). If the points of N depth images are occluded by the current point, the point cloud is marked as a dynamic point. When the moving object gradually moves away from the radar, the current point cloud will surely be repeatedly occluded by its own past point cloud. Check the occlusion situation between the point cloud and the past N images. If N depth images all occlude the current point, the point cloud is marked as a dynamic point. The formula is as follows:

[0110]

[0111] In the formula represents the minimum depth value recorded in the corresponding pixel positions of the past depth images. The identified dynamic point set is as follows:

[0112]

[0113] Step S5: Use a clustering algorithm to cluster the non-ground point cloud, and perform ground contact detection to identify and filter out dynamic objects, which specifically includes the following steps:

[0114] Step S51: Based on the Euclidean distance as the judgment standard, the detected dynamic point is expanded by the nearest neighbor search. If the distance from the detection center is less than the set threshold, it is placed in the cluster C. i In, until C i The number of points in no longer increases, and the clustering ends;

[0115] Step S52: Count the number of point clouds contained in each cluster and dynamic points Clusters with fewer point clouds are considered as noise removal;

[0116] Step S53: Based on the core premise that the moving object in the driving scene is in contact with the ground, ground detection is performed on all dynamic clusters, and the following judgment is made:

[0117]

[0118] Where th D Represents the elimination coefficient, which is used to determine the proportion of dynamic points contained in the laser cluster in the entire laser cluster. low(*) represents the lower limit of the laser cluster height range;

[0119] Step S54: The clusters that meet the conditions are regarded as dynamic objects, and all points in the cluster are marked as dynamic points and removed. Otherwise, the dynamic points in the cluster are rejected.

[0120] Step S6: Calculate the curvature of the static point and extract features by combining vertical clustering. The specific steps are as follows:

[0121] Step S61: Calculate the curvature of each non-ground point in the laser scanning frame, downsample the points with smaller curvature and the ground points and extract them as plane features for subsequent point-to-surface conversion, and use the remaining points as candidate edge points;

[0122] Step S62: Considering the characteristics of horizontal scanning of the laser radar, which is more sensitive to vertical object structures, and unstable edge points are usually scattered and lack vertical characteristics, a method based on vertical clustering is proposed to filter out invalid feature points. Similar to Euclidean clustering, this method sets a larger tolerance in the vertical direction. The formula is as follows:

[0123]

[0124] In the formula represents the pitch angle of the sensor, r th represents the cluster radius, ρ xy and ρ z are the coefficients from different axes;

[0125] Step S63: Perform vertical clustering on the candidate edge points. Select a point p, and use the KD-Tree to find the k points closest to point p. If the relationship described in formula (19) in step S62 is satisfied with point p, add them to the clustering cluster until all candidate edge points are traversed to complete the clustering, and extract the longer clustering points as the final edge points.

[0126] Step S7: Perform scan matching on the extracted feature points and the local point cloud map, estimate the pose transformation of the radar movement, perform backend factor graph optimization, and update the map. The specific process is as follows:

[0127] Step S71: Select the radar point cloud with the amplitude of pose change exceeding the threshold or having a large number of dynamic clusters in the feature point cloud of the previous frame as the key frame, and update the local point cloud map in the form of a sliding window;

[0128] Step S72: Find the feature points in the local subgraph that match the current frame by constructing point-to-line and point-to-plane constraints. Then use the Gauss-Newton method to minimize the weighted distance sum of the current matching points, and obtain the current estimated pose through iterative optimization until convergence;

[0129] Step S73: When the point cloud is the key frame, input the radar odometer and the IMU odometer into the factor graph for optimization to obtain the optimal pose, and construct a static point cloud map.

[0130] In this embodiment, to verify the effectiveness of a laser SLAM method based on motion detection to remove dynamic point clouds proposed by the present invention, experimental verification is given to illustrate that the laser SLAM method based on motion detection to remove dynamic point clouds is effective, as follows:

[0131] The publicly available dataset KITTI is selected for verification in the experiment. This dataset contains LiDAR and IMU data that meet the data requirements of the method; at the same time, this dataset provides the ground truth pose synchronized with the LiDAR, which can be used to test the performance of LIDAR SLAM.

[0132] For all experiments, the algorithm is tested on a laptop equipped with an R7-5800H CPU and 16GB of RAM; the algorithm is written in C++ and uses the Eigen library to accelerate matrix operations; the software architecture is based on the Robot Operating System ROS Noetic to achieve multi-threaded optimization and execution.

[0133] The trajectory map compared with the mainstream laser SLAM algorithm is as Figure 4 、 Figure 5As shown. The trajectory visualization results show that, compared with the trajectories of other algorithms, the trajectory of the method proposed in the present invention is closer to the original true trajectory line and has less drift in the vertical direction. When there are moving vehicles in the scene, due to the traditional method not considering the interference of dynamic objects, the trajectory deviation error increases in the curved road area. However, the method proposed in the present invention can effectively reduce the influence of dynamic objects in the scene on pose estimation through the dynamic object elimination mechanism.

[0134] Figure 6 The tracking effect of the method proposed in the present invention for dynamically identifying moving vehicles at intersections is shown, where the red ones are the identified dynamic vehicles. It can be seen that even when the vehicle leaves the image range, the point cloud can still achieve stable identification and tracking. Figure 7 、 Figure 8 A local comparison diagram of the point cloud map constructed by the method proposed in the present invention and the LIO-SAM method is shown. It can be seen that there are obvious dynamic afterimages on the ground of LIO-SAM, while the proposed algorithm removes the point cloud in the dynamic area, basically retains the original static environment information, and the generated ground is purer without being disturbed by moving vehicles and pedestrians; finally, the constructed global static map is as Figure 9 shown.

[0135] Experiments prove that the proposed method effectively suppresses the phenomenon of dynamic artifact residues in the point cloud map in terms of dynamic object filtering. At the same time, the tightly coupled architecture of IMU and lidar based on dynamic removal improves the absolute positioning accuracy of the system in dynamic interference scenarios. The proposed method uses a moving object detection method based on visible points to identify dynamic point clouds and obtains more accurate dynamic objects through false detection suppression, ensuring the filtering effect of dynamic objects. Comparative tests on different data sets show that the present method has good robustness to various scenarios.

[0136] The specific implementation schemes described above further illustrate the invention purpose, technical solutions and beneficial effects of the present invention. The above embodiments are only used to illustrate the technical solutions of the present invention, rather than limiting the protection scope of the present invention. Any equivalent changes or modifications made according to the spirit and essence of the present invention should be covered within the protection scope of the present invention.

Claims

1. A laser SLAM system for removing dynamic point clouds based on motion detection, characterized in that, It includes: An input module that inputs time-aligned point cloud data and IMU data; An IMU pre-integration module that uses the pre-integration of the IMU to infer the system motion to obtain the initial state estimation of the SLAM system and generate the IMU odometer, providing constraints for the backend factor graph optimization; A dynamic removal module that performs pose transformation on continuous radar point clouds to convert them to the current frame perspective to build a background model, performs motion occlusion detection on the pre-segmented non-ground points and the background model to capture dynamic targets, and then introduces clustering and near-ground detection algorithms in false detection suppression to further clarify dynamic objects, thereby realizing the priority removal of dynamic objects in the laser point cloud; A feature extraction module that calculates the curvature of the remaining static background point cloud and improves the extraction of edge features by combining vertical clustering methods to ensure that the extracted edge features are stable and have distinct geometric characteristics; A matching and optimization module that matches the extracted static feature points with the local sub-map and estimates the pose transformation of the radar motion. If the current frame is detected as a key frame, the radar odometer and the IMU odometer are input into the factor graph for optimization to obtain the optimal pose, and the static point cloud map is updated.

2. The laser SLAM method of the laser SLAM system for removing dynamic point clouds based on motion detection according to claim 1, wherein, It includes the following steps: Step S1: Obtain time-synchronized point cloud data and IMU data according to the lidar and inertial measurement unit, convert the point cloud data to the two-dimensional image coordinates, and complete the depth image projection; Step S2: Use the original IMU measurements for IMU pre-integration to obtain the initial state estimation of the laser SLAM system; Step S3: Perform ground segmentation on the current frame point cloud and label the ground points and non-ground points, and regard the ground points as static points; Step S4: Transform the depth images of the previous several frames to the current frame coordinate system through coordinate transformation, perform motion occlusion detection on the non-ground points of the current frame, and preliminarily screen out dynamic points; Step S5: Perform false detection suppression according to the dynamic points screened out in Step S4. Based on the Euclidean distance as the judgment criterion, cluster with the detected dynamic points as the center, and perform ground detection to determine and filter out dynamic objects; Step S6: Calculate the curvature of the static points and extract features by combining vertical clustering; Step S7: Perform scan matching on the extracted feature points and the local point cloud map, estimate the pose transformation of the radar motion. If the current frame is detected as a key frame, the radar odometer and the IMU odometer are input into the factor graph for optimization to obtain the optimal pose, and the static point cloud map is updated.

3. The laser SLAM method according to claim 2, wherein In Step S3, the specific process of performing ground segmentation on the current frame point cloud is as follows: Step S31: Set the ground range area: Under normal circumstances, the distance between the lidar and the ground points remains within a certain range. According to the installation position of the radar, a height threshold is preset to preliminarily screen out potential ground points; Step S32: Considering the planar characteristics of the local ground area, use the principal component analysis method to calculate the local normal vector of each point in the ground area, and compare it with the ideal upward direction to remove the point cloud with too large normal vector deviation; Step S33: Find the first non-ground point p starting from the bottom of each column in the depth image (i,j) , and use the first non-ground point and the previous ground point in this column as the vector V to be detected i , and set the initial reference vector V R_init , and the formula is as follows: V i = p (i,j) -p (i-1,j) V R_init = [cosθ j , sinθ j , 0] θ j =(j - 1)·r h where θ j is the azimuth angle corresponding to the j-th column, r h , r v represent the horizontal and vertical pixel sizes, respectively; Step S34: Calculate the angle between the vector to be detected and the reference vector. If the angle is less than the threshold, mark p (i,j) as a ground point and iterate the reference vector. The iteration process is as follows: where α i is the included angle between two adjacent vectors, and V Ri-1 is the reference vector for iteration; Step S35: Through the above process, the ordered LIDAR frames are segmented into ground points and non-ground points and labeled.

4. The laser SLAM method according to claim 3, wherein In Step S4, the specific process of performing motion occlusion detection on the non-ground points of the current frame is as follows: Step S41: define the transformation function Γ(·) as follows: wherein respectively represent the rotation matrix and the translation vector from the robot coordinate system to the world coordinate system; Step S42: To segment moving objects in real time, the past N consecutive radar sequences are converted to the current coordinate system. At this time, only the moving objects will change their positions in the depth image. The point cloud is converted to the viewpoint through the transformation relationship between frames. The formula is as follows: Among which P i represents a point b i on the coordinate system, is the transformed point from b i to b j ; Step S43: When the point cloud depth is less than the minimum depth stored in the current pixel and its adjacent pixels, the point is considered to be an occluded point in the depth image. When the point cloud depth is greater than the maximum depth stored in the current pixel and its adjacent pixels, the point is considered to be occluded by a near point in the depth image. Step S44: Observe the relationship between the moving object and the radar scan, and classify the object's motion into two categories: motion along the scan line and motion cutting the scan line. Execute two independent motion detection modules on the current non-ground point cloud. If the result of either judgment is true, mark the current point as a dynamic point. Step S45: Cut the object detection of the scanning line movement, check the occlusion of the point cloud and the past N images, if there are more than M1 depth image points occluded by the current point, then mark the point cloud as a dynamic point, the formula is as follows: where (u, v) represents the point cloud p i is the projection position in the depth image, represents the point cloud p in the k-th frame i corresponding depth value, represents the maximum depth value recorded at the corresponding pixel position in the past depth image; Step S46: Detect objects moving along the scan line. When the moving object gradually approaches the radar along the scan line, if points in N depth images are blocked by the current point, the point cloud is marked as a dynamic point. When the moving object gradually moves away from the radar, the point cloud is checked for occlusion with the past N images. If all N depth images block the current point, the point cloud is marked as a dynamic point. The formula is as follows: wherein represents the minimum depth value recorded at the pixel position corresponding to the past depth image, and the recognized dynamic point set is 5. The laser SLAM method according to claim 4, wherein In step 5, the specific process of suppressing false detection based on the dynamic points screened in step S4 is as follows: Step S51: Using the Euclidean distance as the judgment criterion, perform expansion through nearest neighbor search with the detected dynamic point as the center. If the distance from the detection center is less than the set threshold, put it into the clustering cluster C i until the points in C i no longer increase and the clustering ends; Step S52: Count the number of point clouds and the number of dynamic points included in each clustering cluster and the number of dynamic points Regard the clustering clusters with fewer point clouds as noise and remove them; Step S53: Based on the core premise that the moving object in the driving scene is in contact with the ground, ground detection is performed on all dynamic clusters, and the following judgment is made: where th D represents the rejection coefficient, which is used to judge the proportion of the number of dynamic points included in the laser cluster in the entire laser cluster, and low(*) represents the lower bound of the laser cluster height range; Step S54: The clusters that meet the conditions are regarded as dynamic objects, and all points in the cluster are marked as dynamic points and removed. Otherwise, the dynamic points in the cluster are rejected.

6. The laser SLAM method according to claim 5, wherein In step 6, the specific process of calculating the curvature of the static points and extracting features in combination with vertical clustering is as follows: Step S61: Calculate the curvature of each non-ground point in the laser scanning frame, downsample the points with smaller curvature and the ground points and extract them as plane features for subsequent point-to-surface matching, and use the remaining points as candidate edge points; Step S62: A method based on vertical clustering is proposed to filter out invalid feature points. Similar to Euclidean clustering, this method sets a larger tolerance in the vertical direction. The formula is as follows: wherein represents the pitch angle of the sensor, r th represents the clustering radius, ρ xy and ρ z are coefficients from different axes; Step S63: Perform vertical clustering on the candidate edge points. Select a point p and use KD-Tree to find the k points closest to point p. If they meet the relationship described in step S62 with point p, they are added to the cluster cluster until all candidate edge points are traversed and clustering is completed. The longer cluster point is extracted as the final edge feature point.

Images