Dynamic object detection and tracking method based on laser radar point cloud
By combining dual downsampling filtering, density clustering, and Kalman filtering with the Hungarian algorithm based on LiDAR point clouds, the challenge of dynamic object detection and tracking for mobile robots under limited resources was solved, achieving efficient and accurate dynamic object detection and tracking.
Patent Information
- Application Number
- CN202510934347.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-08
- Publication Date
- 2025-10-31
AI Technical Summary
With limited computing resources and sensor capabilities, mobile robots struggle to achieve real-time and accurate dynamic object detection and tracking. Existing methods are affected by sensor limitations and noise, leading to false detections and delayed responses.
A method based on LiDAR point clouds is adopted, which combines dual downsampling filtering, density clustering, segmentation bounding box selection, and Kalman filtering with the Hungarian algorithm to achieve the detection and tracking of dynamic objects. The specific steps include: 3D point cloud downsampling, density-based clustering, bounding box selection of dynamic object detection regions, and Kalman filtering tracking based on a continuous velocity model.
It improves the accuracy and real-time performance of dynamic object detection, can quickly process multiple dynamic objects in complex dynamic environments, reduces computational complexity, and is suitable for resource-constrained mobile robot systems.
Smart Images
Figure CN120876535A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a dynamic object detection and tracking method, which relates to the fields of 3D point cloud processing, state estimation and multi-target tracking, and specifically to a dynamic object detection and tracking method based on lidar point clouds. Background Technology
[0002] Deploying robots in indoor workspaces requires accurate perception of dynamic objects. However, the limited sensor capabilities and computing resources of mobile robots make real-time dynamic object perception challenging. Therefore, developing a robust and efficient dynamic object perception algorithm is crucial for robot navigation.
[0003] The challenges of dynamic object perception in mobile robots can be summarized in three key aspects. First, the fields of computer vision and autonomous driving have extensively researched and developed complex 3D object detection and tracking methods, which require expensive and high-precision sensor setups and large neural networks, demanding substantial computational resources. Unlike autonomous vehicles that benefit from powerful GPUs and high-precision sensor combinations, mobile robots are limited by computational resources and sensor quality, making computationally intensive learning-based methods impractical. Second, detecting objects from onboard sensor data in cluttered environments often leads to significant inaccuracies. Although previous work has employed various methods, such as image-based, point-based clustering, and map-based detection methods, to improve detection accuracy, false detections remain prevalent. Finally, identifying dynamic objects among detected objects is challenging due to sensor noise. Existing recognition methods combining occlusion principles and occupancy updates can sometimes be overly conservative, leading to false detections and thus high latency responses.
[0004] Currently, most methods utilize lightweight perception algorithms based on airborne cameras. These methods use color and depth images to perceive obstacles. For example, clustering algorithms are applied to points projected from depth images, and a lightweight YOLO algorithm is used to identify dynamic pedestrians. Following similar approaches, some researchers estimate the state of objects based on changes in distance and corresponding changes in depth data across consecutive image frames. However, cameras have limited fields of view and are susceptible to high noise; depth cameras typically provide a reliable depth range of only up to 5 meters.
[0005] Furthermore, 3D LiDAR-based methods typically use point clouds for obstacle perception. Early techniques employed continuous frame point cloud subtraction to identify dynamic points, followed by post-processing to optimize the results. More recently, some studies have successfully identified dynamic objects from LiDAR point clouds by utilizing voxel-based occupancy state updates or by leveraging occlusion principles.
[0006] While the methods described above have been successful in detecting and tracking dynamic obstacles, their performance is still limited by the inherent limitations of sensors. For example, depth cameras typically only provide a reliable depth range of up to 4 meters and have a limited field of view; existing recognition methods that combine occupancy principles and occupancy updates can sometimes be too conservative, leading to false detections and thus high latency responses. Summary of the Invention
[0007] To address the problems existing in the background technology, the present invention provides a dynamic object detection and tracking method based on lidar point clouds.
[0008] The technical solution adopted in this invention is:
[0009] The present invention provides a dynamic object detection and tracking method based on lidar point clouds, comprising:
[0010] Step S1: The 3D point cloud of the current area acquired in real time by the mobile robot equipped with LiDAR is processed by a dual downsampling filter module to obtain a downsampled point cloud. Then, clustering processing is performed to obtain the clustered point cloud of each object in the current area.
[0011] Step S2: After processing the clustered point cloud of each object obtained in real time with segmentation bounding boxes, the detection region of the dynamic object is obtained to realize the detection of the dynamic object.
[0012] Step S3: Based on the clustered point cloud and detection area of each dynamic object obtained in real time, use the tracking module to track the dynamic object.
[0013] In step S1, when the 3D point cloud is processed by the dual downsampling filtering module, the 3D point cloud is first filtered by a range filter to obtain a range-filtered point cloud. Then, a point cloud coordinate system with the location of the mobile robot as the origin is constructed by the LiDAR-Inertial Odometry (LIO) algorithm. In order to be used for dynamic object detection of the mobile robot, the LiDAR-Inertial Odometry (LIO) algorithm needs to provide 3D point cloud data and odometry (or the position of the mobile robot) to facilitate the subsequent acquisition of the movement distance of the mobile robot relative to dynamic clustering. The range-filtered point cloud is then transformed from the point cloud coordinate system to the world coordinate system. Then, the range-filtered point cloud in the world coordinate system is filtered by a distance filter to obtain a distance-filtered point cloud. The retention probability of each point in the distance-filtered point cloud is then obtained. Points with a retention probability greater than a preset probability threshold are retained to construct the final downsampled point cloud.
[0014] The range filter specifically removes points in the 3D point cloud that exceed the farthest visible range of the lidar; the distance filter specifically removes points in the range-filtered point cloud in the world coordinate system that are more than a preset distance threshold from the lidar. In order to further reduce the number of points in the point cloud, the distance filter is used to retain nearby points while discarding more distant points.
[0015] The retention probability P dist Specifically as follows:
[0016]
[0017] Where, p i p0 is the i-th point in the range-filtered point cloud; σ is the position of the lidar; dist is a probability parameter.
[0018] In step S1, the clustering process specifically employs the density-based clustering algorithm DBSCAN (Density-Based Spatial Clustering of Applications with Noise).
[0019] In step S2, the segmentation and selection process involves sequentially using dynamic clustering segmentation and minimum range selection. The specific clustering point cloud for each object is as follows:
[0020] Step S2.1: Obtain the global nearest distance between each point in the current object's clustered point cloud and each point in the previous object's clustered point cloud, that is, the distance between the point in the previous time and the point in the previous time that is closest to the point in the current time.
[0021] Step S2.2: Take the average of the global nearest distances of all points in the clustered point cloud of the object at the current time as the average minimum distance D1; take the normalized average variance of the global nearest distances of all points in the clustered point cloud of the object at the current time as the variance distance D2; determine the dynamic objects among all objects based on the average minimum distance D1 and the variance distance D2, and complete the dynamic clustering segmentation.
[0022] Step S2.3: For the clustered point cloud of each dynamic object, take the centroid of the clustered point cloud as the selection center, then obtain the maximum cubic range of the clustered point cloud, and add a preset offset margin Δ in the length, width and height directions of the maximum cubic range. Finally, construct a cubic selection area centered on the centroid of the clustered point cloud as the detection area to complete the minimum range selection.
[0023] By filtering out point cloud data located within the detection area, precise geometric boundaries are provided for subsequent dynamic object tracking and analysis.
[0024] In the step S2.2, when the average minimum distance D1 > t1 and the variance distance D2 < t2, the current object is a dynamic object, where t1 and t2 are the first and second distance thresholds respectively, and t1 > t2.
[0025] In the step S3, the tracking module specifically first obtains the state information of each dynamic object, including position and velocity, using a Kalman filter based on a continuous velocity model according to the clustered point cloud of each dynamic object. Then, the Hungarian algorithm is used, with the Mahalanobis distance between the center of the detection area of each dynamic object and its state information as the assignment cost, and the assignment cost is input into the matching function of the Hungarian algorithm to track each dynamic object. The smaller the cost, the higher the matching degree.
[0026] The electronic device of the present invention includes: a memory and a processor coupled to each other, wherein the memory stores program data, and the processor calls the program data to execute the method as described above.
[0027] The computer-readable storage medium of the present invention stores program data thereon, and when the program data is executed by a processor, the method as described above is implemented.
[0028] The present invention uses a density-based clustering algorithm to process the downsampled point cloud data, which can quickly generate a set of clusters and effectively distinguish static and dynamic objects; it performs well in processing point cloud data in complex dynamic environments and has high efficiency and accuracy.
[0029] The present invention proposes a minimum bounding range method based on centroid and size, which can effectively reduce the amount of point cloud data to be processed while ensuring the detection and tracking accuracy of dynamic objects; when dealing with multiple dynamic objects, it can significantly reduce the computational complexity and improve the real-time performance of the system.
[0030] The present invention uses a Kalman filter based on a continuous velocity model to estimate the state (position and velocity) of dynamic objects, which can quickly adapt to the motion changes of dynamic objects and improve the accuracy and robustness of tracking; it performs well in dealing with fast-moving dynamic objects and is significantly better than traditional constant velocity or acceleration models.
[0031] The present invention uses the Hungarian algorithm to assign trackers to each dynamic cluster and represents the assignment cost as the Mahalanobis distance between the cluster centroid and the estimated state; it can minimize the assignment cost, reduce mis-matching, and improve the accuracy and efficiency of dynamic object tracking.
[0032] The beneficial effects of the present invention are:
[0033] The method of this invention can improve real-time performance while maintaining high detection accuracy; it can be deployed on the onboard computer of a mobile robot and is suitable for planning and obstacle avoidance of mobile robots in complex dynamic environments. Attached Figure Description
[0034] Figure 1 This is a flowchart of the method of the present invention;
[0035] Figure 2 This is a comparison diagram of point cloud processing using the method of the present invention;
[0036] Figure 3 This is a detection image of a single dynamic object indoors using the method of the present invention;
[0037] Figure 4 This is a diagram showing the detection of multiple dynamic objects indoors using the method of the present invention. Detailed Implementation
[0038] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0039] like Figure 1 As shown, the dynamic object detection and tracking method based on LiDAR point clouds provided in this embodiment of the invention is applied to a mobile robot. The LiDAR on the robot can collect point cloud data in consecutive frames. The specific details of the dynamic object detection and tracking method based on LiDAR point clouds of this invention are as follows:
[0040] Step S1: The 3D point cloud of the current area acquired in real time by the mobile robot equipped with LiDAR is processed by a dual downsampling filter module to obtain a downsampled point cloud. Then, clustering processing is performed to obtain the clustered point cloud of each object in the current area. The LiDAR device can be the MID series, RS series, etc. When the 3D point cloud is processed by the dual downsampling filter module, the 3D point cloud is first filtered by a range filter to obtain a range-filtered point cloud. Then, a point cloud coordinate system with the mobile robot's location as the origin is constructed by the LiDAR-Inertial Odometry (LIO) algorithm. In order to be used for dynamic object detection of the mobile robot, the LiDAR-Inertial Odometry (LIO) algorithm needs to provide 3D point cloud data and odometry (or mobile robot position) to facilitate the subsequent acquisition of the movement distance of the dynamic cluster relative to the mobile robot. The range-filtered point cloud is transformed from the point cloud coordinate system to the world coordinate system. Then, the range-filtered point cloud in the world coordinate system is filtered by a distance filter to obtain a distance-filtered point cloud. Then, the retention probability of each point in the distance-filtered point cloud is obtained. Points with a retention probability greater than a preset probability threshold are retained to construct the final downsampled point cloud. The LiDAR-Inertial Odometry (LIO) algorithm specifically employs methods such as FASTLIO (Fast LiDAR-Inertial Odometry) for odometer calculation and POINTLIO (Point-based Laser Inertial Odometry) for high-performance robust laser inertial odometry.
[0041] The range filter specifically removes points in the 3D point cloud that exceed the furthest visible range of the LiDAR, such as a visible range of 5 meters. The distance filter specifically removes points in the range-filtered point cloud in the world coordinate system that are more than a preset distance threshold from the LiDAR. In order to further reduce the number of points in the point cloud, the distance filter is used to retain nearby points while discarding points that are farther away.
[0042] retention probability P dist Specifically as follows:
[0043]
[0044] Where, p i p0 is the i-th point in the range-filtered point cloud; σ is the position of the lidar; dist This is a probability parameter used to control the distribution of downsampling points; in practice, the probability threshold can be set to 0.7.
[0045] The clustering process specifically employs the density-based clustering algorithm DBSCAN.
[0046] In specific implementation, the 3D point cloud data is input into the double downsampling filtering module. First, the range filter is used to only retain the point cloud within a range of 5 meters from the distance sensor. Then, using the odometer input obtained by the FASTLIO algorithm, the point cloud is transformed into the world coordinate system. Then, the distance filter of the present invention is adopted to retain the nearby point cloud, and finally, the point cloud is further filtered by the retention probability. As Figure 2 shown in (a) of Figure 2 and (b) of
[0047] As shown in
[0048] Step S2: The clustering point cloud of each object obtained in real time is processed by using bounding box selection to obtain the detection area of the dynamic object, so as to realize the detection of the dynamic object. The bounding box selection process is to sequentially use dynamic clustering segmentation and minimum range bounding box selection processing for the clustering point cloud of each object, specifically as follows:
[0049] Step S2.1: Obtain the global nearest distance from each point in the clustering point cloud of the object at the current moment to each point in the clustering point cloud of the object at the previous moment, that is, the distance between the point at the previous moment that is closest to the point at the current moment.
[0050] Step S2.2: Take the average value of the global nearest distances of each point in the clustering point cloud of the object at the current moment as the average minimum distance D1; take the normalized average variance of the global nearest distances of each point in the clustering point cloud of the object at the current moment as the variance distance D2; determine the dynamic objects among all objects based on the average minimum distance D1 and the variance distance D2 to complete the dynamic clustering segmentation. When the average minimum distance D1 > t1 and the variance distance D2 < t2, the current object is a dynamic object, where t1 and t2 are the first and second distance thresholds respectively, and t1 > t2. When D1 < t1, the current object is a static object; when D1 > t1 and D2 > t2, the current object is an unknown object.
[0051] For a previously observed static cluster, the average minimum distance D1 from its points to the previous point cloud should be within a small measurement error range. For a continuously moving cluster, the average minimum distance D1 should be large, with the nearest point in the cluster being its previously observed projection, and the nearest point in the current cluster being at its previously observed location. Therefore, the global nearest distance distribution will be relatively uniform, while occluded or newly observed objects will have a non-uniform distribution. Thus, clustering can be classified using two constant thresholds t1 and t2.
[0052] Step S2.3: For the clustered point cloud of each dynamic object, take the centroid of the clustered point cloud as the selection center, then obtain the maximum cubic range of the clustered point cloud, and add a preset offset margin Δ in the length, width and height directions of the maximum cubic range. Finally, construct a cubic selection area centered on the centroid of the clustered point cloud as the detection area to complete the minimum range selection.
[0053] By filtering out point cloud data located within the detection area, precise geometric boundaries are provided for subsequent dynamic object tracking and analysis.
[0054] In practice, for each clustered point cloud, the global nearest neighbor distance from each point in the clustered point cloud to the previous frame's clustered point cloud is measured, i.e., the distance of the point cloud change between the two frames. Then, dynamic objects in the point cloud data are segmented based on these two distances. After clustering dynamic objects according to the above, the centroid of the dynamic object cluster is used as the selection box center. Then, the maximum range of the cluster in each dimension is calculated, and a fixed offset margin Δ is added to this, constructing a cuboid selection region centered on the centroid. g x g y and g z These represent the length, width, and height of the maximum range of the cluster in each dimension. By filtering out the point cloud data within this region, precise geometric boundaries are provided for subsequent dynamic object tracking and analysis. The detection and bounding box results are as follows: Figure 3 and Figure 4 As shown, it can detect single dynamic objects and multiple dynamic objects in the indoor environment. The detected dynamic objects are marked with bounding lines. The colored dots in the figure are dynamic point clouds, and the white dots are static point clouds. This color distinction is only to optimize the display effect and facilitate the observation of the detection results.
[0055] Step S3: Based on the real-time obtained clustered point cloud and detection region of each dynamic object, a tracking module is used to track the dynamic objects. Specifically, the tracking module first uses a Kalman filter based on a continuous velocity model to obtain the state information of each dynamic object, including its position and velocity, based on its clustered point cloud. Then, it uses the Hungarian algorithm, with the Mahalanobis distance between the center of the detection region of each dynamic object and its state information as the allocation cost. This allocation cost is input into the matching function of the Hungarian algorithm to track each dynamic object; the lower the cost, the higher the matching degree.
[0056] For each clustered point cloud of a dynamic object, the Kalman filter performs system modeling of the dynamic object, defines the state vector, state transition matrix and observation matrix of the dynamic object, and then uses the center of the detection region of the dynamic object and the inter-frame displacement as observations to update the Kalman filter and output the state estimate of the dynamic object.
[0057] The Hungarian algorithm creates an assignment matrix H for D dynamic objects and I trackers. The assignment matrix H is used to assign trackers to the clustered point cloud of each dynamic object. The assignment cost is calculated by the Mahalanobis distance between the center of the detection region of the dynamic object and the estimated position of the existing tracker. This assignment cost is substituted into the matching function. The smaller the cost, the higher the matching degree. Efficient tracking is achieved by minimizing this cost.
[0058] In practice, a tracker is designed for each selected dynamic cluster, and a system model is performed on the dynamic object. Its state vector is defined to obtain the system model of each dynamic object. The observation matrix is adjustable, and the inter-frame displacement of the cluster is used as the observation value and updated. Each Kalman filter-based tracker can track and predict the state of different dynamic objects and output the state estimate, which includes position and velocity information.
[0059] Then, trackers are assigned to the dynamic objects. First, a D×I assignment matrix H is created for the detected D dynamic objects and the designed I trackers. Then, the Hungarian algorithm is used to assign trackers to each dynamic cluster. The assignment cost is calculated by using the Mahalanobis distance between the center of the selected dynamic cluster and the estimated location of the existing tracker. This cost is then substituted into the matching function; the lower the cost, the higher the matching degree. If the cost value is less than a set threshold (thresh), the matching degree is determined. min If the object is not properly matched with the tracker, the match will be rejected. Once matched, each dynamic object has a corresponding tracker, achieving accurate tracking and improved efficiency. Finally, the bounding box of dynamic clustering allows the mobile robot to know the size of dynamic obstacles, and the data from different trackers allows the mobile robot to know the speed and position of the moving obstacles, facilitating the design of obstacle avoidance algorithms by researchers.
[0060] This invention processes downsampled point cloud data using a density-based clustering algorithm, effectively distinguishing between static and dynamic objects and ensuring high accuracy in dynamic object detection. A Kalman filter is used to estimate the state (position and velocity) of dynamic objects, further improving tracking accuracy. Mahalanobis distance is used as the allocation cost for the Hungarian algorithm, enabling more accurate assignment of detected dynamic clusters to corresponding trackers and reducing false matches. This method significantly improves real-time performance while maintaining high detection accuracy. Downsampling and efficient clustering algorithms reduce computational complexity, allowing the entire detection and tracking process to be completed quickly. The radar detection portion has an average runtime of 10.74 ms, and the tracking and recognition process has an average runtime of only 4.29 ms. Since these modules can be executed in parallel, it is well-suited for real-time operation in dynamic and complex environments. This method can handle multiple dynamic objects in complex environments and adapts to objects with different motion patterns, achieving a multi-target tracking accuracy of 83.71%. This invention is applicable to onboard computers in mobile robots, enabling efficient operation in resource-constrained hardware environments and providing strong support for mobile robot planning and dynamic obstacle avoidance.
[0061] The above technical solutions only embody the preferred technical solutions of the present invention. Any modifications that may be made by those skilled in the art to certain parts thereof embody the principles of the present invention and fall within the protection scope of the present invention.
Claims
1. A method for dynamic object detection and tracking based on lidar point clouds, characterized in that, Including: Step S1: The three-dimensional point cloud of the current area obtained by the mobile robot equipped with a lidar in real time is processed by a double downsampling filtering module to obtain a downsampled point cloud, and then clustering processing is performed to obtain the clustering point cloud of each object in the current area; Step S2: The clustering point cloud of each object obtained in real time is processed by segmentation bounding box selection to obtain the detection area of dynamic objects, so as to achieve the detection of dynamic objects; Step S3: According to the clustering point cloud and detection area of each dynamic object obtained in real time, a tracking module is used for tracking to achieve the tracking of dynamic objects.
2. The dynamic object detection and tracking method based on lidar point clouds according to claim 1, characterized in that: In the above-mentioned step S1, when the three-dimensional point cloud is processed by the double downsampling filtering module, first, the three-dimensional point cloud is range-filtered by a range filter to obtain a range-filtered point cloud, and then a point cloud coordinate system with the position of the mobile robot as the origin is constructed through the lidar-inertial odometry LIO algorithm, and the range-filtered point cloud is transformed from the point cloud coordinate system to the world coordinate system. Then, the range-filtered point cloud on the world coordinate system is distance-filtered by a distance filter to obtain a distance-filtered point cloud. Then, the retention probability of each point in the distance-filtered point cloud is obtained, and the points with a retention probability greater than the preset probability threshold are retained to construct the final downsampled point cloud.
3. The dynamic object detection and tracking method based on lidar point clouds according to claim 2, characterized in that: The above-mentioned range filter is specifically to remove the points in the three-dimensional point cloud that exceed the farthest visible range of the lidar; the distance filter is specifically to remove the points in the range-filtered point cloud on the world coordinate system that are farther than the preset distance threshold from the lidar.
4. The dynamic object detection and tracking method based on lidar point clouds according to claim 2, characterized in that: The retention probability P dist Specifically as follows: Where, p i p0 is the i-th point in the range-filtered point cloud; σ is the position of the lidar; dist is a probability parameter.
5. The dynamic object detection and tracking method based on lidar point clouds according to claim 1, characterized in that: In the above-mentioned step S1, the clustering processing specifically uses the density-based clustering DBSCAN algorithm.
6. The dynamic object detection and tracking method based on lidar point clouds according to claim 1, characterized in that: In the above-mentioned step S2, the segmentation bounding box selection is to sequentially use dynamic clustering segmentation and minimum range bounding box selection. For the clustering point cloud of each object, specifically as follows: Step S2.1: Obtain the global nearest distance from each point in the clustering point cloud of the object at the current moment to each point in the clustering point cloud of the object at the previous moment; Step S2.2: Take the average value of the global nearest distances of each point in the clustering point cloud of the object at the current moment as the average minimum distance D1; take the normalized average variance of the global nearest distances of each point in the clustering point cloud of the object at the current moment as the variance distance D2; determine the dynamic objects among all objects based on the average minimum distance D1 and the variance distance D2 to complete the dynamic clustering segmentation; Step S2.3: For the clustering point cloud of each dynamic object, take the centroid of the clustering point cloud as the bounding box center, then obtain the maximum cube range of the clustering point cloud, and add a preset offset margin Δ in the length, width, and height directions of the maximum cube range. Finally, construct a cube bounding box area centered on the centroid of the clustering point cloud as the detection area to complete the minimum range bounding box selection.
7. The dynamic object detection and tracking method based on lidar point clouds according to claim 6, characterized in that: In the above-mentioned step S2.2, when the average minimum distance D1 > t1 and the variance distance D2 < t2, the current object is a dynamic object, where t1 and t2 are the first and second distance thresholds respectively, and t1 > t2.
8. The dynamic object detection and tracking method based on lidar point clouds according to claim 1, characterized in that: In step S3, the tracking module first obtains the state information of each dynamic object, including its position and velocity, using a Kalman filter based on the clustered point cloud of each dynamic object. Then, it uses the Hungarian algorithm, taking the Mahalanobis distance between the center of the detection area of each dynamic object and its state information as the allocation cost, and inputs the allocation cost into the matching function of the Hungarian algorithm to track each dynamic object.
9. An electronic device, characterized in that, include: A memory and a processor are coupled to each other, wherein the memory stores program data, and the processor invokes the program data to perform the method as described in any one of claims 1-8.
10. A computer-readable storage medium storing program data thereon, characterized in that, When the program data is executed by the processor, it implements the method as described in any one of claims 1-8.