A laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor

Abstract

This invention discloses a laser inertial dynamic SLAM system based on pseudo-occupancy grid descriptors. The system consists of a laser inertial SLAM system, a ground constraint subsystem, and a dynamic point cloud filtering subsystem. By adding a ground constraint subsystem, this invention reduces the system's z-axis cumulative error, improves the robustness of ground segmentation, and thus enhances the robustness and accuracy of the dynamic point cloud filtering subsystem. The dynamic point cloud filtering subsystem converts the laser radar point cloud frame and the local map into pseudo-occupancy grid descriptors based on concentric circle regions. It then compares the descriptor differences between the current point cloud frame and historical point cloud frames, as well as between the current point cloud frame and the local map point cloud, removing dynamic point clouds from the current frame and the local submap respectively, thereby improving the accuracy of dynamic point cloud filtering and the accuracy of SLAM system pose estimation. Due to the introduction of the ground constraint subsystem, this invention can still operate in large-scale dynamic scenes.

Description

Technical Field

[0001] The invention relates to a laser inertial dynamic SLAM system based on a pseudo-occupancy grid descriptor, belonging to the technical field of mobile robot autonomous navigation and mapping. Background Art

[0002] SLAM (simultaneous localization and mapping) is essential for mobile robots to move safely in unfamiliar environments. In recent years, laser inertial SLAM solutions have received increasing attention in the field of autonomous movement and navigation of mobile robots. The data preprocessing and feature matching of traditional laser inertial SLAM systems are based on the static assumption that objects in the surrounding environment are stationary. In actual scenarios, there are dynamic objects such as moving vehicles and personnel in the working environment of mobile robots. These dynamic objects will introduce bad values ​​into the feature matching link of SLAM, thereby affecting the accuracy of the final pose estimation and the mapping accuracy. At present, most SLAM methods for dynamic scenes use methods based on deep learning. These methods are heavily dependent on the prior information provided in advance, and face problems such as large computational complexity and poor robustness in complex scenes. In addition, there are some methods that only remove dynamic point clouds by comparing the differences between the current scanned point cloud and the local map, but most of these methods are designed for local maps and have poor filtering effects on dynamic point clouds in the current point cloud frame. Summary of the invention

[0003] The present invention aims to solve the above existing technical problems and proposes a laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor.

[0004] In order to achieve the above object, the present invention provides the following technical solutions:

[0005] A laser inertial dynamic SLAM system based on pseudo-occupancy grid descriptor comprises a laser inertial SLAM subsystem, a ground constraint subsystem and a dynamic point cloud filtering subsystem.

[0006] The laser inertial SLAM subsystem consists of a point cloud preprocessing module, an IMU pre-integration module, a feature matching module, a backend optimization module, and a subgraph construction module. The point cloud preprocessing module acquires the laser radar point cloud data in real time, and filters and removes motion distortion from the point cloud data; the IMU pre-integration module obtains the initial pose of the next frame based on the system optimized pose and IMU data; the feature matching module obtains the pose data of the laser radar by matching the current point cloud data and the local map; and the backend optimization module uses graph optimization to obtain the final optimized radar pose.

[0007] The dynamic point cloud filtering module is composed of a concentric circle area extraction module, a pseudo-occupancy grid descriptor conversion module, a current frame dynamic point cloud filtering module, and a local map dynamic point cloud removal module. The concentric circle area extraction module extracts the point cloud within the concentric circle with the center of the radar as the center and the set distance as the radius according to the characteristics of the laser radar point cloud distribution; the pseudo-occupancy grid descriptor conversion module converts the point cloud extracted from the concentric circle area into different grids according to the form of a polar coordinate system; the current frame dynamic point cloud filtering module removes the dynamic point cloud in the current frame by comparing the descriptor height feature difference between the current frame laser radar point cloud information and the historical frame; the local map dynamic point cloud filtering module removes the dynamic point cloud in the local map by comparing the descriptor height feature difference between the current frame laser radar point cloud and the local map.

[0008] The ground constraint subsystem consists of a ground segmentation module, a ground fitting module, and a ground constraint construction module. The ground segmentation module segments ground point clouds and non-ground point clouds from the static point cloud data of the current frame and the static point cloud data of the local map; the ground fitting module fits the parameters of the ground plane according to the ground point cloud; and the ground constraint construction module obtains the current ground constraint factor according to the ground plane parameters.

