Dynamic obstacle filtering method based on clustering tracking

Abstract

The invention relates to a clustering tracking-based dynamic obstacle filtering method, which comprises the following steps of: performing object-level segmentation on an original point cloud through a self-adaptive Euclidean clustering algorithm of a multi-line laser radar, and then constructing a similarity function by calculating a distance between clusters and a bounding box volume ratio to form a bipartite graph for describing an inter-frame matching degree; and a Kuhn-Munkres algorithm is adopted to search for maximum weight matching, and clustering association is realized. By tracking the motion trail of the clustering center, the dynamic obstacle is judged by using short-term motion consistency, and the point cloud information of the dynamic obstacle is filtered out in the mapping process. According to the method, the defects of the traditional method in real-time performance and precision are effectively overcome, and the navigation and positioning performance of the mobile robot in a complex environment is remarkably improved. Compared with the prior art, the method has the advantages of being high in dynamic obstacle filtering efficiency, high in precision, good in continuity and real-time performance and the like.

Description

Technical Field

[0001] The invention relates to the field of robot technology, and in particular to a dynamic obstacle filtering method based on cluster tracking. Background Art

[0002] Mobile robots use 3D laser radar to collect data. When building 3D point cloud maps, they will inevitably encounter dynamic objects, such as vehicles and pedestrians. Due to the characteristics of 3D maps that accumulate multiple frames of point clouds for mapping, dynamic objects will form continuous afterimages and are then regarded as obstacles in the map, affecting the positioning and navigation performance of the mobile robot.

[0003] In recent years, many technical methods have been developed to eliminate the residual images formed by dynamic objects, and to perform fine-tuning based on the characteristics of the map point cloud after the map is built. Although such methods can filter out dynamic obstacles more accurately, they cannot meet the real-time requirements and are difficult to apply in scenarios such as autonomous driving. The existing methods for filtering out dynamic obstacle information in real time during the map building process are not accurate enough, the filtering strength is not appropriate, and the real-time performance needs to be improved.

[0004] Chinese patent application CN115861968A discloses a dynamic obstacle removal method based on real-time point cloud data, which uses the traditional Euclidean clustering method to match dense point clouds through KDTree. However, the dynamic obstacle detection algorithm has low generalization ability and cannot adapt to complex and changeable actual working conditions, and has low efficiency. Therefore, how to improve the filtering accuracy of dynamic obstacles and ensure the real-time and efficiency of filtering is a technical problem that needs to be solved. Summary of the invention

[0005] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a dynamic obstacle filtering method based on cluster tracking. The original point cloud is segmented at the object level by an adaptive Euclidean clustering algorithm of a multi-line lidar, and the Kuhn-Munkres algorithm is used to find the maximum weight match to achieve cluster association, and the motion consistency is judged in a short period of time to determine the dynamic obstacles.

[0006] The purpose of the present invention can be achieved by the following technical solutions:

[0007] According to one aspect of the present invention, a dynamic obstacle filtering method based on cluster tracking is provided, comprising the following steps:

[0008] S1. Use multi-line radar to perform object-level segmentation and clustering on the original point cloud;

[0009] S2, using the obtained distances between the clusters and the volume ratios of the bounding boxes of each type, a bipartite graph describing the degree of matching between each two frames of point cloud data is obtained;

[0010] S3, using the cluster association algorithm to obtain the maximum weight matching of the weighted bipartite graph, thereby obtaining the optimal cluster matching relationship between the two frame point clouds, and associating the matching points to determine the clustering;

[0011] S4, tracking the movement trajectory of each cluster center over a period of time, and judging whether it is a dynamic obstacle based on the short-term movement consistency;

[0012] S5. The point cloud information determined to be a dynamic obstacle is filtered out as a whole to generate filtered laser point cloud data.

[0013] Furthermore, S1 dynamically calculates the clustering threshold according to the current laser beam distance, horizontal and vertical resolution and redundancy coefficient of the multi-line laser radar. When the distance difference between the adjacent laser beam and the current laser beam is less than the threshold, they are identified as being in the same cluster.

[0014] The expression of the dynamic threshold is:

[0015]

[0016] Among them, the lateral resolution of the point cloud is α, the longitudinal resolution is β, the current laser beam return distance is d, the redundancy coefficient is ε, and the threshold for determining the same cluster is threshold.

[0017] Furthermore, the specific steps in S2 include:

[0018] Based on the uniform motion assumption, the bounding box and volume of each cluster in the two-frame point cloud are calculated to obtain the volume ratio of the cluster bounding box; the similarity function is constructed through the cluster center distance and the bounding box volume ratio; the similarity function is used to calculate the matching score between any two clusters in the two-frame point cloud, and the matching score is formed into a weight matrix, and the point cloud clusters of each continuous frame are established into a bipartite graph, and the obtained similarity weight is used as the edge weight. That is, the similarity function is expressed by the matching score between each two clusters, which is directly related to the cluster distance information and the volume ratio of the bounding box. Through the description of cluster similarity, the cluster matching problem is converted into a weighted bipartite graph matching problem.

