Post-processing method for binocular point cloud map containing low-speed dynamic obstacles

Abstract

The invention discloses a post-processing method for a binocular point cloud map containing low-speed dynamic obstacles, the post-processing method can effectively remove point cloud data of the low-speed dynamic obstacles in the point cloud map, and the post-processing method specifically comprises the following steps: S1, mapping in a working scene containing the low-speed dynamic obstacles, obtaining a prior map and data of a working scene; s2, constructing an auxiliary map by walking the same path again according to the mapping completion trajectory; s3, selecting a region of interest for each frame of auxiliary point cloud data and corresponding point cloud data in the prior map, then performing grid division on the region of interest and calculating a height difference descriptor; s4, comparing the height difference descriptor in the prior map with the height difference descriptor of the auxiliary point cloud data at the same position, and if the height difference descriptor of the auxiliary point cloud data is far smaller than the height difference descriptor in the prior map, considering that the point cloud data existing in the current prior map is low-speed dynamic obstacle point cloud data; and S5, performing ground fitting by adopting a region growing algorithm and a principal component analysis method, and deleting point cloud data on the ground in the current grid. According to the method, the point cloud data of the low-speed dynamic obstacles can be effectively removed from the point cloud map containing the low-speed dynamic obstacles, and more accurate and more real conditions are created for tasks such as working path planning of a subsequent robot.

Description

Technical Field

[0001] The present invention belongs to the field of SLAM mapping, and in particular relates to a binocular point cloud map post-processing method for objects containing low-speed dynamic obstacles. Background Art

[0002] At present, most of the SLAM mapping technologies are based on the assumption of static scenes. Carrying out mapping in scenes containing dynamic obstacles will produce large deviations, and may even fail to complete the mapping normally. Therefore, domestic and foreign researchers have carried out a large number of mapping algorithm studies for scenes containing dynamic obstacles. Since the movement of dynamic obstacles is diverse, they are generally divided into high-speed dynamic obstacles and low-speed dynamic obstacles according to the movement speed of dynamic obstacles, and different methods are used to process them. The current post-processing methods for SLAM mapping with binocular cameras are not mature and complete enough, and their performance in performance tests of various open source data sets is also average. Based on this, the present invention is aimed at binocular cameras in mapping scenes containing low-speed dynamic obstacles, and removes low-speed dynamic obstacles from the constructed point cloud map, providing high-quality map data for subsequent use of the map for navigation or other tasks. Summary of the Invention

[0003] To address the aforementioned issues, the present invention proposes a post-processing method for binocular point cloud maps containing low-speed dynamic obstacles. First, the binocular camera acquires the point cloud information of the current map and constructs a priori point cloud map. The vehicle then drives along the same trajectory as the priori map to construct an auxiliary map. Secondly, the grid divisions of the auxiliary point cloud data frame and the priori map are compared. The height difference descriptor for each grid is calculated to determine whether there are dynamic obstacles within the current grid. Based on the determination result, the priori point cloud map is then deleting the point cloud containing dynamic obstacles, completing the post-processing of the point cloud map containing low-speed dynamic obstacles.

[0004] In order to achieve the above object, the technical solution of the present invention is as follows:

[0005] A binocular point cloud map post-processing method for low-speed dynamic obstacles, characterized by comprising the following steps:

[0006] S1: Build a map in a work scene containing low-speed dynamic obstacles to obtain a priori maps and data of the work scene;

[0007] S2: Based on the trajectory of completed mapping, walk the same path again to construct an auxiliary map;

[0008] S3: Select an area of ​​interest for each frame of auxiliary map point cloud data and the corresponding point cloud data in the prior map, then perform grid division on the area of ​​interest and calculate the height difference descriptor;

[0009] S4: Compare the height difference descriptor in the prior map with the height difference descriptor of the auxiliary point cloud data frame at the same position. If the height difference descriptor of the current point cloud data frame is much smaller than the height difference descriptor in the prior map, it is considered that the point cloud data in the current prior map is the point cloud data of a low-speed dynamic obstacle;

[0010] S5: Use the region growing algorithm and principal component analysis method to perform ground fitting and delete the point cloud data above the ground in the current grid.

