A dynamic obstacle filtering method based on clustering tracking

Abstract

The application relates to a dynamic obstacle filtering method based on clustering tracking, which is characterized in that: the original point cloud is subjected to object-level segmentation through an adaptive Euclidean clustering algorithm of a multi-line laser radar; subsequently, a similarity function is constructed by calculating the distance between clusters and the volume ratio of the boundary box, so as to form a bipartite graph describing the matching degree between frames; the Kuhn-Munkres algorithm is adopted to find the maximum weight matching, so as to realize the association of clusters; the motion trajectory of the cluster center is tracked, the short-term motion consistency is utilized to judge the dynamic obstacle, and the point cloud information of the dynamic obstacle is filtered in the mapping process. The method effectively solves the deficiency of the traditional method in real-time performance and precision, and significantly improves the navigation and positioning performance of the mobile robot in a complex environment. Compared with the prior art, the application has the advantages of high filtering efficiency, high precision and good continuity and real-time performance for dynamic obstacles.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a dynamic obstacle filtering method based on cluster tracking. BACKGROUND

[0002] Mobile robots use three-dimensional laser radars to collect data. When a three-dimensional point cloud map is established, dynamic objects such as vehicles and pedestrians will inevitably be encountered. Due to the characteristics of the three-dimensional map accumulating multiple frames of point clouds for mapping, dynamic objects will form continuous ghosting, and thus be 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 used to eliminate the ghosting formed by dynamic objects. After mapping is completed, the map point cloud characteristics are used for fine-tuning. Although such methods can accurately filter dynamic obstacles, they cannot meet the real-time requirements and are difficult to apply in scenarios such as autonomous driving. The existing methods for filtering dynamic obstacle information in real time during mapping have low accuracy, inappropriate filtering strength, and 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. The method uses a traditional Euclidean clustering method to perform point cloud matching on dense point clouds using KDTree. However, the dynamic obstacle detection algorithm has low generalization ability and cannot adapt to complex and variable actual working conditions. Therefore, how to improve the filtering accuracy of dynamic obstacles and ensure the real-time performance and efficiency of filtering is a technical problem that needs to be solved. SUMMARY

[0005] The present application aims to overcome the shortcomings of the prior art and provides a dynamic obstacle filtering method based on cluster tracking. The method uses a self-adaptive Euclidean clustering algorithm of a multi-line laser radar to perform object-level segmentation on original point clouds and uses a Kuhn-Munkres algorithm to find the maximum weight matching to realize cluster association. The motion consistency is determined in the short term to determine the dynamic obstacles.

[0006] The object of the present application can be achieved by the following technical solutions:

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

[0008] S1. Using a multi-line radar to perform object-level segmentation and clustering on original point clouds;

[0009] S2. Using the distances between the obtained clusters and the volume ratios of the boundary boxes of each cluster, a bipartite graph is obtained to describe the matching degree of each two frames of point cloud data.

[0010] S3, using a clustering association algorithm to obtain a maximum weight matching of a weighted bipartite graph, so as to obtain an optimal clustering matching relationship between two frames of point clouds, and the association matching points determine the clustering;

[0011] S4, tracking the motion trajectory of each cluster center within a period of time to determine whether it is a dynamic obstacle according to short-term motion consistency;

[0012] S5, filtering out the point cloud information of the point cloud information judged as a dynamic obstacle to generate filtered laser point cloud data.

[0013] Further, S1 dynamically calculates the clustering threshold value according to the current laser beam distance of the multi-line laser radar, the horizontal and vertical resolution and the redundancy coefficient. When the distance difference between the adjacent laser beam and the current laser beam is less than the threshold value, it is determined to be in the same cluster.

[0014] The expression of the dynamic threshold value is:

[0015]

[0016] Wherein, the point cloud horizontal resolution is α, the vertical resolution is β, the current laser beam return distance is d, the redundancy coefficient is ε, and the threshold value obtained for determining the same cluster is threshold.

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