[0019] The expression of the matching score is:

[0020]

[0021] in, is the volume of the jth cluster bounding box in the previous frame; is the volume of the kth cluster bounding box in this frame; γ is the coefficient that characterizes the distance between cluster centers, which can avoid matching clusters that are too far away.

[0022] The S2 aligns the point cloud of the previous frame and the current frame, and uses the distance information between the clusters in the two frames of data to calculate the coefficient γ representing the distance between the cluster centers, which is expressed as:

[0023]

[0024] Among them, x is the distance between the two cluster centers after alignment, b is the distance threshold, and a is the coefficient that measures the steepness of the attenuation curve. The larger a is, the steeper the attenuation curve is and the faster it decreases. When the two cluster centers completely overlap, γ is 1. When the distance between the two cluster centers exceeds b, the coefficient drops to 0.

[0025] Furthermore, the Kuhn-Munkres cluster association algorithm is used in S3 to continuously update the matching until it is optimal by finding an augmenting path, that is, the maximum weight matching of the weighted bipartite graph. The cluster association algorithm includes starting from the unmatched vertex on the left, updating the matching if found, and then continuing to find the next unmatched point until no new augmenting path can be found.

[0026] The cluster association algorithm also includes recovery in the event of a matching failure. The failure recovery algorithm searches for a match from the most recent frame, and continues searching if a match is found. If all matches are successful or a frame number threshold is exceeded, searching for a matching relationship is stopped.

[0027] In S4, it is determined whether the movement of the target is a continuous movement with a consistent direction in a short period of time. If so, it is a dynamic obstacle; otherwise, it is not a dynamic obstacle.

[0028] The basis for the direction consistency judgment is the target motion change angle, the motion change angle θ of the i-th recording position i for:

[0029]

[0030] in, is the trajectory of a dynamic obstacle that is continuously tracked, k is the number of frames within a set period of time, m is the number of frames selected for judging consistency, and the number of change angles that are less than the set threshold is counted. When the proportion of the number to the total number is less than the set proportion value, it is judged that the direction is consistent.

[0031] Compared with the prior art, the present invention has the following beneficial effects:

[0032] (1) Improved judgment efficiency and filtering accuracy: By using a new Euclidean clustering method based on multi-line radar, by judging whether several adjacent laser beams belong to the same cluster, the need for KDTree search in the traditional Euclidean clustering method is avoided, thereby improving efficiency. In addition, since the threshold can be dynamically adjusted according to the distance between the object and the lidar, the accuracy of filtering out dynamic obstacles is improved.

[0033] (2) Improved the continuity and robustness of dynamic obstacle tracking: By finding matching relationships in the most recent frames when matching fails, the problem of tracking interruption caused by matching failure is solved, thereby improving the continuity and robustness of tracking.

[0034] (3) Reduce the error caused by the jumping of the point cloud center: Since the relative motion of several adjacent frames is used to judge dynamic obstacles, the error caused by the jumping of the point cloud center itself can be effectively suppressed, and the recognition accuracy can be improved by tracking the changes in the target's motion direction. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a flow chart of a dynamic obstacle filtering method based on cluster tracking;

[0036] Figure 2 Schematic diagram of the point cloud bounding box;

[0037] Figure 3 This is a schematic diagram of the maximum weight matching of a bipartite graph;

[0038] Figure 4 A schematic diagram of a typical road environment with dynamic obstacles;

[0039] Figure 5 This is the dynamic obstacle tracking effect diagram;

[0040] Figure 6 Schematic diagram of the effect of the original method on dynamic obstacle filtering;

[0041] Figure 7 It is a schematic diagram of the effect of the method of the present invention on dynamic obstacle filtering. DETAILED DESCRIPTION

[0042] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0043] like Figure 1 As shown, a dynamic obstacle filtering method based on cluster tracking is provided, which includes the following steps:

[0044] S1. Use multi-line radar to perform object-level segmentation and clustering on the original point cloud;

[0045] S2, using the obtained distances between the clusters and the volume ratios of the bounding boxes of each type, a bipartite graph describing the degree of matching between each two frames of point cloud data is obtained;

[0046] S3, using the cluster association algorithm to obtain the maximum weight matching of the weighted bipartite graph, thereby obtaining the optimal cluster matching relationship between the two frame point clouds, and associating the matching points to determine the clustering;

[0047] S4, tracking the movement trajectory of each cluster center over a period of time, and judging whether it is a dynamic obstacle based on the short-term movement consistency;

