Method for constructing a point cloud map without residual shadows based on point cloud clustering

Abstract

The application discloses a method for constructing a residual shadow-free point cloud map based on a point cloud clustering mode, which comprises the following steps: step one, clustering processing is performed on an input single-frame point cloud map; step two, category judgment is performed based on the geometric features of the clustered point cloud to determine the suspected categories of dynamic targets in the point cloud map, and the suspected categories are projected onto a constructed grid map; step three, pose estimation and registration are performed on the point cloud in the point cloud map except the suspected categories and a previous frame point cloud map; step four, the registered pose is compared with the transfer position of the object in the grid map, if the transfer position changes by more than a predetermined value, the category is determined as a dynamic target and removed to obtain a point cloud processing map; and step five, the point cloud processing map is spliced with a key frame point cloud to obtain a residual shadow-free point cloud map. The method provided by the application can maintain the real-time performance of a slam algorithm and achieve a satisfactory frame map matching positioning effect.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of unmanned positioning and mapping, and particularly relates to a method for constructing a residual-image-free point cloud map based on point cloud clustering, which is proposed to eliminate the map residual image problem caused by dynamic targets in a point cloud map. BACKGROUND

[0002] As an important type of intelligent robots, unmanned vehicles have become a strategic goal of high-tech research in various countries around the world, and have attracted extensive research in both civilian and military fields. A key technology supporting the safe driving of autonomous vehicles is positioning. Currently, GPS is used as a basic tool for vehicle positioning, but it is difficult to meet the needs of autonomous driving technology. GPS is susceptible to atmospheric environment and has low indoor positioning accuracy. Relying solely on GPS for vehicle positioning and navigation technology is prone to errors. In some areas with poor signal, we need to use high-precision maps or other sensors to assist in obtaining more accurate pose estimation.

[0003] Currently, the main way to obtain a high-precision map is to use a slam (simultaneous localization and mapping) scheme. However, the actual world is a dynamic environment, especially in a campus environment, where there are many dynamic objects. Such dynamic objects not only affect the estimation of the odometry when constructing a high-precision point cloud map, but also leave "ghost images" that affect the local geometric features of the high-precision point cloud map constructed in the end, affecting the subsequent frame map matching positioning process. SUMMARY

[0004] An object of the present application is to solve at least the above problems and / or deficiencies, and to provide at least the advantages described later.

[0005] To achieve these objects and other advantages and in accordance with the purpose of the application, as embodied and broadly described herein, there is provided a method for constructing a residual-image-free point cloud map based on point cloud clustering, comprising:

[0006] Step one, clustering processing is performed on the input single-frame point cloud map;

[0007] Step two, based on the geometric features of the clustered point cloud, a category judgment is made to determine the suspected categories of dynamic targets in the point cloud map, and the suspected categories are projected onto the constructed grid map;

[0008] Step three, pose estimation and registration are performed on the point cloud in the point cloud map except the suspected categories and the previous frame point cloud map;

[0009] Step four, the registered pose is compared with the transfer position of the object in the grid map, and if the change in the transfer position is greater than a predetermined value, the object is determined to be a dynamic target and is removed to obtain a corresponding point cloud processing map;

[0010] Step five, the point cloud processing graph is spliced with the key frame point cloud to obtain a point cloud map without residual shadows.

[0011] Preferably, in step one, the clustering processing includes:

[0012] S10, ground points in a single frame point cloud map are roughly removed;

[0013] S11, in the point cloud map after removing the ground points, a geometric relationship with surrounding points is calculated starting from any point to complete the clustering processing;

[0014] In S10, the ground point removal method includes:

[0015] S101, for infinite points in the single frame point cloud map, whether the three-axis direction points are Nan is judged to determine whether the infinite points need to be removed;

[0016] S102, the point cloud points in the single frame point cloud map after removing the infinite points are projected into a depth map;

[0017] S103, in the depth map after S102 processing, an angle is calculated based on two point cloud points in the front and back lines, and whether the calculated angle value is greater than a predetermined threshold value is judged to determine whether it is a ground point.