[0011] The post-processed point cloud map is built by a binocular camera, and the dynamic obstacles in the scene move at a lower speed.

[0012] The space of interest is selected from the camera itself to the maximum depth measurement range, the camera's field of view, and the space from the ground to three meters above the ground as the space of interest, and the space of interest is grid-divided according to the two dimensions of angle and depth.

[0013] The height difference descriptor calculation method for each grid is specifically the difference between the maximum and minimum values ​​in the height direction within the current grid, which is the height difference descriptor within the grid. The height difference descriptor intuitively describes the point cloud distribution of objects above the ground within the current grid.

[0014] The specific method of determining whether there is a dynamic obstacle point cloud in the current grid of the prior map by the comparison method is that if the height difference descriptor of the auxiliary point cloud data frame is less than 0.2 times the height difference descriptor in the current grid of the prior map, it is considered that there is a dynamic obstacle point cloud in the current grid of the prior map.

[0015] The proposed method of ground fitting, using a region growing algorithm and principal component analysis, first selects several points with the lowest height within the grid as seed points. The growth direction is set to the direction of increasing height. Region growing is performed three times based on the distance between the points. The resulting points are considered ground points. Based on these ground points, principal component analysis is used to obtain the minimum eigenvalue, which is the ground normal vector. The ground plane equation is then solved by combining the normal vector with the ground centroid.

[0016] The points above the ground plane are deleted mainly by judging whether the current point satisfies the plane equation of the ground plane. If it satisfies, it is retained; otherwise, it is deleted.

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

[0018] After a binocular camera builds a map in an environment containing low-speed dynamic obstacles, the method of the present invention can be used to remove the low-speed dynamic obstacles from the built map, providing excellent data for subsequent map-based work. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 Figure 1 is an abstract

[0020] Figure 2 The region of interest selected by the binocular camera

[0021] Figure 3 Schematic diagram of rasterization of prior map and auxiliary map

[0022] Figure 4 Flowchart of calculating ground vectors using principal component analysis

[0023] Figure 5 A priori map built based on the open source dataset KITTI binocular sequence

[0024] Figure 6 The effect diagram of using this method to determine whether there is a low-speed dynamic obstacle in the current grid

[0025] Figure 7 This is the effect diagram after ground fitting for dynamic grid

[0026] Figure 8 The prior map processed by the present invention

[0027] Figure 9 The static point retention rate and dynamic point rejection rate performance data of the present invention are

[0028] Figure 10 Robot model established for verification of the present invention in ROS

[0029] Figure 11 A low-speed dynamic obstacle scenario established for the verification of the present invention in ROS

[0030] Figure 12 A priori map established for verification of the present invention in the ROS environment

[0031] Figure 13 The map after post-processing of the prior map by the present invention DETAILED DESCRIPTION

[0032] The present invention is described in detail below with reference to the accompanying drawings and embodiments:

[0033] S1: Build a map in a work scene containing low-speed dynamic obstacles to obtain a priori maps and data of the work scene.

[0034] The original point cloud map of the working environment is obtained by conventional binocular SLAM methods. The point cloud map contains the point cloud information of dynamic obstacles. The point cloud map with dynamic obstacle information is called the prior map, denoted as M. The prior map is composed of the superposition of depth point cloud images at different times. The original point cloud at time t is defined as P. t Q ,P t Q The pose in the entire map coordinate system is The a priori map we can see from this is:

[0035]

[0036] [T] represents the set of the entire time span.

[0037] S2: Based on the trajectory of completed mapping, walk the same path again to construct an auxiliary map.

[0038] At the current pose point As the center, a local auxiliary map is circled in the prior map with the binocular field of view angle and recorded as P t M By comparing P t M and P t Q , we can detect whether there is a point cloud of dynamic obstacles in the area. t M and P t Q There are generally four situations:

[0039] (1) The comparison found that t M There is point cloud data above the ground in P t Q However, no corresponding point cloud data was found.

[0040] (2) The comparison found that t Q There is point cloud data above the ground in P t M However, no corresponding point cloud data was found.

[0041] (3) The comparison found that t Q and P t M There are corresponding point cloud data above the ground.