[0009] The method for filtering the local map dynamic point cloud in the dynamic point cloud filtering module includes the following steps:

[0010] S1: Extract pre-processed radar point cloud data within the concentric circle area Local map point cloud data constructed in real time by the laser inertial SLAM system , the radius and height of the concentric circle area are as follows:

[0011]

[0012] S2: extract the point cloud in the concentric circle area From the LiDAR coordinate system According to the IMU odometer posture obtained by the IMU pre-integration module in the laser inertial SLAM subsystem Go to world coordinate system The specific expressions are as follows:

[0013]

[0014] S3: Convert radar point cloud data from rectangular coordinate system format Convert to polar coordinates ,in , The concentric circle area is divided into fan-shaped areas according to azimuth and radial rules. , specifically expressed as follows:

[0015]

[0016] Here , Indicates the number of radial divisions and azimuth divisions. Project the point cloud to a fan-shaped area according to the polar coordinates of the laser point cloud In each sector area, record Point Cloud Maximum value and minimum value , each sector area The pseudo-occupancy probability is .

[0017] S4: According to the radar point cloud of the current frame obtained in S3 and local maps ,

[0018] Traverse each sector area , calculate the scanning height ratio of the current frame point cloud and the local map point cloud in the sector area And expressed as follows:

[0019]

[0020] According to the calculation Values ​​and threshold settings By comparison, the suspected dynamic area is determined, and the adjacent grid areas of the suspected dynamic area are detected.

[0021] The step S4 specifically includes the following process:

[0022] S4.1 Traverse the divided grid areas within the concentric circle area, according to The number of point clouds is calculated to initially eliminate grid areas with a small number of point clouds to reduce the amount of subsequent calculations.

[0023] S4.2 For areas where the number of current scan point cloud and local map point cloud meets the set threshold requirements Calculate the scan height ratio of the area If the current region , it is considered that the difference between the current scan point cloud and the local map point cloud in this area is small, and it is determined to be the area to be inspected; if the current area , it is considered that the current scan point cloud and the local map point cloud in this area are quite different, and then the point cloud that satisfies The area is determined as a suspected dynamic area.

[0024] S4.3 loops through the suspected dynamic area and checks whether the point cloud height difference of the local map in the suspected dynamic area is greater than the set minimum height threshold of a normal moving object. If the height difference is greater than the threshold, the local map of the area is determined to be a dynamic area.

[0025] S5: Satisfaction The grid area is detected by the neighboring grids. If the neighboring grids of the grid are all suspected dynamic areas, the grid is determined to be a suspected dynamic area.

[0026] Furthermore, step S5 is specifically implemented as follows:

[0027] S5.1 performs ground segmentation on the point clouds in the sector grid area determined as the dynamic area. First, the point clouds in the area are sorted in the z-axis direction, and a certain number of point clouds are fitted from low to high as the initial point clouds. Then, the distances from other point clouds to the plane are recalculated, and the point clouds with smaller distances are added to the plane set. This step is repeated three times to obtain the final ground point cloud and non-ground point cloud.

[0028] S5.2 Ground point cloud in the dynamic area Remove from the point cloud set of the area to obtain the dynamic point cloud of the area .

[0029] S5.3 Use the point cloud currently scanned for the area to replace the original point cloud for the area in the local map.

[0030] S6: Segment the ground point cloud of the suspected dynamic area, and the non-ground point cloud in the area is determined to be a dynamic point cloud, and the point cloud of the area in the current frame replaces the point cloud of the area in the local map.

[0031] Furthermore, the current frame dynamic point cloud filtering method in the dynamic point cloud filtering module includes the following steps:

[0032] (1): Store the preprocessed current frame lidar point cloud and the corresponding IMU odometer pose in the queue respectively.

[0033] (2): The current frame laser radar point cloud and stored in the queue Frame LiDAR Point Cloud Converted to the world coordinate system, the specific implementation is as follows:

[0034]

[0035] in, is the point cloud pose of the current frame, for Frame point cloud pose.

[0036] 3): The point cloud in the world coordinate system in step (2) and The point cloud is obtained through the local map dynamic point cloud removal module (this means the same way as the laser radar is converted into a pseudo-occupancy grid descriptor, not directly through the local map dynamic point cloud removal module) and The corresponding pseudo occupancy grid descriptor.