[0018] Preferably, in step two, when the suspected classes are projected onto the grid map, only the cluster center points of the suspected classes are projected.

[0019] Preferably, in step three, the pose estimation and registration process is configured to include:

[0020] S30, the point cloud in the point cloud map except the suspected classes is registered with the previous frame point cloud map to obtain a rough pose estimation;

[0021] S31, the pose estimation obtained in S30 is registered with the spliced local map through the point cloud to eliminate cumulative errors to obtain an accurate pose;

[0022] S32, the displacement amount of the accurate pose obtained in S31 relative to the previous frame key frame is used to determine whether it can be selected as the next frame key frame. If the determination result is a key frame, the accurate pose is added to the factor graph, and loop detection is performed, and the loop detection result is added to the factor graph for optimization.

[0023] Preferably, in S30, the pose estimation method is configured to include:

[0024] S301, for the point cloud in the point cloud map except the suspected classes, curvature feature points are extracted, and based on the judgment of the curvature and a predetermined threshold value, the point cloud points are divided into line points and surface points;

[0025] S302, for line points, by constructing a residual function, selecting a line and in the previous frame of point cloud map, selecting the nearest point group consisting of this line point and the adjacent points on the line and the adjacent line, calculating their distance, which is the line residual of the line point;

[0026] For face points, select the nearest two points of the line and the nearest point of the adjacent line to form a plane with the face point, and calculate the distance between the plane and the face point. Add all the line residuals and the line residuals to obtain the total residual. The total residual is optimized using LM to obtain the rough pose estimation with the minimum residual.

[0027] Preferably, in S31, the method for obtaining the accurate pose is configured to include:

[0028] S310, based on the pose estimation obtained in S30, moving the point cloud map of the current frame to the map, and finding the five nearest points to the line points in S301 through kdtree;

[0029] S311, judging whether the five points in S310 meet the line feature based on the covariance matrix, and calculating the line residual for the five points meeting the line feature;

[0030] S312, adding all the residuals to optimize to eliminate the cumulative error of the odometry.

[0031] Preferably, in S32, the loop detection process is configured to include:

[0032] The accurate pose is added to the kdtree of the pose point, and the nearest pose in the kdtree is searched. If the distance between the accurate pose and the nearest pose is only 0.3m, the icp algorithm is used for loop detection;

[0033] In the loop detection process, if the matching score of the icp algorithm is less than 0.1, it is considered that the loop is successful, the pose information of the loop is added to the factor graph, and the factor graph optimization is performed at the same time when the loop detection occurs.

[0034] The present application at least includes the following beneficial effects: the present application proposes a way of recognizing dynamic targets in a campus scene based on point cloud clustering, and determines whether it is a dynamic target by the change of the grid in the occupancy grid map to establish a point cloud map without dynamic ghosting, which can effectively eliminate the odometry registration error, and has better map construction accuracy.

[0035] Other advantages, objects and features of the present application will be embodied in part by the following description, and will be understood by those skilled in the art through research and practice of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 The algorithm flowchart of the present application;

[0037] Figure 2 The schematic diagram of the ground segmentation and clustering of the point cloud map of the present application;

[0038] Figure 3 The schematic diagram of the preliminary judgment of the dynamic target in the clustered point cloud map of the present application;

[0039] Figure 4 The comparative schematic diagram of the point cloud map constructed by the method of the present application and the point cloud map constructed by the prior art;

[0040] Figure 5 The schematic diagram of the comparative effect diagram of the point cloud map constructed by the method of the present application and the point cloud map constructed by the prior art;

[0041] Figure 6 The effect diagram of the point cloud map constructed by the method of the present application without residual image;

[0042] Figure 7 The position schematic diagram of the category angle generated by the present application. DETAILED DESCRIPTION

[0043] The present application will be further described in detail below with reference to the accompanying drawings, so that those skilled in the art can implement the present application according to the description.