[0018] On the basis of the uniform motion assumption, the boundary box and the volume of each cluster in two frames of point clouds are calculated to obtain the volume ratio of the cluster boundary box; a similarity function is constructed through the cluster center distance and the boundary box volume ratio; the similarity function is used to calculate the matching score between any two clusters in two frames of point clouds, and the matching score is used to form a weight matrix, and the point cloud clusters of each continuous frame are established into a bipartite graph, and the similarity weight obtained 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 boundary box. Through the cluster similarity description, the cluster matching problem is converted into a matching problem of a weighted bipartite graph.

[0019] The expression of the matching score is:

[0020]

[0021] Wherein, is the volume of the jth cluster boundary box in the last frame; is the volume of the kth cluster boundary box in this frame; γ is a coefficient representing the distance of the cluster center, which can avoid matching clusters with too far distance.

[0022] The S2 aligns the last frame and the current frame point cloud, and uses the distance information between clusters in the two frames of data to calculate the coefficient gamma representing the distance between cluster centers, and the expression is:

[0023]

[0024] Wherein, 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 decay curve, the larger a is, the steeper the decay curve is, and the faster the decline is. When the two cluster centers completely coincide, gamma is 1, and when the distance between the two cluster centers exceeds b, the coefficient decreases to 0.

[0025] Further, in the S3, the Kuhn-Munkres clustering association algorithm is used to constantly update the matching by finding an augmented path until the optimal, that is, the maximum weight matching of the weighted bipartite graph. The clustering association algorithm includes finding an augmented path from the left unmatched vertex, updating the matching if the augmented path is found, and then continuing to find the next unmatched point until a new augmented path cannot be found.

[0026] The clustering association algorithm also includes recovery in the case of matching failure, and the failure recovery algorithm finds a match from the nearest frame, and if a successful match is found, it continues to find, and if all matches are successful or exceed the frame number threshold, it stops finding the matching relationship.

[0027] In the S4, it is judged whether the motion of the target is consistent in the short term, if yes, it is a dynamic obstacle, otherwise it is not a dynamic obstacle.

[0028] The basis for the consistent direction judgment is the motion change angle of the target, and the motion change angle of the i-th recorded position is i :

[0029]

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

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

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

[0033] (2) Improved continuity and robustness of dynamic obstacle tracking: By looking for matching relationships in the last few frames in the case of matching failure, the problem of tracking interruption caused by matching failure is solved, improving the continuity and robustness of tracking.

[0034] (3) Reduce the error caused by the jumping of point cloud center: Since the relative motion of adjacent frames is used to judge the dynamic obstacle, the error caused by the jumping of point cloud center itself can be effectively suppressed, and the change of target motion direction is judged to improve the recognition accuracy. BRIEF DESCRIPTION OF DRAWINGS

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

[0036] Figure 2 is a point cloud bounding box schematic diagram;

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

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

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

[0040] Figure 6 is an effect schematic diagram of the original method in dynamic obstacle filtering;

[0041] Figure 7 is an effect schematic diagram of the method of the present application in dynamic obstacle filtering. DETAILED DESCRIPTION

[0042] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should belong to the scope of protection of the present application.

[0043] As shown in Figure 1 , it is a dynamic obstacle filtering method based on clustering tracking, comprising the following steps:

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

[0045] S2, using the distance between the obtained clusters and the volume ratio of the class bounding box, obtaining a bipartite graph describing the matching degree of each two frames of point cloud data;

[0046] S3, using a clustering correlation algorithm to obtain a maximum weight matching of a weighted bipartite graph, so as to obtain an optimal clustering matching relationship between two frames of point clouds, and the correlation matching points determine the clusters;

[0047] S4, tracking the motion trajectory of each cluster center in a period of time to determine whether it is a dynamic obstacle according to short-term motion consistency;

[0048] S5, filtering out the point cloud information of the point cloud information judged as a dynamic obstacle to generate filtered laser point cloud data.

[0049] In S1, the clustering threshold is dynamically calculated according to the current laser beam distance of the multi-line laser radar, the horizontal and vertical resolution and the redundancy coefficient. When the distance difference between the adjacent laser beam and the current laser beam is less than the threshold, it is determined to be in the same cluster.

