Dynamic point cloud removal method based on grid division

Abstract

A method for removing dynamic point clouds based on grid division belongs to the technical field of high-precision map construction, navigation and planning. The present invention solves the problem that existing methods are difficult to achieve high-precision and real-time dynamic point cloud data removal under conditions of no illumination and no prior information. The present invention identifies the grids where dynamic point clouds exist by calculating and comparing the point cloud descriptors of the current key frame and the local map corresponding to the grid in the area of ​​interest, thereby effectively removing the dynamic point clouds in the current scanning frame of the lidar, improving the accuracy of dynamic point cloud data removal, and ultimately being able to construct an accurate global point cloud map after filtering out the dynamic point clouds. Moreover, the implementation of the method of the present invention is not limited to illumination and prior conditions, and can still be implemented under conditions of no illumination and no prior information, and can work around the clock. The method of the present invention can be used to remove dynamic point clouds.

Description

Technical Field

[0001] The present invention belongs to the technical field of high-precision map construction, navigation and planning, and specifically relates to a dynamic point cloud removal method based on grid division. Background Art

[0002] With the advancement of technology, the demand for positioning and mapping is growing stronger, and increasingly accurate map construction and positioning have become the current trend. With the development of SLAM (Simultaneous Localization and Mapping) technology, SLAM has become one of the most fundamental technologies in autonomous driving. However, in today's real-world environments, the presence of dynamic point clouds, such as pedestrians, bicycles, and cars, can reduce the accuracy of map construction, which in turn affects positioning accuracy.

[0003] Currently, the main methods for filtering out dynamic point clouds include the following: 1. Based on semantic prior information: This method uses semantic priors to identify clusters of potentially dynamic point clouds and calculates the reprojection error of edge pixels within each cluster to construct an optimization function. However, this algorithm requires prior information and cannot effectively identify and remove dynamic point clouds that have never appeared before. Furthermore, it relies on external lighting conditions and cannot filter out dynamic point clouds in dark environments. 2. Based on point cloud clustering: This method uses a point cloud scene flow estimation algorithm to estimate motion point by point, combining point cloud clustering and principal component analysis to remove dynamic point clouds. The advantage of this method is that it is independent of the vision system and can operate in dark conditions. However, the algorithm is slow in scene flow inference, resulting in an overall algorithm execution time that cannot be guaranteed to be within 0.1 seconds, making it difficult to ensure real-time performance. Furthermore, it lacks a more flexible matching mechanism for bounding boxes of dynamic point clouds. 3. Based on ground detection: This method uses the Euclidean clustering method to detect dynamic point clouds in non-ground areas and remove dynamic point cloud data. However, it is easy to misjudge dynamic point clouds, resulting in the filtering of static point clouds, which affects the mapping accuracy.

[0004] In summary, under the conditions of no illumination and no prior information, it is difficult for existing methods to achieve high-precision and real-time dynamic point cloud data removal. Therefore, it is very necessary to propose a new dynamic point cloud removal method. Summary of the Invention

[0005] The purpose of the present invention is to solve the problem that existing methods are difficult to achieve high-precision and real-time dynamic point cloud data removal under conditions of no illumination and no prior information, and propose a dynamic point cloud removal method based on grid division.

[0006] The technical solution adopted by the present invention to solve the above technical problems is:

[0007] A dynamic point cloud removal method based on grid division, the method specifically comprises the following steps:

[0008] Step 1: Set a time threshold T and a space threshold S, and select a historical key frame from the key frame database based on the time threshold T and the space threshold S for the current key frame; use the current key frame and the selected historical key frames to form the current local map;

[0009] Step 2: According to the position of the lidar coordinate system relative to the world coordinate system in the current key frame, the current local map is converted to the lidar coordinate system;

[0010] Step 3: Construct a spatial cylindrical structure with the laser radar as the center of the cylinder, the height difference of the region of interest as the cylinder height, and the radius of the region of interest as the cylinder radius. The area contained in the constructed spatial cylindrical structure is the region of interest.

[0011] Step 4: radially and axially divide the point cloud data of the current key frame within the region of interest to obtain the point cloud of each grid of the current key frame; radially and axially divide the point cloud data of the local map within the region of interest to obtain the point cloud of each grid of the local map;

[0012] Step 5: Calculate the descriptors of each grid of the current key frame and the descriptors of each grid of the current local map respectively;

[0013] Step 6: Classify each grid of the current key frame according to the descriptor calculated in step 5 to determine the grid where the dynamic point cloud is located;