[0044] The purpose of the present application is to solve the problem that in the process of simultaneous localization and mapping in a campus environment, the dynamic environment causes point cloud registration error, resulting in inaccurate front-end odometer estimation, which leads to insufficient map accuracy, and at the same time, residual images are left in the map, which will affect the back-end loop detection and the final use of high-precision map for point cloud registration and positioning. In order to solve the above problems, a method for removing map residual images based on clustering is proposed, which can maintain the real-time performance of the slam algorithm and achieve satisfactory frame map matching positioning effect. Specifically, the method of the present application is to perform rough ground point rejection according to the geometric features of the input point cloud map, then perform classification processing through the geometric structure relationship between points, then perform category judgment according to the length-width-height ratio and length-width-height size of the clustered points, perform dynamic target rejection, then perform point cloud registration on the processed point cloud to calculate the front-end odometer pose, then perform a fine registration between the point cloud map and the map to obtain a more accurate pose estimation. Finally, the point cloud map is spliced to obtain the final map according to the estimated pose, and the pose is added to the factor graph, and when loop detection occurs, the map accuracy is optimized and corrected.

[0045] Example 1:

[0046] As Figure 1 shown, the technical solution of the application is a scheme for constructing a residual point cloud map based on point cloud clustering, comprising the following steps:

[0047] S1, input a single frame of point cloud, and remove infinite points.

[0048] S2, project the point cloud points into a depth map.

[0049] S3, calculate the angle of the two point cloud points according to the front and back lines, and judge whether it is a ground point according to whether the angle value is greater than a threshold.

[0050] S4, after removing all ground points, start calculating the geometric relationship with the surrounding points for clustering with any point.

[0051] S5, judge whether it is a dynamic target according to the geometric characteristics of clustering.

[0052] S6, register the processed point cloud with the previous frame of point cloud map to obtain a rough pose estimation.

[0053] S7, according to the pose estimation calculated in the foregoing, register the point cloud with the spliced local map to eliminate cumulative error.

[0054] S8, add the pose calculated in the foregoing to the factor graph, simultaneously perform loop closure detection, and add the loop closure detection result to the factor graph for optimization.

[0055] S9, splice the point cloud map to obtain the final result.

[0056] Further, the point cloud map in S1 is input, because some point cloud beams may not hit obstacles within the range of radar speed. Infinite points will be returned. At this time, whether it needs to be removed is judged according to whether the three-axis direction points are Nan.

[0057] Further, in S2, the laser point cloud points are divided into 32 vertical lines and 1800 horizontal points for sorting. The vertical line number is mainly judged according to the following formula

[0058]

[0059] raw is the line number of the laser radar, and x, y and z are respectively the coordinates of the point cloud points in the laser radar coordinate system. At the same time, the column line number of the point cloud points projected into the depth map is judged according to the following formula:

[0060] col=arctan(y / x)

[0061] By using this formula to get the line number, we can get the coordinates of the point cloud point projected onto the image. The point cloud 3D point can be projected onto a range image, and the pixel value of the image is the distance between the point cloud points:

[0062]

[0063] In S3, we determine the number of downward scanning lines of the laser radar to obtain the point cloud points. Generally, a 32-line laser radar has 11 downward beams. Then, we calculate the angle of the points with the same col but a difference of one in the raw image projected in S2. If the angle is less than 10 degrees, we consider it to be a ground point. This can be expressed as follows:

[0064]

[0065] θ<10°.

[0066] In step S4, starting from the first point of the first line, it is first determined whether it has been marked. If it has not been marked, the category is determined. The four points in front, behind, left and right of the point are determined according to the BFS four-neighborhood algorithm, and a category angle is generated according to the following formula:

[0067]

[0068] Where beta is the category angle, d1 is the point with the largest distance between the point and its neighbors, d2 is the point with the smallest distance between the point and its neighbors, and α is the resolution, where the horizontal resolution is 0.2 and the vertical resolution is 0.33°. Its position is roughly as follows Figure 7 As shown:

[0069] Based on the generated beta angle and the category threshold, we determine whether they belong to the same category. If they belong to the same category, we continue to perform BFS until all four directions do not belong to the same category. Each row and column (row first, then column) is judged and processed until all points are traversed. For each category, if the number of points in the category is greater than 30 or the number of points in the category is greater than 10 and the number of occupied lines is greater than 5, it is considered a valid category. Otherwise, it is an invalid category. Each category is wrapped with a box. If the length and width of the final box are 0.2-0.4m and the height is 1.6 to 1.8 meters, we consider it to be a person. Similarly, bicycles are judged in the same way, and these point cloud point categories are marked as rough dynamic targets.

[0070] In step S5, S4 has already roughly parsed the scene's semantics. Clustering information for various objects is obtained, and some objects are roughly identified as dynamic targets. In this step, a grid map is constructed and the dynamic targets are projected onto it. To simplify the process, we only project the cluster centers. The pose calculation is then performed on all points other than those initially identified as dynamic targets.

[0071] Further S6, we will handle the point cloud, with the curvature of the feature points, namely:

[0072]

[0073] Where s is the curvature of the point cloud, when the curvature is greater than a certain threshold t2, we think it is a line point, when the curvature is less than a certain threshold t1, we think it is a face point, we can put them into two sets, for the line point, we can build a residual function, select the line point and its previous frame of point cloud, select the nearest point on the line and the nearest point on the adjacent line to form a line segment, calculate their distance, we think it is the residual of the point. For the face point, we select the nearest point on the line and the nearest point on the adjacent line and our face point to calculate the distance, and the total residual is the sum of the line residual and the face residual. The last total residual is optimized by LM to get the optimal pose with the minimum residual. That is the rough pose we need. The process of LM optimization is as follows:

[0074] The point cloud we get is mainly the point in the local radar coordinate system, the conversion relationship is as follows:

[0075]

[0076] Where, is a 4*4 rotation and translation matrix, that is, the transformation matrix from the last frame of radar coordinate system to this frame of radar coordinate system. According to the previous derivation:

[0077]

[0078] Where, loss is the residual constructed, X n is the point cloud point of the current frame, is the coordinate transformation matrix of the previous and next frames. We need to find the appropriate to make the loss minimum, the variables to be optimized are mainly r x , r y , r z , t x , t y , t z . That is, three degrees of freedom of pitch angle, three degrees of freedom of translation. That is, the Jacobian of loss to :

[0079]

[0080] Where R, t is the rotation and translation transformation matrix from the last frame to the next frame, since T is a six degree of freedom variable, we can further simplify the above formula:

[0081] For rotation variables:

[0082]

[0083] For ryand rz, the same as rx.

[0084] For translation variables:

[0085]

[0086] J n*6 For t y , t z The same as the above equation. We can get the Jacobian matrix for a feature point, and the Jacobian feature matrix of all other feature points is calculated in the same way to combine the total Jacobian matrix n is the number of feature points.

[0087] According to the LM algorithm:

[0088]

[0089] Update according to the above equation until the pose vector length changes less than 0.05, and the convergence ends, then the pose transformation matrix from the last frame to this frame can be obtained.

[0090] Further S7, according to the rough pose calculated in the previous S6 step, move the point cloud of the frame to the map, find the line point found in S6 step through kdtree to find the nearest five points, calculate the covariance matrix through the five points and the points of this frame, and the eigenvalues of the covariance matrix are calculated. If the largest eigenvalue is greater than the second largest eigenvalue, we consider that the five points roughly conform to the line feature. And the direction of the line feature is the eigenvector corresponding to the largest eigenvalue. Then calculate the average of the five points and the direction of the straight line to fit the straight line, and the distance from the line point to the straight line is the residual error. Similarly, for the line feature, we directly fit the plane according to the following equation:

[0091]

[0092] A is a matrix composed of the coordinates of the five points, and a, b, and c can be calculated by the least square idea to obtain the equation of the plane:

[0093] ax+by+cz+1=0

[0094] According to the plane equation and the plane point, the distance from the point to the plane is calculated to obtain the surface residual error. Add all the residual errors to optimize the calculation. We can get a relatively high-precision odometer to eliminate the cumulative error of the odometer.

[0095] Further S8, according to the accurate pose calculated in S7, the displacement relative to the previous key frame is determined whether it can be selected as the next key frame, we take 0.3m as the standard, if the distance from the last key frame is more than 0.3m, it is selected as the next key frame. If the point cloud graph is a key frame, the pose information of the point cloud graph is added to the factor graph, and the pose is added to the kdtree of the saved pose point, and the nearest pose in the kdtree is searched. If the distance between the pose and the nearest pose is only 0.3m, loop detection is performed. The loop detection here uses the icp algorithm. If the icp algorithm matching score is less than 0.1, that is, the average error of the matched point cloud is less than 0.1, it is considered that the loop is successful, and the loop pose information is added to the factor graph. If loop detection occurs, the factor graph optimization is performed once.

[0096] The last step is to judge the change of the last registered pose and the object transfer distance of the grid map obtained by projection in S5. If the change of the pose and the position change of the object in the current frame point cloud graph are greater than 1m, it is considered that the object is a dynamic target and is removed, and then the result of the pose calculation and the key frame point cloud spliced map are obtained.

[0097] Embodiment 2

[0098] Taking the campus scene as an example, because the original LeGO-LOAM algorithm constructs a point cloud map, which will cause ghosting in the map due to the movement of pedestrians and vehicles in the environment, and will cause errors in the odometer registration due to dynamic conditions, therefore, the present application improves the LeGO-LOAM mapping algorithm, and the implementation steps include:

[0099] Step one: the unmanned vehicle moves at a constant speed to collect point cloud graphs, and the point cloud graphs are ground segmented and clustered as shown in the accompanying Figure 2 .

[0100] Step two: preliminarily judge whether it is a dynamic target and eliminate it according to the length, width and height shape of the cluster, as shown in the accompanying Figure 3 .

[0101] Step three: construct a grid map on the basis of step two, and calculate the grid where the dynamic target is located.

[0102] Step four: calculate the odometer and construct the point cloud map according to the point cloud output of the point cloud obtained by removing the dynamic target in the previous step, and the comparison graph without step two is shown in Figure 4 . The left graph is our scheme, and the right graph is a general laser slam scheme. It can be seen that our scheme has effective ghost removal effect.

[0103] Step five: according to the calculated pose relationship between the front and rear frames in step four, calculate whether the grid range where the dynamic target center point should appear is consistent with the final result. If yes, it is considered not to belong to the dynamic target, and the preliminary screened dynamic target point cloud is added to the map. The schematic diagram of this step is shown in FIG. 10. The left and right figures are front and rear frames respectively, and the people inside are dynamic targets. After point cloud registration, the grid where the people are located is different, and it is judged as a dynamic target. Figure 5

[0104] Further, the final residual-free point cloud map as shown in FIG. 11 is obtained. Figure 6

[0105] The above scheme is only a description of a preferred example, but is not limited thereto. In the implementation of the present application, appropriate replacement and / or modification can be made according to the user's needs.

[0106] The number of devices and the scale of processing described herein are used to simplify the description of the present application. The application, modification and change of the present application are obvious to those skilled in the art.

[0107] Although the embodiments of the present application have been disclosed as above, it is not limited to the application listed in the specification and the embodiments. It can be fully applied to various fields suitable for the present application. Those skilled in the art can easily make further modifications. Therefore, the present application is not limited to specific details and figures shown and described herein without departing from the general concept defined by the claims and the equivalent scope.​​

Claims