[0050] The expression of the dynamic threshold is:

[0051]

[0052] Where, the point cloud horizontal resolution is α, the vertical 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 three-dimensional point cloud clustering method. The segmentation results obtained on the KITTI dataset are shown in Table 1. The higher the IoU, the higher the segmentation accuracy:

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

[0055]

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

[0057] The specific steps in S2 include:

[0058] On the basis of the assumption of uniform motion, the boundary box and volume of each cluster in two frames of point clouds are calculated to obtain the volume ratio of the cluster boundary box; a similarity function is constructed by the cluster center distance and the boundary box volume ratio, such as Figure 2The shown is a point cloud bounding box; a matching score between any two clusters in two frames of point clouds is calculated using a similarity function, the matching score is used to form a weight matrix, and the point cloud clusters of each continuous frame are established into a bipartite graph, and the similarity weight is obtained as an edge weight. That is, the similarity function uses the matching score between each two clusters to represent the direct relationship between the cluster distance information and the volume ratio of the bounding box. Through the cluster similarity description, the cluster matching problem is converted into a matching problem of a weighted bipartite graph.

[0059] The expression of the matching score is:

[0060]

[0061] wherein, is the volume of the jth cluster bounding box in the last frame; is the volume of the kth cluster bounding box in this frame; γ is a coefficient representing the distance of the cluster center, which can avoid matching clusters that are too far apart.

[0062] S2 aligns the point clouds of the last frame and the current frame, and calculates the coefficient γ representing the distance of the cluster center using the distance information between the clusters in the two frames of data, which is between 0 and 1, and the expression is:

[0063]

[0064] wherein, x is the distance between the two aligned cluster centers, b is the distance threshold, and a is a coefficient for measuring the steepness of the decay curve. The larger a is, the steeper the decay curve is, and the faster the decline is. When the two cluster centers completely coincide, γ is 1, and when the distance between the two cluster centers exceeds b, the coefficient decreases to 0. In this embodiment, a = 2 and b = 2 are set.

[0065] In S3, the Kuhn-Munkres cluster association algorithm is used to constantly update the matching by finding an augmented path until the optimal, i.e., the maximum weight matching of the weighted bipartite graph. The cluster association algorithm includes finding an augmented path from the left unmatched vertex, updating the matching if an augmented path is found, and then continuing to find the next unmatched point until a new augmented path cannot be found. The bipartite graph maximum weight matching diagram is shown in Figure 3 .

[0066] The cluster association algorithm also includes recovery in the case of matching failure. The failure recovery algorithm finds a match from the nearest frame, and if a successful match is found, it continues to find a match. If all the matches are successful or the frame number threshold is exceeded, the search for the matching relationship is stopped.

[0067] In S4, it is determined whether the motion of the target is a consistent and continuous motion in a short period of time. If yes, it is a dynamic obstacle, otherwise it is not a dynamic obstacle.

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

[0069]

[0070] wherein, is the trajectory of the dynamic obstacle tracked continuously, k is the frame number in a set period of time, m is the selected frame number for consistency determination, the number of change angles less than the set threshold value is counted, and the direction is determined to be consistent when the proportion of the number in the total is less than the set proportion value. In this embodiment, k = 10, m = 5, and the set threshold value is 0.9.

[0071] In this embodiment, by observing the original continuous frame 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., as shown in Figure 4 As shown in Figure 5 , different objects are accurately segmented first, and the obstacles are correctly associated. After a period of trajectory tracking of the objects, it is determined that they are dynamic obstacles, so that this part of the point cloud is removed, providing 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 the positioning and navigation tasks, and improve its overall performance and reliability. As shown in Figure 6 and Figure 7 , the comparative effects of the method and the original algorithm in dynamic obstacle filtering are shown.

[0073] The current mainstream and advanced LeGO-LOAM algorithm is selected as the baseline, and the dynamic obstacle filtering scheme of this embodiment is introduced into it. The test is carried out on the KITTI dataset, and the relative pose error of LOAM and LeGO-LOAM algorithms in the mapping process is compared as shown in Table 2:

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