[0037] (4): Traverse each sector grid area and scan according to the height ratio , find the suspected dynamic area in the point cloud of the current frame.

[0038] (5): Free space detection of point clouds in suspected dynamic areas;

[0039] S5-1 traverses the point clouds in the suspected dynamic area of ​​the current frame point cloud and performs plane fitting on the nearby point clouds.

[0040] S5-2 Search If the laser beam that scans a suspected dynamic point cloud in the frame passes through a plane near the point cloud, the point cloud is considered to be a dynamic point cloud; if the laser beam hits a plane near the point cloud, the point cloud is considered to be a static point cloud.

[0041] (6): Perform local plane constraint detection on the dynamic point cloud of the current frame that has completed free space detection.

[0042] S6-1 Traverse the dynamic point cloud detected in free space and use the nearest neighbor search to query the points near the point cloud. Laser point cloud in frame.

[0043] S6-2 Fitting queries using principal component analysis The plane where the laser point cloud in the frame is located calculates the distance between the current dynamic point cloud and the plane. If the distance is less than the set threshold, the point cloud is considered to be a misjudged point and is restored to a static point cloud in the current scan point cloud set.

[0044] (7): The dynamic point cloud obtained in step (6) is used as the initial point cloud, and the final dynamic point cloud is obtained by the DBSCAN clustering algorithm. Finally, the dynamic point cloud is subtracted from the current frame point cloud to obtain the current frame lidar static point cloud.

[0045] Preferably, steps (5) and (6) are free space detection and local plane constraint detection in the suspected dynamic area, both of which aim to reduce the filtering of erroneous dynamic point clouds.

[0046] The workflow of the ground restraint subsystem in the present invention includes the following steps:

[0047] ①: Sort the point clouds filtered by dynamic point clouds by height, and select the lowest N point clouds as the initial ground point clouds.

[0048] ②: Perform principal component analysis on the initial ground point cloud to obtain the initial ground plane model parameters.

[0049] ③: Calculate the distance between all point clouds in the current frame and the plane model, and further make a distance threshold judgment to obtain new ground point clouds and non-ground point clouds.

[0050] ④: Repeat the above steps ② and ③ until the final ground point cloud and ground plane model parameters are obtained.

[0051] ⑤: Construct ground constraints based on the current frame ground plane model parameters and the local map ground plane model parameters and add them to the back-end optimization module of the laser inertial subsystem.

[0052] 5-1 Plane Pass Indicates that the current local map ground fitted in step ④ is Predicting pose based on IMU From world coordinate system to In the radar coordinate system, the specific form is as follows:

[0053]

[0054] in, is the plane normal vector of the local map ground in the laser radar coordinate system,

[0055] Is , and The plane normal vector composed of is the intercept of the local map ground in the laser radar coordinate system, represents the plane intercept; and Transformation matrix from the world coordinate system to the current lidar coordinate system The rotational and translational components of .

[0056] 5-2 Based on the current frame laser point cloud ground and local map ground converted to radar coordinate system Construct ground constraint equations separately , which is expressed as follows:

[0057]

[0058] in, is the plane normal vector The component in the axial direction, is the plane normal vector Component in the axial direction; The ground constraint equation is defined as Defined as ;

[0059] 5-3 Constructing the ground constraint residual model based on the ground constraint equation , specifically expressed as follows:

[0060]

[0061] in, , , Respectively of , ;

[0062] 5-4 Calculate the Jacobian matrix from the residual model , add a custom ground factor in the backend optimization module. Specifically, input the residual model into the express automatic differentiation of the GTSAM library to obtain the Jacobian matrix .

[0063] The beneficial effects of the present invention are as follows:

[0064] A laser inertial dynamic SLAM system based on pseudo-occupancy grid descriptor is proposed, which can efficiently and accurately remove dynamic point clouds in the current frame and local sub-map, eliminate the interference of dynamic objects in complex dynamic environments on the SLAM system, and improve the positioning accuracy of the SLAM system. By adding a current frame point cloud filtering module in the laser inertial dynamic SLAM system and using pseudo-occupancy grid descriptors to quickly determine the dynamic area of ​​the current frame, the current frame dynamic point cloud filtering speed and the final dynamic point cloud filtering effect are greatly improved; by adding a ground constraint subsystem, the elevation error of the SLAM system in large-scale dynamic scenes is reduced, and the stability of the ground point cloud filtering part in the dynamic point cloud filtering module is improved, thereby improving the dynamic point cloud filtering effect and mapping effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 It is a system block diagram of the laser inertial dynamic SLAM algorithm based on pseudo-occupancy grid descriptor of the present invention.