1. A method for constructing a point cloud map without afterimages based on point cloud clustering, characterized in that: include: Step 1: cluster the input single-frame point cloud image; Step 2: Based on the geometric features of the clustered point cloud, the class is judged to determine the suspected class of the dynamic target in the point cloud image, and the suspected class is projected onto the constructed grid map; Step 3: perform pose estimation and registration on the point clouds other than the suspected class in the point cloud image and the point cloud image of the previous frame; Step 4: The registered pose and the object's shift position in the grid image are compared. If the shift position change differs by more than a predetermined value, the object is identified as a dynamic target and removed to obtain the corresponding point cloud processing image. Step 5: splicing the point cloud processing image with the key frame point cloud to obtain a point cloud map without residual shadows; In step 1, the clustering process includes: S10, roughly removing ground points in the single-frame point cloud image; S11. In the point cloud image with the ground points removed, starting from any point, calculate the geometric relationship with the surrounding points to complete the clustering process; In S10, the ground point removal method includes: S101, for the infinite point in the single-frame point cloud image, determine whether it needs to be eliminated based on whether the three-axis direction point is Nan; S102, projecting the point cloud points in the single-frame point cloud image after removing the infinitely far points into the depth map; S103, in the depth map processed by S102, calculating the angle based on the two point cloud points in the front and back lines, and judging whether it is a ground point according to whether the calculated angle value is greater than a predetermined threshold.

2. The method for constructing a point cloud map without afterimage based on point cloud clustering as claimed in claim 1, characterized in that: In step 2, when projecting the suspected class onto the grid map, only the cluster center points of the suspected class are projected.

3. The method for constructing a point cloud map without afterimage based on point cloud clustering as claimed in claim 1, characterized in that: In step three, the pose estimation and registration process is configured to include: S30, registering the point clouds other than the suspected class in the point cloud image with the point cloud image of the previous frame to obtain a rough pose estimate; S31, registering the pose estimate obtained in S30 with the spliced ​​local map through the point cloud to eliminate the accumulated error and obtain the accurate pose; S32. The displacement of the precise pose obtained in S31 relative to the previous key frame is used to determine whether it is selected as the next key frame. If the judgment result is a key frame, the precise pose is added to the factor graph, and loop detection is performed at the same time. The result of the loop detection is added to the factor graph for optimization.

4. The method for constructing a point cloud map without afterimage based on point cloud clustering as claimed in claim 3, characterized in that: In S30, the pose estimation method is configured to include: S301, for point clouds other than the suspected class in the point cloud image, extract feature points using curvature, and divide the point cloud into line points and surface points based on the judgment of curvature and a predetermined threshold; S302: For a line point, a residual function is constructed to select the line point and the line segment consisting of the adjacent points on the same line from the line point in the point cloud image of the previous frame and the nearest points on the adjacent line. The distance between them is calculated. This distance is the line residual of the line point. For a surface point, the distance between the plane formed by the two nearest points on the same line and the nearest points on the adjacent line is calculated, and all the surface residuals and line residuals are added together to obtain the total residual. The total residual is optimized using LM to obtain a rough pose estimate with the minimum residual.

5. The method for constructing a point cloud map without afterimage based on point cloud clustering as claimed in claim 4, characterized in that: In S31, the method for obtaining the precise posture is configured to include: S310, based on the pose estimation obtained in S30, the point cloud image of the current frame is moved to the map, and the five line points closest to S301 are found through kdtree; S311, judging whether the five points in S310 meet the line feature based on the covariance matrix, and calculating the line residual for the five points that meet the line feature; S312: All residuals are added together for optimization to eliminate the accumulated error of the odometer.

6. The method for constructing a point cloud map without afterimage based on point cloud clustering as claimed in claim 5, characterized in that: In S32, the loop detection process is configured to include: Add the accurate pose to the kdtree of the pose point and search for the pose closest to the pose point in the kdtree. If the distance between the accurate pose and the nearest pose is only 0.3m, use the ICP algorithm for loop detection. During the loop detection process, if the matching score of the ICP algorithm is less than 0.1, the loop is considered successful, and the pose information of the loop is added to the factor graph. When the loop detection occurs, a factor graph optimization is performed at the same time.

Images