[0014] Step 7: Perform ground fitting on the grid where the dynamic point cloud is located and remove the dynamic point cloud data in the grid;

[0015] Step 8: Write the remaining point clouds in the grid of step 7, the point clouds in other grids, and the current keyframe point clouds outside the area of ​​interest into the keyframe database to construct a global map.

[0016] Furthermore, in step 1, a historical key frame is selected from the key frame database according to the time threshold T and the space threshold S for the current key frame; the specific process is as follows:

[0017] Select the historical key frames whose time distance to the current key frame is less than the time threshold T, and then construct a pose KD tree for each selected historical key frame. Then calculate the spatial distance between the current key frame and each historical key frame according to the constructed pose KD tree, and then select the historical key frames whose spatial distance to the current key frame is less than the spatial threshold S from the selected historical key frames.

[0018] Furthermore, the position and posture of the lidar coordinate system relative to the world coordinate system in the current key frame is obtained by a lidar inertial odometry system.

[0019] Furthermore, the current local map is converted into the LiDAR coordinate system; specifically:

[0020]

[0021] Among them, P World is the point cloud coordinate of the local map; is the position of the lidar coordinate system relative to the world coordinate system in the current key frame; the superscript -1 represents the inverse of the matrix; P LiDAR is the point cloud coordinate converted to the lidar coordinate system.

[0022] Furthermore, in step 4, the method of dividing the current key frame point cloud data and the local map point cloud data in the region of interest into blocks is the same; the specific dividing method is:

[0023] Step 4. Take the center of the upper surface of the space cylindrical structure as the center of the circle and make N r concentric circles, and set the radius of the largest circle to be the radius of the spatial cylindrical structure;

[0024] Divide the largest circle into N equal-sized circles. θ sectors, N θ The radius of the sector and N r The concentric circles all intersect, that is, for each circle, the area inside the circle is divided into N equal areas. θ a sector;

[0025] Then on the upper surface of the space cylindrical structure, the upper surface of the space cylindrical structure is divided into N r N θ regions, and the (i, j)th region obtained by division is represented as G′ (i,j) , i=1,2,…,N r , i is the number of concentric circles, j = 1, 2, ..., N θ , N θ The number of sectors each circle is divided into;

[0026] Step 42: For any area obtained by division, project the area onto the lower surface of the spatial cylindrical structure to obtain the projection area; then the portion between the area and the projection area is taken as the grid corresponding to the area, and the grid corresponding to the (i, j)th area is recorded as the (i, j)th grid G (i,j) ;

[0027] Similarly, each area obtained by division is processed separately to obtain the grid corresponding to each area.

[0028] Furthermore, the grid G (i,j) Point p in k ={x k ,y k ,z k}satisfy:

[0029]

[0030] Among them, P t For the grid G (i,j) The point cloud collection within r k In the polar coordinate system, p k The polar radius of the projection point in the xy plane, θ k =arctan(y k ,x k ),θ k In the polar coordinate system, p k Polar angle of the projection point in the xy plane; N r is the number of rings in the concentric region model; N θ is the number of sectors in the concentric region model; L max is the grid G (i,j) The maximum diameter of L min is the grid G (i,j) The minimum diameter, ΔL m is the grid G (i,j) The difference in polar diameter, ΔL m =L max -L min .

[0031] Furthermore, the number of sectors N θ The value of is 24, the number of concentric circles is N r The value of is 3.

[0032] Furthermore, the specific process of step five is as follows:

[0033]

[0034]

[0035] in, Represents the height difference of the point cloud in the (i, j)th grid under the current key frame, that is, the descriptor of the (i, j)th grid under the current key frame, Represents the height difference of the point cloud within the (i, j)th grid under the local map, that is, the descriptor of the (i, j)th grid under the local map, Represents the z-axis coordinate set of all point clouds in the (i, j)th grid under the current keyframe, Represents the z-axis coordinate set of all point clouds within the (i, j)th grid under the local map.

[0036] Furthermore, the specific process of step six is ​​as follows:

[0037] For the (i, j)th grid under the current keyframe:

[0038]

[0039] in, is the height difference of the point cloud in the (i, j)th grid under the current keyframe, is the height difference of the point cloud in the (i, j)th grid under the local map, S r is an intermediate variable;

[0040] According to S r Determine the grid where the dynamic point cloud is located. The determination method is: if the number of point clouds contained in the (i, j)th grid is greater than or equal to 10, and meets condition (1) or condition (2), then the (i, j)th grid is the grid where the dynamic point cloud is located;