[0066] Figure 2 It is a grid area map based on concentric circle area division.

[0067] Figure 3 Schematic diagram of scanning height detection.

[0068] Figure 4It is a schematic diagram of plane fitting of principal component analysis of ground point cloud.

[0069] Figure 5 It is a schematic diagram of point cloud ground segmentation.

[0070] Figure 6 This is the dynamic point cloud filtering effect diagram. DETAILED DESCRIPTION

[0071] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments of the present invention:

[0072] like Figure 1 As shown in the figure, a laser inertial dynamic SLAM system based on pseudo-occupancy grid descriptor is shown in the figure. The specific process is as follows:

[0073] Step 1: Complete the calibration of the LiDAR and IMU, use the LiDAR to obtain point cloud data in a dynamic environment, input the point cloud data into the point cloud preprocessing module, and obtain point cloud data without motion distortion.

[0074] Step 2: Input the current frame point cloud data, the local map point cloud constructed by the local map construction module, and the current frame IMU odometer data estimated by the IMU pre-integration module into the dynamic point cloud filtering subsystem to filter out the dynamic point cloud. The dynamic point cloud filtering subsystem filters out the dynamic point clouds in the current frame and the local map respectively.

[0075] The filtering process of the current frame dynamic point cloud is as follows: (1) The current frame point cloud and the historical frame point cloud are filtered according to Figure 2 The concentric circle areas shown are extracted and converted into pseudo occupancy grid descriptors respectively.

[0076] (2) Traverse all grids according to the following Figure 3 The scanning height detection compares the height difference of the pseudo-occupancy grid descriptors of the current frame point cloud and the historical frame point cloud, and quickly finds the grid area with a large difference in descriptor height and determines it as the suspected dynamic area of ​​the current frame.

[0077] (3) Obtain the point cloud in the suspected dynamic area of ​​the current frame as the suspected dynamic point cloud and perform free space detection on it. Free space refers to the space where the laser beam reaches the laser point cloud from the radar position. Traverse this part of the suspected dynamic point cloud and use the nearest neighbor search method to find out whether each suspected dynamic point cloud is in the free space of the laser point cloud of the historical frame. The suspected dynamic point cloud that is not in the free space is re-added to the static point set.

[0078] (4) In order to further reduce the number of misjudged points, the current dynamic point cloud set is traversed to find the point cloud near the current point cloud in the historical frame for plane fitting, and the distance between the point and the plane is calculated. The point cloud with a distance less than the set threshold is judged as a static point on a plane such as a wall and is deleted from the dynamic point cloud set of the current frame.

[0079] (5) The remaining point clouds in the dynamic point cloud set are clustered using the DBSCAN clustering algorithm, the point clouds outside the clusters are removed from the current dynamic point cloud set, and the remaining clusters are merged into the final dynamic point cloud.

[0080] The dynamic point cloud filtering process of the local map is as follows:

[0081] (1) The current frame point cloud and the local map point cloud are also based on Figure 2 The concentric circle areas shown are extracted and converted into pseudo occupancy grid descriptors respectively.

[0082] (2) Traverse each grid area and calculate the point cloud height difference between the pseudo-occupancy grid descriptor of the current frame and the point cloud height difference between the pseudo-occupancy grid descriptor of the local map in each area. Compare the ratio of the height difference between the two areas to find the suspected dynamic area of ​​the local map and the suspected dynamic point cloud area of ​​the current frame.

[0083] (3) Traverse all suspected dynamic point cloud areas of the current frame and perform neighborhood grid detection on them. Check the grid areas near the area. If all nearby areas are suspected dynamic areas of the local map, then the area is also determined to be a suspected dynamic area of ​​the local map.

[0084] (4) Traverse all suspected dynamic areas of the local map, perform ground segmentation on the point cloud in each grid area respectively, regard the non-ground point cloud of each area as a dynamic point and add it to the dynamic point cloud set of the local map, and replace the point cloud of the area in the local map with the point cloud of the area in the current frame.

[0085] Step 3: Input the current frame static point cloud and the local map static point cloud that have passed through the dynamic point cloud filtering subsystem into the ground constraint subsystem to construct the ground constraint and add it to the backend optimization module. The construction process of the ground constraint is as follows:

[0086] (1) Sort the point cloud after dynamic filtering by height and select the lowest The point cloud is regarded as the initial ground point cloud.

[0087] (2) If Figure 4 As shown in Figure 1, the principal component analysis is performed on the initial ground point cloud to obtain the initial ground plane model parameters. Figure 5 Shows the effect of ground point cloud extraction.

[0088] (3) Calculate the distance between all point clouds in the current frame and the plane model, and further make a distance threshold determination to obtain new ground point clouds and non-ground point clouds.

[0089] (4) Repeat the above steps (2) and (3) according to the number of ground fitting iterations until the final ground point cloud and ground plane model parameters are obtained.

[0090] (5) The local map obtained by fitting Predicting pose based on IMU From world coordinate system to In the radar coordinate system. According to the current frame laser point cloud ground and local map ground converted to radar coordinate system Construct ground constraint equations respectively, and then construct ground constraint residual model based on ground constraint equations , and finally the Jacobian matrix is ​​calculated by the residual model , add custom ground factors in the backend optimization module.

[0091] Step 4: Perform scan-to-map matching on the static point cloud of the current frame obtained by the dynamic point cloud filtering subsystem and the static point cloud of the local map, and input the initial value of the lidar pose obtained by matching into the back-end optimization module.

[0092] Step 5: In the back-end optimization module, use the GTSAM optimization library to input the odometer factor constructed by the lidar pose and the ground factor constructed by the ground constraint subsystem into the factor graph model for optimization, and finally optimize to obtain the accurate radar pose.

[0093] Step 6: Input the current frame radar point cloud into the local map construction module according to the optimized pose to build the local map in real time. At the same time, input the optimized lidar pose into the IMU pre-integration module to optimize the bias and IMU odometer pose.

[0094] Experimental Section

[0095] The laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor proposed in the present invention is used to perform real-time positioning and mapping on the high dynamic dataset UrbanLoco, and the errors of the posture trajectories of the results of running different algorithms are compared.

[0096] The UrbanLoco dataset is collected in a highly urbanized scene with a large number of moving objects. The experiment selected the CAMarktStreet and CARussianHill sequences in the UrbanLoco dataset for the experiment.

[0097] Table 1 Comparison of absolute trajectory error RMSE (root mean square error) of different algorithms in UrbanLoco dataset

[0098] Dataset LOAM LIO-SAM RF-LIO The present invention CAMarktStreet 203.78 62.43 15.89 8.89 CARussianHill 175.69 36.89 12.17 10.23

[0099] like Figure 6 The figure shows the effect of dynamic point cloud filtering of the present invention. Compared with LIO-SAM, the absolute trajectory accuracy of the present invention on CAMarktStreet and CARussianHill data sets is improved by 85.76% and 72.26%. Since the present invention has an additional ground constraint subsystem in addition to the dynamic point cloud filtering function, the system elevation error is greatly reduced. Compared with the advanced dynamic SLAM system RF-LIO, the absolute trajectory accuracy is improved by 44.05% and 15.94%. These results show that the laser inertial dynamic SLAM system based on pseudo-occupancy grid descriptor proposed by the present invention can have a better effect in high dynamic scenes.

[0100] It should be noted that the specific implementation is only an explanation and description of the technical solution of the present invention, and cannot be used to limit the scope of protection of the rights. Any partial changes made according to the claims and description of the present invention should still fall within the scope of protection of the present invention.

Claims