[0048] S5. The point cloud information determined to be a dynamic obstacle is filtered out as a whole to generate filtered laser point cloud data.

[0049] In S1, the clustering threshold is dynamically calculated based on the current laser beam distance, horizontal and vertical resolution and redundancy coefficient of the multi-line laser radar. When the distance difference between the adjacent laser beam and the current laser beam is less than the threshold, they are identified as being in the same cluster.

[0050] The expression of dynamic threshold is:

[0051]

[0052] Among them, the lateral resolution of the point cloud is α, the longitudinal resolution is β, the current laser beam return distance is d, the redundancy coefficient is ε, and the threshold for determining the same cluster is threshold.

[0053] This embodiment compares the clustering segmentation algorithm with the traditional 3D point cloud clustering method. Table 1 shows the segmentation results obtained on the KITTI dataset. The higher the IoU, the higher the segmentation accuracy:

[0054] Table 1 Performance of different clustering methods on the KITTI dataset

[0055]

[0056] It can be concluded that this embodiment shows the highest segmentation accuracy on the KITTI dataset, which means that this method can more accurately segment point cloud data into clusters of objects. This advantage enables this method to better capture features such as the shape, size, and position of objects when processing complex point cloud data, thereby providing more refined and accurate segmentation results.

[0057] The specific steps in S2 include:

[0058] Based on the uniform motion assumption, the bounding box and volume of each cluster in the two frame point cloud are calculated to obtain the volume ratio of the cluster bounding box; the similarity function is constructed by the cluster center distance and the bounding box volume ratio, such as Figure 2The figure shows the point cloud bounding box. The similarity function is used to calculate the matching scores between any two clusters in the two frames of point cloud. The matching scores are used to form a weight matrix. The point cloud clusters of each consecutive frame are built into a bipartite graph, and the obtained similarity weights are used as edge weights. That is, the similarity function is expressed as the matching score between each two clusters, which is directly related to the volume ratio of the cluster distance information and the bounding box. Through the description of cluster similarity, the cluster matching problem is converted into a weighted bipartite graph matching problem.

[0059] The expression for the matching score is:

[0060]

[0061] in, is the volume of the jth cluster bounding box in the previous frame; is the volume of the kth cluster bounding box in this frame; γ is the coefficient that characterizes the distance between cluster centers, which can avoid matching clusters that are too far away.

[0062] S2 aligns the point cloud of the previous frame and the current frame, and uses the distance information between the clusters in the two frames of data to calculate the coefficient γ representing the distance between the cluster centers, which is between 0 and 1 and is expressed as:

[0063]

[0064] Where x is the distance between the two cluster centers after alignment, b is the distance threshold, and a is the coefficient for measuring the steepness of the attenuation curve. The larger a is, the steeper the attenuation curve is and the faster it decreases. When the two cluster centers completely overlap, γ is 1. When the distance between the two cluster centers exceeds b, the coefficient drops to 0. In this embodiment, a=2, b=2 are set.

[0065] S3 uses the Kuhn-Munkres cluster association algorithm to continuously update the matching by finding augmenting paths until the optimal, that is, the maximum weight matching of the weighted bipartite graph. The cluster association algorithm includes starting from the unmatched vertex on the left, updating the matching if found, and then continuing to find the next unmatched point until no new augmenting path can be found. The schematic diagram of the maximum weight matching of the bipartite graph is as follows Figure 3 shown.

[0066] The cluster association algorithm also includes recovery in the event of a matching failure. The failure recovery algorithm searches for a match from the most recent frame. If a match is successful, the search continues. If all matches are successful or the frame number threshold is exceeded, the search for a matching relationship stops.

[0067] In S4, it is determined whether the target's movement is a continuous movement with a consistent direction in a short period of time. If so, it is a dynamic obstacle; otherwise, it is not a dynamic obstacle.

[0068] The basis for judging the direction consistency is the target motion change angle. The motion change angle θ of the i-th recorded position is i for:

[0069]

[0070] in, is the trajectory of the dynamic obstacle being tracked continuously, k is the number of frames within a set period of time, m is the number of frames selected for judging consistency, and the number of change angles less than the set threshold is counted. When the proportion of the number to the total number is less than the set proportion value, it is judged that the direction is consistent. In this embodiment, k=10, m=5, and the threshold is set to 0.9.

[0071] In this embodiment, by observing the original continuous frames of point cloud data, it can be seen that there are a large number of dynamic obstacles in the urban environment, such as pedestrians, vehicles, etc. Figure 4 As shown. Figure 5 As shown in the figure, different objects are first segmented accurately and obstacles are correctly associated. After tracking the trajectory of the object for a period of time, it is judged to be a dynamic obstacle, and this part of the point cloud is removed, providing a cleaner point cloud for the subsequent mapping process.