[0041] Condition (1): 0<S r <0.2 and

[0042] Condition (2): S r > 0.2 and there are grids with dynamic point clouds around the (i, j)th grid.

[0043] Furthermore, the specific process of step seven is as follows:

[0044] Step 71: For any grid where the dynamic point cloud is located, select M point clouds with the smallest height from the grid as seed points to obtain a seed point set;

[0045] Step 72: Initialize the number of iterations l = 1;

[0046] Step 7.3: Calculate the average height H of all seed points in the seed point set mean , and then select the height lower than H mean +H ground The points are taken as candidate ground points, where H ground is the ground threshold height;

[0047] Perform ground fitting on all seed points and ground candidate points:

[0048] ax+by+cz+d=0

[0049] Among them, a, b, c, d are the coefficients of the plane model, and x, y, z are the three-dimensional coordinates of the points in the plane model; that is,

[0050]

[0051] in, The superscript T stands for transpose;

[0052] Step 74. Calculation Value:

[0053] Calculate the covariance matrix C of the seed point set:

[0054]

[0055] in, is the mean of the three-dimensional coordinates of all seed points in the seed point set, s1, s2, ..., s M are the three-dimensional coordinates of the first, second, ..., Mth seed points in the seed point set respectively;

[0056] After eigendecomposition of the covariance matrix C, Equal to the eigenvector corresponding to the minimum eigenvalue;

[0057] Step 75: After substituting into the fitting equation in step 7-3, we get the fitting plane π0;

[0058] Step 76: Traverse all points in the grid and calculate the distance from each point to the fitting plane π0:

[0059]

[0060] Where D is the distance from the point (x0, y0, z0) in the grid to the fitting plane π0;

[0061] If D is less than the threshold, the point (x0, y0, z0) is a ground point, and the point (x0, y0, z0) is added to the seed point set; otherwise, D is greater than or equal to the threshold, and the point (x0, y0, z0) is not a ground point;

[0062] Step 77: Determine whether the set maximum number of iterations has been reached;

[0063] If the maximum number of iterations is reached, all point clouds contained in the seed point set after the last iteration are used as ground points, that is, the dynamic point cloud data is removed;

[0064] If the maximum number of iterations is not reached, set l=l+1, use the updated seed point set in step 76, and return to step 72.

[0065] The beneficial effects of the present invention are:

[0066] This paper proposes a real-time dynamic point cloud filtering algorithm. By calculating and comparing the point cloud descriptors of the current keyframe and the corresponding grids in the local map in the area of ​​interest, it identifies grids containing dynamic point clouds, effectively removing dynamic point clouds from the current laser radar scan frame. This improves the accuracy of dynamic point cloud data removal and ultimately constructs a precise global point cloud map after filtering out dynamic point clouds. Furthermore, the implementation of the method is not limited by lighting or prior conditions and can be implemented in the absence of lighting and prior information, allowing it to operate around the clock. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 is a flow chart of the dynamic point cloud removal method based on grid division of the present invention;

[0068] Figure 2 It is a schematic diagram of ROI;

[0069] Figure 3 It is a concentric area model diagram;

[0070] Figure 4 is a schematic diagram of the descriptor;

[0071] Figure 5 It is a ground fitting schematic diagram;

[0072] In the figure, the triangle symbol represents the seed point set selected for the first time;

[0073] Figure 6a This is a comparison of the CAColiTower dataset before and after filtering;

[0074] Figure 6b This is a comparison chart of the CALombardStreet dataset before and after filtering;

[0075] Figure 6c This is a comparison chart of the KITTI07 dataset before and after filtering;

[0076] Figure 6d This is a comparison chart of the garden dataset before and after filtering. DETAILED DESCRIPTION

[0077] Specific implementation method 1: Combination Figure 1 The method for removing dynamic point clouds based on grid division described in this embodiment specifically includes the following steps:

[0078] Step 1: Set a time threshold T and a space threshold S (in the present invention, the time threshold is 0.3s and the space threshold is 0.8m). Select a historical key frame from the key frame database based on the time threshold T and the space threshold S for the current key frame; use the current key frame and the selected historical key frames to form the current local map;

[0079] The initial frame can be used as the first key frame, and then a frame is selected as a key frame at every fixed time step. In the present invention, only the key frames are processed;

[0080] Step 2: According to the position of the lidar coordinate system relative to the world coordinate system in the current key frame, the current local map is converted to the lidar coordinate system;