[0075]

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

[0077] The above merely illustrates the specific embodiments of the present application, but the protection scope of the present application is not limited thereto, and any skilled person in the art can easily think of various equivalent modifications or replacements within the technical range disclosed by the present application, and these modifications or replacements shall be covered within the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.

Claims

1. A dynamic obstacle rejection method based on clustering tracking, characterized in that, The method comprises the following steps: S1, using a multi-line radar to perform object-level segmentation and clustering on original point clouds; S2, using the distance between each cluster obtained and the volume ratio of each class bounding box to obtain a bipartite graph describing the matching degree of each two frames of point cloud data; S3, using a cluster correlation algorithm to obtain the maximum weight matching of the weighted bipartite graph, so as to obtain the optimal cluster matching relationship between two frames of point clouds, and the correlation matching points determine the clusters; S4, tracking the motion trajectory of each cluster center to determine whether it is a dynamic obstacle according to short-term motion consistency; S5, filtering out the point cloud information determined as a dynamic obstacle to generate filtered laser point cloud data; The S1 dynamically calculates a clustering threshold according to the current laser beam distance of the multi-line laser radar, the horizontal and vertical resolution and the redundancy coefficient, and when the distance difference between the adjacent laser beam and the current laser beam is less than the threshold, it is determined to be in the same cluster; The expression of the dynamic threshold is: , Wherein, the point cloud transverse resolution is , the longitudinal resolution is , the current laser beam return distance is , the redundancy coefficient is , the obtained determination is the same cluster threshold value ; The specific steps in the S2 include: On the basis of the uniform motion assumption, the bounding box and the volume of each cluster in two frames of point clouds 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 two frames of point clouds, the matching score is used to form a weight matrix, and each continuous frame of point cloud clusters is established into a bipartite graph, and the similarity weight obtained is used as an edge weight; The expression of the matching score is: ， wherein, is the volume of the boundary box of the i-th cluster in the previous frame; is the volume of the boundary box of the i-th cluster in the current frame; is the volume of the boundary box of the i-th cluster in the previous frame; is the volume of the boundary box of the i-th cluster in the current frame; is a coefficient representing the distance of the cluster center, which can avoid matching clusters with too far distance. The S2 aligns the point clouds of the previous frame and the current frame, and calculates the coefficients representing the distances between the cluster centers using the distance information between the clusters in the two frames of data The expression is: ， wherein, is the distance between the two cluster centers after alignment, is the distance threshold, a is the decay coefficient, and is 1, the two cluster centers distance exceeds b , the coefficient drops to 0.

2. The dynamic obstacle rejection method based on clustering tracking according to claim 1, characterized in that, In the S3, a cluster correlation algorithm is used to find an augmented path and constantly update the matching until the optimum, and the cluster correlation algorithm comprises finding an augmented path from the left side of an unmatched vertex, updating the matching if the augmented path is found, and then continuing to find the next unmatched point until a new augmented path cannot be found.

3. The dynamic obstacle rejection method based on clustering tracking according to claim 2, characterized in that, The cluster correlation algorithm further comprises recovery in the case of matching failure, and the failure recovery algorithm finds a match from the nearest frame, continues to find if the matching is successful, and stops finding the matching relationship if all the matching is successful or the frame number threshold is exceeded.

4. The dynamic obstacle elimination method based on clustering tracking according to claim 1, characterized in that, In the S4, it is determined whether the motion of the target is consistent and continuous in a short period, and if yes, it is a dynamic obstacle, and if not, it is not a dynamic obstacle.

5. The dynamic obstacle elimination method based on clustering tracking according to claim 4, characterized in that, The basis for the direction consistency judgment is the target motion change angle, the motion change angle of the first recorded position is: ， wherein, is a dynamic obstacle trajectory tracked continuously, k is the number of frames in a set period of time, is the number of frames selected for judging consistency, and the number of changes in angle less than a set threshold is counted, and when the proportion of the number to the total number is less than a set proportion value, the direction is judged to be consistent.

Images