[0042] (4) The comparison found that tQ and P t M There is no corresponding point cloud data above the ground.

[0043] Obviously, the first and second situations are caused by the movement of dynamic obstacles. The third and fourth situations indicate that there are no dynamic obstacles in the current viewing angle, and all obstacles are static. Since the present invention mainly performs the post-processing work of removing dynamic obstacles from the constructed point cloud map, the main work is to select the dynamic obstacle point cloud in the current map for situation (1) and remove it.

[0044] S3: Select an area of ​​interest for each frame of prior point cloud data and the corresponding point cloud data in the prior map, then grid the area of ​​interest and calculate the height difference descriptor.

[0045] Depending on the binocular camera used, there will be a maximum depth calculation range. The space from the camera itself to the maximum depth measurement range, as well as the camera's field of view and the space from the ground to three meters above the ground are selected as the space of interest for grid division in this article. Figure 2 shown.

[0046] Within this spatial range, the space of interest is divided into grids according to the angle of view and the radius of depth, such as Figure 3 As shown. For the point cloud in each grid, the height difference in the height direction is calculated. This height difference describes the point cloud distribution information in the current grid, which is called the height difference descriptor. The height difference descriptor is defined as shown in formula (2):

[0047] Z des =Z max -Z min (2)

[0048] where Z des is the height difference descriptor within the grid, Z max Z is the maximum value of the point cloud coordinate in the height direction within the grid. min It is the minimum value of the point cloud coordinate in the height direction within the grid.

[0049] S4: Compare the height difference descriptor in the prior map and the height difference descriptor in the auxiliary point cloud data frame at the same location.

[0050] After obtaining the height difference descriptor of the grid, the design according to the present invention can be used to perform P t M and P t Q According to the comparison results set previously, the present invention needs to pay attention to the following: t QThe height difference of the middle grid is much smaller than P t M The height difference of the middle grid, then the current grid will be judged as the area with dynamic obstacles. The area is much smaller than the required parameter for judgment. After experimental comparison, when P t Q The height difference descriptor is less than 0.2 times P t M When the height difference descriptor is used, the present invention believes that P t Q The height difference of the middle grid is much smaller than P t M The height difference of the middle grid is the point cloud where the dynamic obstacle exists in the current grid. By performing the above operation process for each grid in turn, the grid area where the dynamic obstacle exists can be well detected.

[0051] S5: Use the region growing algorithm and principal component analysis method to perform ground fitting and delete the point cloud data above the ground in the current grid.

[0052] The method of fitting the ground mainly uses the region growing algorithm and principal component analysis. First, several points with the lowest point cloud coordinates in the grid in the height direction are selected as seed points, and then three region growings are performed upward to find the ground points. Then, the principal component analysis method is used to calculate the eigenvectors and eigenvalues ​​of the ground point cloud. The algorithm flow is as follows: Figure 4 shown.

[0053] First, assume that the point cloud data consists of N three-dimensional points, represented by the matrix P = [p1, p2, ... p N ] T , where each point p i =(x i ,y i ,z i ). Calculate the center of mass μ of the point cloud, which is the average coordinate value of all points:

[0054]

[0055] This step will centralize the data and eliminate the effect of translation. Then subtract the centroid from each point coordinate to get the centered point:

[0056] q i =p i -μ (4)

[0057] It is represented by the matrix:

[0058] Q=P-μ (5)

[0059] Then construct the covariance matrix and calculate the covariance matrix C of the centralized data:

[0060]

[0061] The covariance matrix is ​​a symmetric matrix that reflects the correlation between the three coordinate axes. Finally, the covariance matrix C is decomposed into its own features:

[0062] Cv j =λ j v j (7)

[0063] where λ j is the eigenvalue, v j is the corresponding eigenvector. The smallest eigenvector is the normal vector of the fitted ground. Based on the normal vector and the points of the ground point cloud, the plane equation of the ground can be calculated. After fitting the ground, the dynamic point cloud in the grid containing dynamic obstacles can be deleted.