[0081] Step 3: Figure 2 As shown in the figure, a spatial cylindrical structure is constructed with the lidar as the center of the cylinder, the height difference of the region of interest (ROI) as the cylinder height, and the radius of the region of interest as the cylinder radius. The area contained in the constructed spatial cylindrical structure is the region of interest.

[0082] Height difference setting rule: Set the height difference based on the height difference between the lidar and the ground and the highest distance of the dynamic object to the lidar. For example, if the lidar is 1 meter above the ground, the height difference between the highest height of all dynamic objects and the lidar should not exceed 3 meters, and the ROI height range should be set to -1m to 3m.

[0083] Radius setting rules: Set according to different environments. For example, on open roads, 30-50m is generally used, which has a better effect.

[0084] Step 4: radially and axially divide the point cloud data of the current key frame within the region of interest to obtain the point cloud of each grid of the current key frame; radially and axially divide the point cloud data of the local map within the region of interest to obtain the point cloud of each grid of the local map;

[0085] Step 5: Calculate the descriptors of each grid of the current key frame and the descriptors of each grid of the current local map respectively;

[0086] Step 6: Classify each grid of the current key frame according to the descriptor calculated in step 5 to determine the grid where the dynamic point cloud is located;

[0087] Step 7: Perform ground fitting on the grid where the dynamic point cloud is located and remove the dynamic point cloud data in the grid;

[0088] Step 8: Write the remaining point clouds in the grid of step 7, the point clouds in other grids (that is, grids other than the grid where the dynamic point cloud is located), and the current key frame point cloud outside the area of ​​interest into the key frame database to construct a global map.

[0089] Step 8 is as follows:

[0090] The LiDAR odometry can be used to obtain the LiDAR's pose relationship relative to the world coordinate system. The point cloud of each key frame is converted to the world coordinate system using this pose relationship, and the accumulation can be used to obtain the final global map after filtering out the dynamic point cloud.

[0091] The specific calculation formula is as follows:

[0092]

[0093] Among them, P is the point set of the global map, P LiDAR is the coordinate of a point in the laser radar coordinate system under a key frame, is the position of the lidar relative to the world coordinate system at a certain key frame, P World The coordinates of the point converted to the world coordinate system.

[0094] The present invention proposes a real-time filtering method for dynamic point clouds based on grid occupancy differences, which can be widely used in various current environments and has good robustness.

[0095] Specific embodiment 2: This embodiment differs from specific embodiment 1 in that: in step 1, a historical key frame is selected from the key frame database based on the time threshold T and the space threshold S for the current key frame; the specific process is as follows:

[0096] Select the historical key frames whose time distance to the current key frame is less than the time threshold T, and then construct a pose KD tree (K-Dimensional tree) for each selected historical key frame. Then, calculate the spatial distance between the current key frame and each historical key frame based on the constructed pose KD tree, and then select the historical key frames whose spatial distance to the current key frame is less than the spatial threshold S from the selected historical key frames.

[0097] Other steps and parameters are the same as those in the first embodiment.

[0098] Specific implementation method three: This implementation method differs from specific implementation methods one or two in that the position and posture of the laser radar coordinate system relative to the world coordinate system in the current key frame is obtained through the laser radar inertial odometer system.

[0099] Other steps and parameters are the same as those in the first or second embodiment.

[0100] Specific embodiment 4: This embodiment differs from any one of specific embodiments 1 to 3 in that: the current local map is converted into the laser radar coordinate system; specifically:

[0101]

[0102] Among them, P World is the point cloud coordinate of the local map; is the position of the lidar coordinate system relative to the world coordinate system in the current key frame; the superscript -1 represents the inverse of the matrix; P LiDAR is the point cloud coordinate converted to the lidar coordinate system.

[0103] The other steps and parameters are the same as those in the first to third embodiments.

[0104] Specific implementation method five: Combination Figure 3 The difference between this embodiment and any one of the specific embodiments 1 to 4 is that in step 4, the method of dividing the current key frame point cloud data and the local map point cloud data in the area of ​​interest into blocks is the same; the specific dividing method is:

[0105] Step 4. Take the center of the upper surface of the space cylindrical structure as the center of the circle and make N r concentric circles, and set the radius of the largest circle to be the radius of the spatial cylindrical structure;

[0106] Divide the largest circle into N equal-sized circles. θ sectors, N θ The radius of the sector and N r The concentric circles all intersect, that is, for each circle, the area inside the circle is divided into N equal areas. θ a sector;