1. A laser inertial dynamic SLAM system based on pseudo-occupancy grid descriptor, characterized in that: The system consists of a laser inertial subsystem, a ground constraint subsystem, and a dynamic point cloud filtering subsystem; The laser inertial subsystem consists of a point cloud preprocessing module, an IMU pre-integration module, a feature matching module, a back-end optimization module, and a local map construction module; the point cloud preprocessing module acquires the laser radar point cloud data in real time, and filters and removes motion distortion of the point cloud data; the IMU pre-integration module obtains the initial pose of the next frame according to the pose optimized by the back-end optimization module and the acquired IMU data; the feature matching module obtains the pose data of the laser radar by matching the current point cloud data processed by the point cloud preprocessing module with the local map constructed by the local map construction module; the back-end optimization module obtains the final optimized radar pose by using graph optimization; the local map construction module obtains the local map by splicing the point cloud according to the radar pose optimized by the back-end optimization module; The dynamic point cloud filtering subsystem consists of a concentric circle area extraction module, a pseudo-occupancy grid descriptor conversion module, a current frame dynamic point cloud filtering module, and a local map dynamic point cloud removal module; the concentric circle area extraction module extracts point clouds within concentric circles with the center of the radar as the center and a set distance as the radius according to the distribution of the laser radar point cloud; the pseudo-occupancy grid descriptor conversion module converts the point clouds extracted from the concentric circle areas into different grid areas in the form of a polar coordinate system to obtain corresponding descriptor height features; The current frame dynamic point cloud filtering module removes the dynamic point cloud in the current frame by comparing the difference in descriptor height features between the current frame and the selected historical frame; the local map dynamic point cloud filtering module removes the dynamic point cloud in the local map by comparing the difference in descriptor height features between the current frame and the local map; The ground constraint subsystem is composed of a ground segmentation module, a ground fitting module, and a ground constraint construction module; the ground segmentation module segments ground point cloud and non-ground point cloud from the current frame static point cloud data and the local map static point cloud data; the ground fitting module fits the parameters of the ground plane according to the ground point cloud; the ground constraint construction module obtains the current ground constraint factor according to the ground plane parameters; The implementation of the local map dynamic point cloud removal module in the dynamic point cloud filtering subsystem includes the following steps: S1: Extract the preprocessed current frame radar point cloud P within the concentric circle area t The local map point cloud M constructed in real time by the laser inertial SLAM system t , the radius R and height H of the concentric circle area are as follows: Among them, R max Indicates the maximum radius, H min Indicates the minimum height, H max Indicates the maximum height; S2: extract the current frame point cloud P from the concentric circle area t The IMU odometer pose obtained from the IMU pre-integration module in the laser inertial subsystem described in the lidar coordinate system {L} Switch to the world coordinate system {W}, which is expressed as follows: Among them, P t L Represents the point cloud P in the laser radar coordinate system {L} t ;P t W Represents the point cloud P in the world coordinate system {W} t ; S3: Convert the radar point cloud data in the world coordinate system {W} from the rectangular coordinate format (x, t, z) to the polar coordinate format (ρ, θ), where θ=arctan2(y,x); the concentric circle area is divided into multiple sector areas according to radial direction and angle bin(i,j)=(ρ n ,θ n ), which is specifically expressed as follows: Among them, N r 、N θ Indicates the number of radial divisions and azimuth divisions; i represents the radial coordinate, j represents the angular coordinate, and n represents the nth sector area; project the point cloud into the sector area bin(i,j) according to the polar coordinates of the laser point cloud, and record the maximum z value of the point cloud in each sector area bin(i,j). max and the minimum value z min , the descriptor height feature corresponding to each sector area bin(i,j) is Δh(i,j)=z max (i,j)-z min (i,j); S4: Based on the descriptor height feature Δh of each grid area of ​​the current frame point cloud obtained in step S3 Q (i, j) and the descriptor height feature Δh of the local map point cloud M (i, j), traverse each sector area bin(i, j), calculate the scanning height ratio SHR of the current frame point cloud and the local map point cloud in the sector area and express it as follows: The calculated SHR value is compared with the set threshold value ATH to determine the area where the dynamic point cloud is suspected to exist; S5: To meet SHR <ATH,Δh Q (i,j)>Δh M The grid area of ​​(i, j) is used for neighborhood grid detection; S6: Segment the ground point cloud of the suspected dynamic area obtained by the neighborhood grid detection, the non-ground point cloud in the area is determined to be a dynamic point cloud, and the point cloud of the area in the current frame is used to replace the point cloud of the area in the local map; The implementation of the current frame dynamic point cloud filtering module in the dynamic point cloud filtering subsystem includes the following steps: (1): The preprocessed lidar point cloud P t And the corresponding IMU odometer posture T are stored in the queue respectively; (2): The current frame laser radar point cloud P t and the tkth frame of the laser radar point cloud P stored in the queue t-k Converted to the world coordinate system, the specific implementation is as follows: in, is the point cloud pose of the current frame, is the point cloud pose of the tk frame; (3): The point cloud P in the world coordinate system in step (2) t W and P t-k W The point cloud P is obtained by the local map dynamic point cloud removal module. t W and P t-k W the corresponding pseudo occupancy grid descriptor; (4): Traverse each sector grid area and find the suspected dynamic area in the current frame point cloud according to the scanning height ratio SHR; (5): Free space detection of point clouds in suspected dynamic areas; (6): Perform local plane constraint detection on the dynamic point cloud of the current frame that has completed free space detection; (7): The dynamic point cloud obtained through local plane constraint detection is used as the initial point cloud, and the final dynamic point cloud is obtained by the DBSCAN clustering algorithm on the initial point cloud; finally, the dynamic point cloud is subtracted from the current frame point cloud to obtain the current frame lidar static point cloud.