[0064] Find P by the above method t M The dynamic point cloud in the image is marked and displayed, which is recorded as The dynamic point detected at a certain moment is Repeat the above operation for the entire time span, and you can finally get a static point cloud map with dynamic obstacles filtered out.

[0065]

[0066] This paper first selects the KITTI open source dataset to test the algorithm effect, and the prior map establishment effect is as follows Figure 5 As shown, the grid division and height difference descriptor effects during the processing are as follows Figure 6 As shown, the final result of ground fitting to remove dynamic obstacles is as follows Figure 7 As shown, the point cloud map after processing is as follows Figure 8 As shown, finally, according to the true value provided by the open source data set, the static point retention rate of the present invention is 88.59%, and the dynamic point rejection rate is 98.31%, which proves that the present invention is effective.

[0067] In addition, the present invention builds a robot model based on ROS2 and simulates a dynamic working environment to verify the algorithm again. Figure 10 As shown, the simulated dynamic operating environment is as follows Figure 11 As shown. The established prior map is as follows Figure 12 As shown in the figure, it can be seen that the green man-made dynamic obstacle produces many ghost images. The effect after using the post-processing method of the present invention is as follows: Figure 13 As shown, you can see that dynamic obstacles are removed.

[0068] The above two experiments show that low-speed dynamic obstacles seriously affect the completion of the visual SLAM mapping task. The post-processing method proposed in this invention can clearly see that the point cloud of low-speed dynamic obstacles in the scene is accurately removed, and the post-processing method has excellent results.

[0069] Those skilled in the art will appreciate that the open source KITTI dataset and the simulation environment built in ROS2 used here are intended to help readers understand the principles of the present invention, and should be understood that the scope of protection of the present invention is not limited to such specific scenarios and implementation cases. Those skilled in the art can make various other specific variations and combinations based on the technical inspiration disclosed by the present invention without departing from the essence of the present invention, and these variations and combinations are still within the scope of protection of the present invention.

Claims

1. A binocular point cloud map post-processing method for low-speed dynamic obstacles, characterized in that: The following steps are involved: S1: Build a map in a work scene containing low-speed dynamic obstacles to obtain a priori maps and data of the work scene; S2: Based on the trajectory of completed mapping, walk the same path again to construct an auxiliary map; S3: Select an area of ​​interest for each frame of auxiliary map point cloud data and the corresponding point cloud data in the prior map, then perform grid division on the area of ​​interest and calculate the height difference descriptor; S4: Compare the height difference descriptor in the prior map with the height difference descriptor in the auxiliary point cloud data frame at the same position. If the height difference descriptor in the auxiliary point cloud data frame is much smaller than the height difference descriptor in the prior map, it is considered that the point cloud data in the current prior map is low-speed dynamic obstacle point cloud data; S5: Use the region growing algorithm and principal component analysis method to perform ground fitting and delete the point cloud data above the ground in the current grid.

2. A binocular point cloud map post-processing method for low-speed dynamic obstacles according to claim 1, characterized in that Point cloud maps are constructed using binocular cameras.

3. The binocular point cloud map post-processing method for low-speed dynamic obstacles according to claim 1 is characterized in that The dynamic obstacles in the working scene move at a low speed, and some real-time point cloud removal methods cannot effectively remove them.

4. The binocular point cloud map post-processing method for low-speed dynamic obstacles according to claim 1 is characterized in that The space from the camera itself to the maximum depth measurement range, the camera's field of view, and the space from the ground to three meters above the ground are selected as the space of interest, and the space of interest is divided into grids according to the two dimensions of angle and depth.

5. The binocular point cloud map post-processing method for low-speed dynamic obstacles according to claim 1 is characterized in that The height difference descriptor of the point cloud in each grid in the prior map and the auxiliary map is calculated, and the height difference descriptor of the current point cloud data frame is compared with the prior map to determine whether the current grid contains a low-speed dynamic obstacle point cloud.

6. The binocular point cloud map post-processing method for low-speed dynamic obstacles according to claim 1 is characterized in that The region growing algorithm and principal component analysis method are used to fit the ground within the grid containing low-speed dynamic obstacles. Then the point cloud of low-speed dynamic obstacles above the ground is deleted and the ground point cloud is retained.

Images