[0107] Then on the upper surface of the space cylindrical structure, the upper surface of the space cylindrical structure is divided into N r N θ regions, and the (i, j)th region obtained by division is represented as G′ (i,j) , i=1,2,…,N r , i is the number of concentric circles, j = 1, 2, ..., N θ , N θ The number of sectors each circle is divided into;

[0108] Step 42: For any area obtained by division, project the area onto the lower surface of the spatial cylindrical structure to obtain the projection area; then the portion between the area and the projection area is taken as the grid corresponding to the area, and the grid corresponding to the (i, j)th area is recorded as the (i, j)th grid G (i,j);

[0109] Similarly, each area obtained by division is processed separately to obtain the grid corresponding to each area.

[0110] The other steps and parameters are the same as those in the first to fourth embodiments.

[0111] In this embodiment, the upper and lower surfaces of the cylinder are parallel to the ground. For any region after division, the grid corresponding to that region is segmented using a plane parallel to the ground and at any height. The resulting cross-sections all have the same shape as the region. That is, each grid is a cylinder that extends vertically through the entire spatial cylinder, and any horizontal cross-section of the cylinder has the same shape. After dividing the grid using the method of this embodiment, the point cloud for the current keyframe within each grid and the point cloud for the local map within each grid are obtained, completing the point cloud segmentation operation.

[0112] Specific embodiment 6: This embodiment differs from any one of the specific embodiments 1 to 5 in that: the grid G (i,j) Point p in k ={x k ,y k ,z k}satisfy:

[0113]

[0114] Among them, P t For the grid G (i,j) The point cloud collection within r k In the polar coordinate system, p k The polar radius of the projection point in the xy plane, θ k =arctan(y k ,x k ),θ k In the polar coordinate system, p k Polar angle of the projection point in the xy plane; N r is the number of rings in the concentric region model; N θ is the number of sectors in the concentric region model; L max is the grid G (i,j) The maximum diameter of L min is the grid G (i,j) The minimum diameter, ΔL m is the grid G (i,j) The difference in polar diameter, ΔL m =L max -L min .

[0115] The other steps and parameters are the same as those in the first to fifth embodiments.

[0116] Specific embodiment seven: This embodiment differs from any one of the specific embodiments one to six in that: the number of sectors N θ The value of is 24, the number of concentric circles is N r The value of is 3.

[0117] The other steps and parameters are the same as those in the first to sixth embodiments.

[0118] Specific implementation method eight: combination Figure 4 This embodiment is different from the first to seventh embodiments in that the specific process of step 5 is as follows:

[0119]

[0120]

[0121] in, Represents the height difference of the point cloud in the (i, j)th grid under the current key frame, that is, the descriptor of the (i, j)th grid under the current key frame, Represents the height difference of the point cloud within the (i, j)th grid under the local map, that is, the descriptor of the (i, j)th grid under the local map, Represents the z-axis coordinate set of all point clouds in the (i, j)th grid under the current keyframe, Represents the z-axis coordinate set of all point clouds within the (i, j)th grid under the local map.

[0122] The other steps and parameters are the same as those in the first to seventh embodiments.

[0123] Specific embodiment 9: This embodiment differs from any one of specific embodiments 1 to 8 in that the specific process of step 6 is as follows:

[0124] For the (i, j)th grid under the current keyframe:

[0125]

[0126] in, is the height difference of the point cloud in the (i, j)th grid under the current keyframe, is the height difference of the point cloud in the (i, j)th grid under the local map, S r is an intermediate variable;

[0127] According to S rDetermine the grid where the dynamic point cloud is located. The determination method is: if the number of point clouds contained in the (i, j)th grid is greater than or equal to 10, and meets condition (1) or condition (2), then the (i, j)th grid is the grid where the dynamic point cloud is located;

[0128] Condition (1): 0<S r <0.2 and

[0129] Condition (2): S r > 0.2 and there are grids with dynamic point clouds around the (i, j)th grid.

[0130] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0131] From the previous step 5, we can get the descriptors corresponding to the grids in the current key frame and the grids in the local map. We use the following rules to classify the grids in the current key frame. Finally, we divide the grids in the current key frame into four categories: sparse point cloud grid G little , the current keyframe dynamic point cloud grid G dynamic , is not a dynamic point in the current keyframe, but is a dynamic point cloud grid G ​​in the local map map and the uncertainty grid G uncertainty The pseudo code of the classification process is shown in Table 1:

[0132] Table 1

[0133]