2. A laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor according to claim 1, characterized in that: The workflow of the ground restraint subsystem includes the following steps: ①: Sort the point clouds filtered by dynamic point clouds by height, and select the lowest N point clouds as the initial ground point clouds; ②: Perform principal component analysis on the initial ground point cloud to obtain the initial ground plane model parameters; ③: Calculate the distance between all point clouds and the plane model in the current frame, and further make distance threshold determination to obtain new ground point clouds and non-ground point clouds; ④: Repeat the above steps ② and ③ until the final ground point cloud and ground plane model parameters are obtained; ⑤: Construct ground constraints based on the current frame ground plane model parameters and the local map ground plane model parameters and add them to the back-end optimization module of the laser inertial subsystem.

3. A laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor according to claim 2, characterized in that: Step S4 specifically includes the following process: S4.1 traverses the divided grid areas within the concentric circle area, and preliminarily removes the grid areas with point cloud numbers less than a threshold value according to the number of point clouds in bin (i, j); S4.2 For the area bin(i,j) where the number of the current scan point cloud and the local map point cloud both meet the set threshold requirements, calculate the scanning height ratio SHR of the area; divide it into the area to be inspected or the suspected dynamic area according to the scanning height ratio SHR; S4.3 loops through the suspected dynamic area and checks whether the point cloud height difference of the local map in the suspected dynamic area is greater than the set minimum height threshold of a normal moving object; if the height difference is greater than the threshold, the local map of the area is determined to be a dynamic area.

4. A laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor according to claim 3, characterized in that: The determination of the area to be inspected or the suspected dynamic area in step S4.2 is as follows: If the SHR of the current area is ≥ ATH, it is considered that the difference between the current scan point cloud and the local map point cloud in this area is small, and it is determined as the area to be inspected; if the SHR of the current area < ATH, it is considered that the difference between the current scan point cloud and the local map point cloud in this area is large, and then the area that satisfies Δh M (i,j) > Δh Q (i,j) is determined as the suspected dynamic area.

5. A laser inertial dynamic SLAM system based on pseudo occupancy grid descriptor according to claim 2 or 3, characterized in that: The step ⑤ specifically includes the following process: S5.1 plane through n x x+n y y+n z z+d=0 means that the current local map ground GP fitted in step ④ is M Predicting posture based on IMU Convert from world coordinate system to current frame laser point cloud ground GP Q In the radar coordinate system, the specific form is as follows: GP M ′=[n' d'] Where n'=R1*n * is the plane normal vector of the local map ground in the laser radar coordinate system, n * By n x 、n y and n z The plane normal vector composed of d'=d-n'*t1 is the intercept of the local map ground in the laser radar coordinate system, d represents the plane intercept; R1 and t1 are the transformation matrices from the world coordinate system to the current laser radar coordinate system The rotational and translational components of S5.2 Based on the current frame laser point cloud ground GP Q And the local map ground GP converted to the radar coordinate system M 'Construct the ground constraint equation T(GP) respectively, expressed as follows: Among them, n x is the component of the plane normal vector in the x-axis direction, n y is the component of the plane normal vector in the y-axis direction; The ground constraint equation T(GP) is defined as [αβd] T ; S5.3 Constructing the ground constraint residual model based on the ground constraint equation t , specifically expressed as follows: and t =T(GP M,t ')-T(GP Q ,t)=[α' M,t -α Q,t β' M,t -β Q,t d' M,t -d Q,t ] T Among them, α' M,t , β' M,t , d' M,t Respectively represent T(GP M,t ') of α, β, d; S5.4 obtains the Jacobian matrix J by calculating the residual model, and adds a custom ground factor in the back-end optimization module; specifically, the residual model is input into the express automatic differentiation of the GTSAM library to obtain the Jacobian matrix J.

Images