[0072] In this embodiment, a clearer and cleaner map is generated without the interference of residual images and dynamic obstacles in the original algorithm. This will help the mobile robot to more accurately perceive and avoid obstacles in positioning and navigation tasks, and improve its overall performance and reliability. Figure 6 and Figure 7 The figure shows the comparative effect of this method and the original algorithm in dynamic obstacle filtering.

[0073] The current mainstream and advanced LeGO-LOAM algorithm is selected as the baseline, and the dynamic obstacle filtering solution of this embodiment is introduced into it. It is tested on the KITTI dataset. The relative pose errors of the two algorithms in the mapping process are compared as shown in Table 2:

[0074] Table 2 Relative pose errors of different methods on the KITTI dataset

[0075]

[0076] By filtering out dynamic obstacles in this embodiment, the mapping quality of most scenes is improved.

[0077] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A dynamic obstacle filtering method based on cluster tracking, characterized in that: The following steps are involved: S1. Use multi-line radar to perform object-level segmentation and clustering on the original point cloud; S2, using the obtained distances between the clusters and the volume ratios of the bounding boxes of each type, a bipartite graph describing the degree of matching between each two frames of point cloud data is obtained; S3, using the cluster association algorithm to obtain the maximum weight matching of the weighted bipartite graph, thereby obtaining the optimal cluster matching relationship between the two frame point clouds, and associating the matching points to determine the clustering; S4, tracking the motion trajectory of each cluster center and judging whether it is a dynamic obstacle based on short-term motion consistency; S5. The point cloud information determined to be a dynamic obstacle is filtered out as a whole to generate filtered laser point cloud data.

2. According to claim 1, a dynamic obstacle filtering method based on label-free cluster tracking is characterized in that: The S1 dynamically calculates the clustering threshold according to the current laser beam distance, horizontal and vertical resolution and redundancy coefficient of the multi-line laser radar. When the distance difference between the adjacent laser beam and the current laser beam is less than the threshold, they are identified as being in the same cluster.

3. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 2, characterized in that: The expression of the dynamic threshold is: Among them, the lateral resolution of the point cloud is α, the longitudinal resolution is β, the current laser beam return distance is d, the redundancy coefficient is ε, and the threshold for determining the same cluster is threshold.

4. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 1, characterized in that: The specific steps in S2 include: On the basis of the uniform motion assumption, the bounding box and volume of each cluster in the two-frame point cloud are calculated to obtain the volume ratio of the cluster bounding box; a similarity function is constructed through the cluster center distance and the bounding box volume ratio; the similarity function is used to calculate the matching score between any two clusters in the two-frame point cloud, the matching score is formed into a weight matrix, the point cloud clusters of each consecutive frame are established into a bipartite graph, and the obtained similarity weight is used as the edge weight.

5. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 4, characterized in that: The expression of the matching score is: in, is the volume of the jth cluster bounding box in the previous frame; is the volume of the kth cluster bounding box in this frame; γ is the coefficient that characterizes the distance between cluster centers, which can avoid matching clusters that are too far away.

6. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 5, characterized in that: The S2 aligns the point cloud of the previous frame and the current frame, and uses the distance information between the clusters in the two frames of data to calculate the coefficient γ representing the distance between the cluster centers, which is expressed as: Among them, x is the distance between the two cluster centers after alignment, b is the distance threshold, and a is the attenuation coefficient. When the two cluster centers completely overlap, γ is 1. When the distance between the two cluster centers exceeds b, the coefficient drops to 0.

7. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 1, characterized in that: In S3, a cluster association algorithm is used to find an augmenting path and continuously update the matching until it is optimal. The cluster association algorithm includes starting from the unmatched vertex on the left to find an augmenting path, updating the matching if found, and then continuing to find the next unmatched point until no new augmenting path can be found.

8. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 7, characterized in that: The cluster association algorithm also includes recovery in the event of a matching failure. The failure recovery algorithm searches for a match from the most recent frame, and continues searching if a match is found. If all matches are successful or a frame number threshold is exceeded, searching for a matching relationship is stopped.

9. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 1, characterized in that: In S4, it is determined whether the movement of the target is a continuous movement with a consistent direction in a short period of time. If so, it is a dynamic obstacle; otherwise, it is not a dynamic obstacle.

10. The method for filtering dynamic obstacles based on label-free cluster tracking according to claim 9, characterized in that: The basis for the direction consistency judgment is the target motion change angle, the motion change angle θ of the i-th recording position i for: in, is the trajectory of a dynamic obstacle that is continuously tracked, k is the number of frames within a set period of time, m is the number of frames selected for judging consistency, and the number of change angles that are less than the set threshold is counted. When the proportion of the number to the total number is less than the set proportion value, it is judged that the direction is consistent.

Images