[0134] Specific implementation method ten: Combination Figure 5 This embodiment is different from the first to ninth embodiments in that the specific process of step 7 is as follows:

[0135] Step 71: For any grid where the dynamic point cloud is located, select the M point clouds with the smallest heights from the grid as seed points to obtain a seed point set (i.e., sort all the point clouds in the grid in ascending order of height, and use the top M point clouds as seed points);

[0136] Step 72: Initialize the number of iterations l = 1;

[0137] Step 7.3: Calculate the average height H of all seed points in the seed point set mean , and then select the height lower than H mean +H ground The points are taken as candidate ground points, where H ground is the ground threshold height;

[0138] Perform ground fitting on all seed points and ground candidate points:

[0139] ax+by+cz+d=0

[0140] Among them, a, b, c, d are the coefficients of the plane model, and x, y, z are the three-dimensional coordinates of the points in the plane model; that is,

[0141]

[0142] in, The superscript T stands for transpose;

[0143] Step 74. Calculation Value:

[0144] Calculate the covariance matrix C of the seed point set:

[0145]

[0146] in, The mean of the three-dimensional coordinates of all seed points in the seed point set (that is, the mean of the x-axis coordinates of all seed points, the mean of the y-axis coordinates of all seed points, and the mean of the z-axis coordinates of all seed points, the three means are composed of ), s1,s2,…,s M are the three-dimensional coordinates of the first, second, ..., Mth seed points in the seed point set respectively;

[0147] The scatter of the seed point set can be obtained through the covariance matrix C. By performing eigenvalue decomposition on the covariance matrix C, three eigenvectors can be obtained. Since the ground is fitted with a plane model, the normal vector perpendicular to the plane is It should correspond to the eigenvector with the smallest eigenvalue. Finally, the initial plane π0 is obtained.

[0148] After eigendecomposition of the covariance matrix C, Equal to the eigenvector corresponding to the minimum eigenvalue;

[0149] Step 75: After substituting into the fitting equation in step 7-3, we get the fitting plane π0;

[0150] Step 76: Traverse all points in the grid and calculate the distance from each point to the fitting plane π0:

[0151]

[0152] Where D is the distance from the point (x0, y0, z0) in the grid to the fitting plane π0;

[0153] If D is less than the threshold, the point (x0, y0, z0) is a ground point, and the point (x0, y0, z0) is added to the seed point set; otherwise, D is greater than or equal to the threshold, and the point (x0, y0, z0) is not a ground point;

[0154] Step 77: Determine whether the set maximum number of iterations (set to 5 times in the present invention) has been reached;

[0155] If the maximum number of iterations is reached, all point clouds contained in the seed point set after the last iteration are used as ground points, that is, the dynamic point cloud data is removed;

[0156] If the maximum number of iterations is not reached, set l=l+1, use the updated seed point set in step 76, and return to step 72.

[0157] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0158] Experimental part

[0159] The method of this invention can be used to filter out dynamic point clouds from LiDAR in real time. The effectiveness of the method will be demonstrated below using both public and private datasets. The dynamic point cloud filtering rate and the trajectory error of a mobile robot will be calculated and compared.

[0160] First, we used the UrbanLoco unmanned driving dataset for experiments, including the CAColiTower dataset (high dynamic level) and the CALombardStreet dataset (medium dynamic level); secondly, we used the KITTI dataset (medium dynamic level); finally, we conducted experiments on the garden dataset (low dynamic level) we collected ourselves.

[0161] The specific information of the dataset is shown in Table 2.

[0162] Table 2 Dataset data table

[0163]

[0164] In terms of trajectory accuracy, the trajectory accuracy is compared with the current mainstream algorithms LeGO-LOAM and LIO_SAM. APE (absolute pose error) is used to calculate its RMSE (m). The experimental results are shown in Table 3.

[0165] Table 3. Comparison of absolute pose error RMSE (m)

[0166]

[0167]

[0168] Compared to LIO-SAM, the proposed method improves trajectory accuracy by approximately 20.96% on the CAColiTower dataset, by approximately 20.04% on the CALombardStreet dataset, and by 28.24% on the KITTI07 dataset. For the Garden dataset, ODF-LINS (the proposed method) improves trajectory accuracy by approximately 5.3%. These results indicate that ODF-LINS has a greater impact on improving trajectory accuracy in high- and medium-dynamic scenes, while having a smaller impact in low-dynamic scenes, which is consistent with our expectations.

[0169] For the filtering of dynamic point clouds, the final mapping effect is as follows Figure 6a 、 Figure 6b 、 Figure 6c as well as Figure 6d In order to evaluate the filtration effect of the method of the present invention, the following indicators are defined:

[0170]

[0171]

[0172] The indicator data are shown in Table 4:

[0173] Table 4 Dynamic point cloud removal test results

[0174]

[0175] In order to illustrate the real-time performance of the algorithm, the algorithm time consumption is calculated through the program. The experimental results are shown in Table 5.

[0176] Table 5 Average time consumption of different data sets

[0177]

[0178] As shown in Table 5, the time consumption of the method of the present invention for all data sets can be guaranteed to be within 0.1s, meeting the real-time requirement.

[0179] The above examples are merely illustrative of the calculation model and process of the present invention and are not intended to limit the embodiments of the present invention. Persons skilled in the art will readily appreciate that other variations or modifications based on the above description are possible. This list of embodiments is not exhaustive; however, any obvious variations or modifications derived from the technical solution of the present invention remain within the scope of protection of the present invention.

Claims

1. A dynamic point cloud removal method based on grid division, characterized in that: The method specifically comprises the following steps: Step 1: Set a time threshold T and a space threshold S, and select a historical key frame from the key frame database based on the time threshold T and the space threshold S for the current key frame; use the current key frame and the selected historical key frames to form the current local map; Step 2: According to the position of the lidar coordinate system relative to the world coordinate system in the current key frame, the current local map is converted to the lidar coordinate system; Step 3: Construct a spatial cylindrical structure with the laser radar as the center of the cylinder, the height difference of the region of interest as the cylinder height, and the radius of the region of interest as the cylinder radius. The area contained in the constructed spatial cylindrical structure is the region of interest. Step 4: radially and axially divide the point cloud data of the current key frame within the region of interest to obtain the point cloud of each grid of the current key frame; radially and axially divide the point cloud data of the local map within the region of interest to obtain the point cloud of each grid of the local map; Step 5: Calculate the descriptors of each grid of the current key frame and the descriptors of each grid of the current local map respectively; Step 6: Classify each grid of the current key frame according to the descriptor calculated in step 5 to determine the grid where the dynamic point cloud is located; Step 7: Perform ground fitting on the grid where the dynamic point cloud is located and remove the dynamic point cloud data in the grid; Step 8: Write the remaining point clouds in the grid of step 7, the point clouds in other grids, and the current keyframe point clouds outside the area of ​​interest into the keyframe database to construct a global map.

2. The dynamic point cloud removal method based on grid division according to claim 1, characterized in that: In the step 1, a historical key frame is selected from the key frame database according to the time threshold T and the space threshold S for the current key frame; the specific process is as follows: Select the historical key frames whose time distance to the current key frame is less than the time threshold T, and then construct a pose KD tree for each selected historical key frame. Then calculate the spatial distance between the current key frame and each historical key frame according to the constructed pose KD tree, and then select the historical key frames whose spatial distance to the current key frame is less than the spatial threshold S from the selected historical key frames.

3. The dynamic point cloud removal method based on grid division according to claim 2 is characterized in that: The position of the lidar coordinate system relative to the world coordinate system in the current key frame is obtained by the lidar inertial odometry system.

4. The dynamic point cloud removal method based on grid division according to claim 3 is characterized in that: The current local map is converted to the LiDAR coordinate system; specifically: Among them, P World is the point cloud coordinate of the local map; is the position of the lidar coordinate system relative to the world coordinate system in the current key frame; the superscript -1 represents the inverse of the matrix; P LiDAR is the point cloud coordinate converted to the lidar coordinate system.

5. The dynamic point cloud removal method based on grid division according to claim 4 is characterized in that: In step 4, the method of dividing the current key frame point cloud data and the local map point cloud data in the area of ​​interest into blocks is the same; the specific dividing method is: Step 4. Take the center of the upper surface of the space cylindrical structure as the center of the circle and make N r concentric circles, and set the radius of the largest circle to the radius of the spatial cylindrical structure; Divide the largest circle into N sectors with equal areas. θ sectors, N θ The radius of the sector and N r The concentric circles all intersect, that is, for each circle, the area inside the circle is divided into N equal areas. θ a sector; Then on the upper surface of the space cylindrical structure, the upper surface of the space cylindrical structure is divided into N r N θ regions, and the (i, j)th region obtained by division is represented as G′ (i,j) , i=1,2,…,N r , i is the number of concentric circles, j = 1, 2, ..., N θ , N θ The number of sectors each circle is divided into; Step 42: For any area obtained by division, project the area onto the lower surface of the spatial cylindrical structure to obtain the projection area; then the portion between the area and the projection area is taken as the grid corresponding to the area, and the grid corresponding to the (i, j)th area is recorded as the (i, j)th grid G (i,j) ; Similarly, each area obtained by division is processed separately to obtain the grid corresponding to each area.

6. The method for removing dynamic point clouds based on grid division according to claim 5, characterized in that: The grid G (i,j) Point p in k ={x k ,y k ,z k }satisfy: Among them, P t For the grid G (i,j) The point cloud collection within r k In the polar coordinate system, p k The polar radius of the projection point in the xy plane, θ k =arctan(y k ,x k ),θ k In the polar coordinate system, p k Polar angle of the projection point on the xy plane; N r is the number of rings in the concentric region model; N θ is the number of sectors in the concentric region model; L max is the grid G (i,j) The maximum diameter of L min is the grid G (i,j) The minimum diameter, ΔL m is the grid G (i,j) The difference in polar diameter, ΔL m =L max -L min .

7. The dynamic point cloud removal method based on grid division according to claim 6, characterized in that: The number of sectors N θ The value of is 24, the number of concentric circles is N r The value of is 3.

8. The dynamic point cloud removal method based on grid division according to claim 7, characterized in that: The specific process of step five is: in, Represents the height difference of the point cloud in the (i, j)th grid under the current key frame, that is, the descriptor of the (i, j)th grid under the current key frame, Represents the height difference of the point cloud within the (i, j)th grid under the local map, that is, the descriptor of the (i, j)th grid under the local map, Represents the z-axis coordinate set of all point clouds in the (i, j)th grid under the current keyframe, Represents the z-axis coordinate set of all point clouds within the (i, j)th grid under the local map.

9. The method for removing dynamic point clouds based on grid division according to claim 8, characterized in that: The specific process of step six is ​​as follows: For the (i, j)th grid under the current keyframe: in, is the height difference of the point cloud in the (i, j)th grid under the current keyframe, is the height difference of the point cloud in the (i, j)th grid under the local map, S r is an intermediate variable; According to S r Determine the grid where the dynamic point cloud is located. The determination method is: if the number of point clouds contained in the (i, j)th grid is greater than or equal to 10, and meets condition (1) or condition (2), then the (i, j)th grid is the grid where the dynamic point cloud is located; Condition (1): 0<S r <0.2 and Condition (2): S r > 0.2 and there are grids with dynamic point clouds around the (i, j)th grid.

10. The method for removing dynamic point clouds based on grid division according to claim 9, characterized in that: The specific process of step seven is as follows: Step 71: For any grid where the dynamic point cloud is located, select M point clouds with the smallest height from the grid as seed points to obtain a seed point set; Step 72: Initialize the number of iterations l = 1; Step 7.3: Calculate the average height H of all seed points in the seed point set mean , and then select the height lower than H mean +H ground The points are taken as candidate ground points, where H ground is the ground threshold height; Perform ground fitting on all seed points and ground candidate points: ax+by+cz+d=0 Among them, a, b, c, d are the coefficients of the plane model, and x, y, z are the three-dimensional coordinates of the points in the plane model; that is, in, The superscript T stands for transpose; Step 74. Calculation Value: Calculate the covariance matrix C of the seed point set: in, is the mean of the three-dimensional coordinates of all seed points in the seed point set, s1, s2, ..., s M are the three-dimensional coordinates of the first, second, ..., Mth seed points in the seed point set respectively; After eigendecomposition of the covariance matrix C, Equal to the eigenvector corresponding to the minimum eigenvalue; Step 75: After substituting into the fitting equation in step 7-3, we get the fitting plane π0; Step 76: Traverse all points in the grid and calculate the distance from each point to the fitting plane π0: Where D is the distance from the point (x0, y0, z0) in the grid to the fitting plane π0; If D is less than the threshold, the point (x0, y0, z0) is a ground point, and the point (x0, y0, z0) is added to the seed point set; otherwise, D is greater than or equal to the threshold, and the point (x0, y0, z0) is not a ground point; Step 77: Determine whether the set maximum number of iterations has been reached; If the maximum number of iterations is reached, all point clouds contained in the seed point set after the last iteration are used as ground points, that is, the dynamic point cloud data is removed; If the maximum number of iterations is not reached, set l=l+1, use the updated seed point set in step 76, and return to step 72.

Images