Dynamic object filtering and tracking method and device and computer readable storage medium

Abstract

The embodiment of the invention provides a dynamic object filtering and tracking method and device and a computer readable storage medium, and relates to the technical field of synchronous positioning and mapping. According to the method, a local map is constructed according to a key frame near a current frame laser radar point cloud; template matching is carried out on the current frame laser radar point cloud and the local map in a detection frame, a dynamic object is detected, and an object exists in the detection frame; filtering point clouds of dynamic objects from the current frame laser radar point clouds and the local map; the dynamic object is tracked, and template matching is not carried out on the tracked dynamic object any more. The method can improve the detection precision of the dynamic object, effectively filters the current laser radar point cloud and the point cloud of the dynamic object in the local map, thereby effectively eliminating the interference of the dynamic object in the synchronous positioning and mapping process, and improving the precision and robustness of the synchronous positioning and mapping.

Description

Technical Field

[0001] This application relates to the field of synchronous positioning and mapping technology, and more specifically, to a method, apparatus and computer-readable storage medium for dynamic object filtering and tracking. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a technology for autonomous mobile devices to map and locate themselves in their environment. SLAM techniques can be divided into vision-based methods and laser-based methods.

[0003] Vision-based methods first perform object detection on the image, and then use geometric constraints to remove dynamic feature points. Although vision-based methods can remove dynamic feature points and significantly improve the accuracy of visual odometry, in outdoor scenes, the accuracy of visual odometry is far inferior to that of laser odometry, and it cannot provide accurate pose for autonomous mobile devices over long periods of time.

[0004] Laser-based methods compare the height difference between two point cloud depth maps and identify objects whose height difference exceeds a threshold as dynamic objects. However, these methods compress 3D LiDAR point clouds into 2D depth maps, resulting in the loss of some point cloud information. Therefore, detecting dynamic objects solely based on the height difference between two point cloud depth maps is prone to failure in high-dynamic scenes. Furthermore, most laser-based methods generate point cloud maps after removing dynamic objects offline, failing to provide more accurate laser odometry for localization after real-time removal of dynamic objects.

[0005] In other words, the accuracy and robustness of dynamic object detection technology in simultaneous localization and mapping (SLAM) are insufficient, which means that the accuracy and robustness of SLAM in dynamic environments need to be improved.

[0006] P1CN-MDJT240111, P202406985 Summary of the Invention

[0007] This application proposes a method, apparatus, and computer-readable storage medium for filtering and tracking dynamic objects, in order to solve the technical problem of insufficient accuracy and robustness in dynamic object detection, thereby improving the accuracy and robustness of synchronous positioning and mapping technology in dynamic environments.

[0008] In a first aspect, embodiments of this application provide a method for filtering and tracking dynamic objects. The method includes: constructing a local map based on keyframes near the current frame LiDAR point cloud; performing template matching between the current frame LiDAR point cloud and the local map within a detection box to detect dynamic objects, wherein there are objects within the detection box; filtering out the point cloud of the dynamic object from the current frame LiDAR point cloud and the local map; and tracking the dynamic object, wherein for tracked dynamic objects, the template matching is no longer performed.

[0009] Secondly, embodiments of this application provide a dynamic object filtering and tracking device, which includes: a map building module for building a local map based on keyframes near the current frame LiDAR point cloud; an object detection module for performing template matching between the current frame LiDAR point cloud and the local map within a detection box to detect dynamic objects, wherein there are objects within the detection box; an object filtering module for filtering out the point cloud of dynamic objects from the current frame LiDAR point cloud and the local map; and an object tracking module for tracking the dynamic objects, wherein for tracked dynamic objects, template matching is no longer performed.

[0010] Thirdly, embodiments of this application provide an autonomous mobile device, which includes a camera, a lidar, a memory, and a processor. The memory stores an application program, which, when invoked by the processor, causes the processor to execute the method provided in the embodiments of this application.

[0011] Fourthly, embodiments of this application provide a computer-readable storage medium storing program code, which, when invoked by a processor, causes the processor to execute the method provided in embodiments of this application.

[0012] The dynamic object filtering and tracking method provided in this application has the following technical effects: By matching the current frame's LiDAR point cloud with a local map to detect dynamic objects, the detection and tracking of dynamic objects can be achieved by combining vision and LiDAR, thereby improving the accuracy of dynamic object detection. Furthermore, filtering out dynamic object point clouds from the current LiDAR point cloud and the local map yields higher-precision LiDAR point cloud data and a cleaner local map, effectively eliminating interference from dynamic objects during simultaneous localization and mapping (SMR), and improving the accuracy and robustness of SMR. Attached Figure Description

[0013] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments and drawings obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0014] Figure 1 This application provides a schematic diagram illustrating the operation of an autonomous mobile device according to an embodiment.

[0015] Figure 2 A flowchart illustrating a dynamic object filtering and tracking method provided in an embodiment of this application is shown;

[0016] Figure 3 A flowchart illustrating step S120 provided in an embodiment of this application is shown;

[0017] Figure 4 This illustration shows a grayscale diagram of the visual detection result of the current frame image provided by an exemplary embodiment of this application;

[0018] Figure 5 A flowchart illustrating step S122 provided in an embodiment of this application is shown;

[0019] Figure 6 A schematic diagram of different types of voxels provided in an exemplary embodiment of this application is shown;

[0020] Figure 7 This illustrates a grayscale representation of a lidar scan frame at time t provided in an exemplary embodiment of this application.

[0021] Figure 8 This illustration shows a grayscale diagram of a lidar scan frame at time t+1 provided in an exemplary embodiment of this application.

[0022] Figure 9 This illustration shows a grayscale diagram of a lidar scan frame at time t+2 provided in an exemplary embodiment of this application.

[0023] Figure 10 This illustration shows a grayscale diagram of a lidar scan frame at time t+3 provided in an exemplary embodiment of this application.

[0024] Figure 11 This illustration shows a grayscale diagram of a lidar scan frame at time t, provided in another exemplary embodiment of this application.

[0025] Figure 12 A grayscale schematic diagram of a lidar scan frame at time t+1 provided by another exemplary embodiment of this application is shown;

[0026] Figure 13 A schematic diagram of the structure of an autonomous mobile device (experimental apparatus) provided in an exemplary embodiment of this application is shown;

[0027] Figure 14 This illustration shows a grayscale diagram of a lidar scan frame at time t, provided in yet another exemplary embodiment of this application.

[0028] Figure 15 This illustration shows a grayscale diagram of a lidar scan frame at time t+1 provided in another exemplary embodiment of this application;

[0029] Figure 16 This illustration shows a grayscale diagram of a lidar scan frame at time t+2 provided in another exemplary embodiment of this application;

[0030] Figure 17 This illustration shows a grayscale diagram of a lidar scan frame at time t+3 provided in another exemplary embodiment of this application;

[0031] Figure 18 A schematic diagram of the structure of the dynamic object filtering and tracking device provided in an embodiment of this application is shown;

[0032] Figure 19 A schematic diagram of the structure of the autonomous mobile device provided in an embodiment of this application is shown. Detailed Implementation

[0033] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. It should be noted that the description of "A and / or B" in the following text includes one of the following three cases: (1) A; (2) B; (3) A and B.

[0034] Please see Figure 1 , Figure 1 This diagram illustrates the operation of the autonomous mobile device provided in an embodiment of this application. The autonomous mobile device 100 may include a lidar 110, a camera 120, and an inertial measurement unit (IMU) 130. The lidar 110 may be a three-dimensional lidar or other three-dimensional laser detector. The camera 120 may be a two-dimensional camera or a three-dimensional camera.

[0035] like Figure 1 As shown, the lidar 110 may include a feature extraction module 111, a global matching module 112, a laser odometry module 113, a mapping module 114, a target detection module 115, a template matching module 116, and a dynamic object tracking module 117.

[0036] like Figure 1 As shown, the lidar 110 acquires lidar point clouds as input. The feature extraction module 111 extracts features from the lidar point clouds. The global matching module 112 matches the extracted features with initial values ​​(output of the visual odometry or IMU odometry) to detect obstacles (e.g., dynamic and / or static obstacles), thus achieving global obstacle detection. The lidar odometry 113 removes dynamic object point clouds from the current frame's lidar point cloud based on obstacle detection and dynamic object tracking results, obtaining a static point cloud, and uses this static point cloud for pose estimation of the autonomous mobile device. The mapping module 114 constructs a map based on obstacle detection results and initial values ​​(output of the visual odometry or IMU odometry), outputting a local map. After detecting and tracking dynamic objects, the point clouds of dynamic objects in the local map are filtered out, thus eliminating interference from dynamic objects in the SLAM process. Here, the local map is a lidar map; in the following text, the lidar point clouds in the local map will be referred to simply as local map point clouds.

[0037] like Figure 1 As shown, the LiDAR 110 acquires LiDAR point clouds as input. The target detection module 115 performs target detection on the LiDAR point clouds to obtain target detection results. The target detection results may include detection boxes, and the objects within the detection boxes can be dynamic or static objects. The template matching module 116 matches the LiDAR point clouds within the detection boxes with the local map point clouds to detect whether the objects within the detection boxes are static or dynamic, thus achieving local obstacle detection. After the template matching module 116 detects a dynamic object, the dynamic object tracking module 117 tracks the dynamic object. The process described in this paragraph is the core part of this application, namely, detecting the point clouds of dynamic objects by matching the current frame LiDAR point clouds within the detection boxes with the local map, continuing to track and eliminate dynamic objects, and outputting higher-precision LiDAR ranging and a clean map. For a detailed description, please refer to the embodiment section of the dynamic object filtering and tracking method below.

[0038] like Figure 1As shown, the camera 120 may include a visual feature extraction module 121, a dynamic feature point removal module 122, and a visual odometry 123. The camera 120 acquires images as input. The visual feature extraction module 121 extracts features from the images. The dynamic feature point removal module 122 removes dynamic feature points from the images to obtain static feature points, where dynamic feature points refer to feature points belonging to dynamic objects. The visual odometry 123 estimates the pose of the autonomous mobile device based on the static feature points and outputs the visual odometry result. In scenarios where the visual odometry is effective, the visual odometry result provides initial values ​​for the LiDAR's matching module 112 and mapping module 114.

[0039] like Figure 1 As shown, the inertial measurement unit 130 may include a pre-integration module 131 and an IMU odometry 132. The inertial measurement unit 130 acquires IMU data (e.g., pose data of an autonomous mobile device) as input, and the pre-integration module 131 performs pre-integration based on the IMU data. The IMU odometry 132 estimates the pose of the autonomous mobile device based on the pre-integration result and outputs the IMU odometry result. In scenarios where the visual odometry fails, the IMU odometry result is used to provide initial values ​​for all matching modules 112 and mapping modules 114 of the LiDAR.

[0040] The dynamic object filtering and tracking method in this application can be applied to dynamic object filtering and tracking devices or autonomous mobile devices. The dynamic object filtering and tracking device can be integrated into an autonomous mobile device. Autonomous mobile devices can include, but are not limited to, Automated Guided Vehicles (AGVs) and Autonomous Mobile Robots (AMRs). The dynamic object filtering and tracking method of this application will be described in detail below.

[0041] Please see Figure 2 , Figure 2 A flowchart illustrating a dynamic object filtering and tracking method according to an embodiment of this application is shown. The dynamic object filtering and tracking method may include steps S110 to S140.

[0042] Step S110: Construct a local map based on keyframes near the current frame's LiDAR point cloud.

[0043] In this embodiment of the application, the local map is a laser point cloud map, which includes multiple feature points (i.e., laser points). In the following text, the laser radar point cloud in the local map will be referred to as the local map point cloud.

[0044] In some embodiments, keyframes near the current frame of the LiDAR point cloud can be extracted in the world coordinate system, and a local map can be constructed based on the keyframes. The number of keyframes can be one or more frames; a local map constructed from multiple keyframes has higher accuracy. The nearby keyframes can be multiple keyframes before and after the current frame of the LiDAR point cloud, or keyframes before or after the current frame of the LiDAR point cloud.

[0045] Step S120: Perform template matching between the current frame lidar point cloud and the local map within the detection box to detect dynamic objects, wherein there are objects within the detection box.

[0046] Please see Figure 3 , Figure 3 A flowchart illustrating step S120 according to an embodiment of this application is shown. Step S120 may include steps S121 and S122.

[0047] Step S121: Perform target detection on the current frame of the LiDAR point cloud to obtain the detection box.

[0048] The detection box is a 3D detection box in the LiDAR coordinate system. Objects within the detection box can be dynamic or static. The target detection steps can be performed using methods for target detection in LiDAR point clouds from related technologies; this application does not impose specific limitations. For example, a 3D feature encoder can first divide the disordered LiDAR point cloud into regular cells (e.g., Voxel or Pillar), then a point-based network (e.g., PointNet or PointNet++) can extract features from all points in each cell, and pooling operations can be used to retain the most important features. The retained features are input into a backbone network (VoxelNet or PointPillars) to generate a feature map, where the width, length, and number of channels represent the feature map, respectively. Based on the feature map, a single-stage or two-stage detection head can generate the target detection result. Another example is the CenterPoint target detection algorithm, based on Bird's-Eye-View (BEV) images, which can be used to detect targets in the current frame of the LiDAR point cloud and obtain detection boxes.

[0049] Step S122: Based on the LiDAR point cloud and local map point cloud within the detection box, determine whether the object within the detection box is a dynamic object.

[0050] In some embodiments, where the visual odometry is effective, such as when the visual odometry is working normally, the result of the visual odometry is preferentially selected as the initial value for template matching. Specifically, feature points in the current frame image can be extracted, including static and dynamic feature points; dynamic feature points in the current frame image are filtered out to obtain a visual odometry after removing dynamic feature points; the visual odometry after removing dynamic feature points is used as the initial value for template matching and is used to perform template matching with the local map to detect dynamic objects. The current frame image can be an original true-color image (i.e., an RGB image).

[0051] Among these methods, the following can be used, including but not limited to the Histogram of Oriented Gradients (HOG) algorithm, the Scale-Invariant Feature Transform (SIFT) algorithm, the Speeded Up Robust Features (SURF) algorithm, the Oriented FAST and Rotated BRIEF (ORB) algorithm, the Local Binary Patterns (LBP) algorithm, and the Haar classifier, to extract feature points from the current frame image.

[0052] In some embodiments, optical flow can be used before filtering out dynamic feature points, based on the feature points of the previous frame image (denoted as P). i-1 Track the feature points of the current frame image (denoted as P). i Define the feature points within the detection box in the current frame image as target feature points; determine the epipolar line (denoted as L) of the current frame image based on the fundamental matrix (denoted as F). i Based on the distance between each target feature point and the epipolar line, determine whether each target feature point is a dynamic feature point.

[0053] One optical flow method is the Lucas-Kanade (LK) optical flow method. The Lucas-Kanade optical flow method is a dense optical flow estimation method used to calculate the motion vector of each pixel in an image. It assumes that the grayscale value of a pixel does not change significantly between adjacent frames, therefore the motion vector can be solved by minimizing the sum of squares of the pixel value residuals. The Lucas-Kanade optical flow method may include the following steps:

[0054] ① For the previous frame image and the current frame image, select the region of interest, and then select some key points (such as face points and corner points) within the region of interest.

[0055] ② For each keypoint, calculate its position in the previous frame and use the surrounding pixels to calculate a local image gradient.

[0056] ③ In the current frame image, the position of the key point in the previous frame image is used as the initial position, and iteration is performed along its gradient direction until a position is found that minimizes the pixel residual between the two images. Specifically, step ③ requires solving a least squares problem, that is, finding a displacement vector that minimizes the pixel value residual between the two images.

[0057] ④ Repeat step ③ until the motion vectors of all key points have been calculated.

[0058] ⑤ These motion vectors can be used to estimate the motion of the camera or the motion of an object.

[0059] In computer vision, the fundamental matrix F is a 3×3 matrix that represents the correspondence between image points in a stereo pair. In epipolar geometry, for a pair of corresponding points in a stereo pair, their homogenized image coordinates are P and P′, representing a straight line (epidial line L) that must pass through P′. i This means that all pairs of corresponding points in a stereo image satisfy P′. T FP = 0. The F matrix contains the spatial geometric relationship (extrinsic parameters) between the two images of the stereo pair during capture, as well as the camera calibration parameters (intrinsic parameters), including rotation, translation, principal point coordinates, and focal length. Because the rank of the F matrix is ​​2 and it can be freely scaled, only 7 pairs of corresponding points are needed to estimate the value of F.

[0060] After obtaining the epipolar lines, the 3D detection bounding box output by the LiDAR target detection module can be obtained. Since the detection bounding box in the LiDAR coordinate system is 3D, while the detection bounding box in the camera pixel coordinate system is 2D, the 3D detection bounding box in the LiDAR coordinate system (denoted as P) can be used to obtain the detection bounding box. box ) is converted into a two-dimensional detection box in the camera pixel coordinate system (denoted as P). xy The feature points in the 2D detection bounding box of the current frame image are used as target feature points. Then, the distance between the target feature points and the epipolar line is calculated (denoted as d). i ).

[0061] Based on experience or experimental data, a distance threshold can be preset. For each target feature point, it can be determined whether the distance between the target feature point and the epipolar line is greater than the distance threshold. If the distance between the target feature point and the epipolar line is greater than the distance threshold, the target feature point is determined to be a dynamic feature point. If the distance between the target feature point and the epipolar line is not greater than the distance threshold, the target feature point is determined to be a static feature point.

[0062] As an example, please refer to Figure 4 , Figure 4 This diagram illustrates a grayscale representation of the visual detection result of the current frame image provided in an exemplary embodiment of this application. Circles represent static feature points (i.e., feature points of static objects), triangles represent dynamic feature points (i.e., feature points of dynamic objects), and rectangles represent two-dimensional detection boxes in the current frame image. Figure 4 As shown, in scenarios where visual odometry is effective, visual odometry, as an initial value, can filter out the point cloud of dynamic objects in the current LiDAR point cloud, obtaining a local map of the LiDAR point cloud that only includes static LiDAR objects. This can effectively identify and filter dynamic feature points, improving the accuracy of visual ranging in dynamic scenes.

[0063] In other embodiments, in scenarios where visual odometry fails, such as drastic changes in lighting, lack of image texture, or camera obstruction, the results of IMU odometry can be used as initial values. Although the accuracy of IMU odometry is lower than that of visual odometry, it can still provide the necessary initial values ​​when visual odometry fails, thus ensuring the overall robustness of the method.

[0064] In some other embodiments, the validity of the visual odometry can be determined based on the current frame image upon acquisition. For example, if the current frame image indicates drastic changes in lighting, lacks texture, or is partially occluded, the visual odometry can be determined to be faulty; otherwise, it can be determined to be valid.

[0065] As shown in the expression below, the pose of the LiDAR in the world coordinate system can be obtained using visual odometry, and the local map can be transformed to the LiDAR coordinate system of the current frame. The LiDAR point cloud and the local map point cloud within the detection box can be obtained. The LiDAR point cloud and the local map point cloud within the detection box are then matched with templates to identify whether the object within the detection box is a dynamic object or a static object, thereby achieving local obstacle detection and further improving the accuracy of dynamic object detection.

[0066]

[0067] in, This indicates the transformation relationship between the lidar coordinate system and the local map. This indicates the transformation relationship between the coordinate systems of the lidar and the camera. This indicates the transformation relationship between the world coordinate system and the camera coordinate system. This indicates the conversion relationship between the world coordinate system and a local map.

[0068] Please see Figure 5 , Figure 5A flowchart illustrating step S122 of an embodiment of this application is shown. The step of determining whether the object within the detection frame is a dynamic object in step S122 may include steps S1221 to S1223.

[0069] Step S1221: Use an octree to occupy the map, and discretize the LiDAR point cloud and local map point cloud within the detection box into voxels. Each voxel is of one type: dynamic voxel, static voxel, or pseudo-dynamic voxel.

[0070] The lidar point cloud within the detection frame is obtained (denoted as...). ) and local map point cloud (denoted as Following this, this application uses the concept of an octree occupancy map to occupy the grid, and discretizes the LiDAR point cloud and local map point cloud within the detection box into voxels. Within the occupied network, the same space can be marked as either idle or occupied, and these states can be switched. The octree occupancy map is a tree-like data structure used to describe 3D space. Each node in the octree occupancy map represents a volume element of a cube, and each node has eight child nodes. The sum of the volume elements represented by the eight child nodes equals the volume of the parent node.

[0071] This application proposes a classification method for dynamic voxels, static voxels, and pseudo-dynamic voxels for each voxel. Please refer to [link / reference]. Figure 6 , Figure 6 This illustration shows a schematic diagram of different types of voxels provided in an exemplary embodiment of this application. Dynamic voxels ( Figure 6 The light gray cube in the image refers to a voxel that is not simultaneously occupied by both the current frame's LiDAR feature points and local map feature points. In other words, a dynamic voxel is occupied only by either the current frame's LiDAR feature points or local map feature points. Dynamic voxels are assigned a larger dynamic weighting factor, thus effectively distinguishing dynamic objects. Static voxels ( Figure 6 The white cube in the image refers to a voxel in the current frame's LiDAR point cloud that can be found in the octree-occupied map as a local map feature point that is very close to it. Objects corresponding to static voxels are identified as static objects, which is beneficial for building and updating a stable map. Figure 6 The dark gray cube shown has a large difference in the distance between feature points in its voxel for slow-moving objects, which poses a potential risk of dynamic voxels. Therefore, it can be classified as a pseudo-dynamic voxel.

[0072] Specifically, the octree is used to occupy the map, and the LiDAR point cloud and local map point cloud within the detection box are discretized into voxels. Based on the distribution of LiDAR feature points and local map feature points within each voxel, the type of each voxel is determined to be one of dynamic voxel, static voxel, or pseudo-dynamic voxel.

[0073] If a voxel is occupied only by LiDAR feature points or only by local map feature points, it can be determined that the object corresponding to the voxel is a fast-moving object, and the type of voxel can be determined as a dynamic voxel.

[0074] If a voxel is occupied by both lidar feature points and local map feature points, and the distance between these feature points is less than a first distance, then the object corresponding to the voxel can be determined to be a static object, and the voxel type can be determined to be a static voxel. The first distance can be preset based on experience or experimental data.

[0075] If a voxel is occupied by both lidar feature points and local map feature points, and the distance between these feature points is greater than or equal to a second distance, the object corresponding to the voxel can be identified as a slow-moving object, and the voxel type can be determined as a pseudo-dynamic voxel. The second distance can be preset based on experience or experimental data. To facilitate the differentiation between static and pseudo-dynamic voxels, the second distance can be set to be greater than or equal to the first distance.

[0076] Step S1222: Determine the occupancy rate of dynamic voxels within the detection box based on the distance difference between feature points within the voxel.

[0077] The method described in this application can further determine the dynamic probability (i.e., the occupancy rate of dynamic voxels) of each voxel by calculating the distance difference between feature points within each voxel, thereby helping to identify pseudo-dynamic voxels and improve the accuracy and reliability of the map.

[0078] As shown in the expression below, the average distance difference between feature points in the pseudo-dynamic voxel can be determined based on the number and location of LiDAR feature points within the same voxel and the number and location of local map feature points; the occupancy rate of dynamic voxels within the detection box can be determined based on the number of dynamic voxels, the number of pseudo-dynamic voxels, and the average value.

[0079]

[0080] in, P represents the average distance difference between feature points in a pseudo-dynamic voxel, h represents the number of lidar feature points within the voxel, and P represents the average distance difference between feature points in the pseudo-dynamic voxel. i The voxel represents the position of the lidar feature point in the world coordinate system, w represents the number of local map feature points in the voxel, and S represents the position of the lidar feature point in the world coordinate system. j Indicates with P i The location of the nearest local map feature point in the world coordinate system. This indicates the occupancy rate of dynamic voxels within the detection frame. Indicates the number of dynamic voxels. This indicates the number of pseudo-dynamic voxels.

[0081] In other embodiments, to improve the accuracy of the determined occupancy rate, a weighted judgment method can be used to determine the occupancy rate of dynamic voxels within the detection frame for all voxels. For dynamic voxels, which are more likely to move, a weight ε can be set for them. D (e.g. ε) D =1). Conversely, assuming the static voxel is stationary, the weight of the static voxel can be set to ε. S (e.g. ε) S =0). For pseudo-dynamic voxels, which are designed for slowly moving objects, considering the possibility of them being static, the weight of the pseudo-dynamic voxel can be set to ε. K (e.g. ε) K =0.2). As shown in the expression below, the weights of the dynamic voxels and the pseudo-dynamic voxels can be obtained; based on the number and weights of the dynamic voxels, the number and weights of the pseudo-dynamic voxels, and the average value, the occupancy rate of the dynamic voxels within the detection box is determined.

[0082]

[0083] By calculating the occupancy rate of dynamic voxels using an octagonal occupancy map, dynamic objects in the current frame of the LiDAR point cloud can be effectively identified.

[0084] Step S1223: Determine whether the object in the detection box is a dynamic object based on the occupancy rate of the dynamic voxels within the detection box.

[0085] An occupancy rate threshold can be preset based on experience or experimental data. The calculated occupancy rate of dynamic voxels within the detection frame is compared with the occupancy rate threshold. If the occupancy rate of dynamic voxels within the detection frame is greater than the occupancy rate threshold, the object within the detection frame is determined to be a dynamic object. If the occupancy rate of dynamic voxels within the detection frame is not greater than the occupancy rate threshold, the object within the detection frame is determined to be a static object.

[0086] In some embodiments, since dynamic objects can interfere with synchronous localization and mapping, for example, dynamic objects can interfere with the pose estimation of the laser odometry, the point cloud of dynamic objects can be filtered out from the current frame lidar point cloud and local map.

[0087] Step S130: Filter out the point cloud of dynamic objects from the current frame lidar point cloud and the local map.

[0088] By filtering out the point clouds of dynamic objects in the current lidar point cloud and local map, only static feature points can be retained for subsequent pose estimation of autonomous mobile devices and point cloud map creation, thereby improving the accuracy of synchronous positioning and mapping.

[0089] Step S140: Track the dynamic object. For the tracked dynamic object, template matching is no longer performed.

[0090] In some embodiments, to avoid repeated detection of the same dynamic object, this application introduces a dynamic object tracking mechanism. A pre-defined motion model (e.g., a uniform motion model) can be used to predict the pose of the dynamic object, obtaining a prediction result; target detection is performed on the next frame of the LiDAR point cloud to obtain a new detection box; the prediction result and the new detection box are associated to track the dynamic object.

[0091] For example, when a dynamic object is identified in the current frame of the LiDAR point cloud, a uniform motion model can be used to recursively predict the state of the dynamic object. In subsequent LiDAR scan frames, the predicted result can be matched with the pose of the new detection box, thereby achieving continuous tracking of the dynamic object and updating the pose of its detection box in real time. For example, assuming the predicted result is that dynamic object A has moved 10 meters, and the new detection box closest to dynamic object A has moved 9 meters, then the new detection box is close to the predicted result, and can be considered to be dynamic object A, thus allowing tracking of dynamic object A.

[0092] This application uses state prediction and data fusion to track dynamic objects, which can improve the accuracy of dynamic object recognition and significantly improve the efficiency of the entire synchronous localization and mapping.

[0093] Please see Figures 7 to 10 , Figure 7 This illustration shows a grayscale representation of a lidar scan frame at time t, provided in an exemplary embodiment of this application. Figure 8 This illustration shows a grayscale diagram of a lidar scan frame at time t+1 provided in an exemplary embodiment of this application. Figure 9 This illustration shows a grayscale diagram of a lidar scan frame at time t+2 provided in an exemplary embodiment of this application. Figure 10 This illustration shows a grayscale diagram of a lidar scan frame at time t+3 provided in an exemplary embodiment of this application. Figures 7 to 10 In the diagram, the type I detection box is the detection box for dynamic objects, and the type II detection box is the detection box for static objects. Figures 7 to 10 The process of tracking a dynamic object from time t to time t+3 is shown.

[0094] In other embodiments, after detecting a moving object, a vision-based method can also be used for moving object tracking. For example, the moving object can be marked, and then tracked based on the mark. Alternatively, the identification information of the moving object (e.g., a license plate number) can be recognized, and then tracked based on the license plate number.

[0095] This application tracks dynamic objects, using previously detected dynamic objects as prior information, and smoothly and efficiently handles the interference caused by dynamic objects to localization and mapping.

[0096] Steps S110 to S140 have the following technical effects: Matching the current frame's LiDAR point cloud with the local map to detect dynamic objects allows for the detection and tracking of dynamic objects by combining vision and LiDAR, thereby improving object detection accuracy. Furthermore, filtering out the point cloud containing dynamic objects from the current LiDAR point cloud and the local map yields higher-precision LiDAR point cloud data and a cleaner local map, effectively eliminating interference from dynamic objects during simultaneous localization and mapping (SLT), thus improving the accuracy and robustness of SLT.

[0097] This application provides detailed experimental verification of the laser vision fusion dynamic culling technique in dynamic scenes using the publicly available KITTI dataset and a self-made dataset. To achieve quantitative comparison and evaluation, the LVI-SAM algorithm is selected as the benchmark to measure the accuracy of the laser odometry. Furthermore, the root mean square error (RMSE) is used as the evaluation metric, specifically including the absolute trajectory error (ATET, unit: m) of the translation component and the absolute trajectory error (ATER, unitless) of the rotation component, thus comprehensively measuring the performance of the laser odometry. In addition, this application extracts typical dynamic scene segments from the dataset and conducts in-depth qualitative analysis. All experiments were performed on a high-performance computer configured with an Intel i9-13900 CPU, 16GB RAM, and a GeForce RTX 4060 8GB graphics card.

[0098] Table 1

[0099]

[0100]

[0101] As shown in Table 1, the method described in this application significantly improves the localization accuracy in dynamic scenes through an effective dynamic feature point removal strategy. Compared with the LVI-SAM algorithm, the method in this application demonstrates superior performance on multiple key sequences. In particular, the method shows a more significant advantage in the KITTI_2011_0930_0016 and KITTI_2011_0926_0013 sequences. In the KITTI_2011_0930_0016 sequence, a car is approaching an intersection surrounded by numerous moving vehicles, which significantly interfere with pose estimation. However, the method in this application successfully overcomes this challenge by accurately removing these dynamic feature points, achieving more precise localization. Similarly, in the KITTI_2011_0926_0013 sequence, a fast-moving car is in front of the robot, which also affects pose estimation. However, the method of this application can still accurately identify and remove these dynamic feature points, thereby achieving more robust performance. The method of this application can improve the accuracy of laser odometer by about 5%.

[0102] Please see Figure 11 and Figure 12 , Figure 11 This illustration shows a grayscale diagram of a lidar scan frame at time t, provided by another exemplary embodiment of this application. Figure 12 A grayscale schematic diagram of a lidar scan frame at time t+1 provided by another exemplary embodiment of this application is shown. Figure 11 and Figure 12 This describes a dynamic scene from the KITTI_2011_0930_0016 dataset. The robot is currently at an intersection, waiting to cross. There are stationary vehicles parked on the roadside and vehicles crossing the intersection. A screenshot is shown below. Figure 11 and Figure 12 The point cloud data at two adjacent times t and t+1 shown demonstrate that the method of this application can effectively segment the laser points within the detection box in the current frame of the LiDAR point cloud. Furthermore, by matching the current frame of the LiDAR point cloud with the point cloud in the local map within the detection box, it can effectively separate the dynamic point cloud from the static point cloud.

[0103] This application also further tested its method in a real-world environment; please refer to [link / reference]. Figure 13 , Figure 13This illustration shows a schematic diagram of the structure of an autonomous mobile device (experimental apparatus) provided in an exemplary embodiment of this application. This application uses a Hesai 16-line lidar, an Xsence G710 IMU, and a Realsence D435 camera (using only its raw RGB images, not its depth images) to conduct algorithm testing in an open outdoor area, with numerous stationary cars parked along the roadside and cars also moving on the road.

[0104] Please see Figure 14 , Figure 14 This illustration shows a grayscale diagram of a lidar scan frame at time t, provided in another exemplary embodiment of this application. Figure 15 This illustration shows a grayscale diagram of a lidar scan frame at time t+1 provided in another exemplary embodiment of this application. Figure 16 This illustration shows a grayscale diagram of a lidar scan frame at time t+2 provided in another exemplary embodiment of this application. Figure 17 This illustration shows a grayscale diagram of a lidar scan frame at time t+3, provided in another exemplary embodiment of this application. Figures 14 to 17 As shown, in the experimental scenario, there is a moving truck and a car parked on the side of the road. The method of this application can correctly distinguish between the stationary car and the moving truck, and differentiate their dynamic laser feature points. The area selected by the white rectangular box is the dynamic laser feature point.

[0105] Please see Figure 18 , Figure 18 A schematic diagram of the dynamic object filtering and tracking device provided in this application embodiment is shown. The dynamic object filtering and tracking device 200 can be integrated into the autonomous mobile device 100. The dynamic object filtering and tracking device 200 may include: a map building module 210, an object detection module 220, an object filtering module 230, and an object tracking module 240. Specifically: the map building module 210 is used to build a local map based on keyframes near the current frame LiDAR point cloud. The object detection module 220 is used to perform template matching between the current frame LiDAR point cloud and the local map within a detection frame to detect dynamic objects, where objects are present within the detection frame. The object filtering module 230 is used to filter out the point cloud of dynamic objects from the current frame LiDAR point cloud and the local map. The object tracking module 240 is used to track the dynamic objects; for tracked dynamic objects, template matching is no longer performed.

[0106] In some embodiments, the object detection module 220 is further configured to perform target detection on the current frame LiDAR point cloud to obtain a detection box; and determine whether the object in the detection box is a dynamic object based on the LiDAR point cloud and the local map point cloud within the detection box.

[0107] In some embodiments, the object detection module 220 is further configured to use an octree to occupy the map, discretize the LiDAR point cloud and local map point cloud within the detection box into voxels, each voxel being of one of the following types: dynamic voxel, static voxel, or pseudo-dynamic voxel; determine the occupancy rate of dynamic voxels within the detection box based on the distance difference between feature points within the voxels; and determine whether the object within the detection box is a dynamic object based on the occupancy rate of dynamic voxels within the detection box.

[0108] In some embodiments, the object detection module 220 is further configured to determine the average distance difference between feature points in the pseudo-dynamic voxel based on the number and location of LiDAR feature points within the same voxel and the number and location of local map feature points; and to determine the occupancy rate of dynamic voxels within the detection frame based on the number of dynamic voxels, the number of pseudo-dynamic voxels, and the average value.

[0109] In some embodiments, the object detection module 220 is further configured to obtain the weights of the dynamic voxels and the weights of the pseudo-dynamic voxels; and determine the occupancy rate of the dynamic voxels within the detection frame based on the number and weights of the dynamic voxels, the number and weights of the pseudo-dynamic voxels, and the average value.

[0110] In some embodiments, the object detection module 220 is further configured to use an octree to occupy the map, discretize the LiDAR point cloud and local map point cloud within the detection box into voxels; and determine the type of each voxel as dynamic voxel, static voxel, or pseudo-dynamic voxel based on the distribution of LiDAR feature points and local map feature points within each voxel.

[0111] In some embodiments, the object detection module 220 is further configured to determine the type of the voxel as a dynamic voxel if the voxel is occupied only by LiDAR feature points or only by local map feature points; determine the type of the voxel as a static voxel if the voxel is occupied by both LiDAR feature points and local map feature points and the distance between the LiDAR feature points and local map feature points is less than a first distance; and determine the type of the voxel as a pseudo-dynamic voxel if the voxel is occupied by both LiDAR feature points and local map feature points and the distance between the LiDAR feature points and local map feature points is greater than or equal to a second distance, wherein the second distance is greater than or equal to the first distance.

[0112] In some embodiments, the object detection module 220 is further configured to determine that the object in the detection box is a dynamic object if the occupancy rate of the dynamic voxels in the detection box is greater than the occupancy rate threshold; and to determine that the object in the detection box is a static object if the occupancy rate of the dynamic voxels in the detection box is not greater than the occupancy rate threshold.

[0113] In some embodiments, the object tracking module 240 is further configured to predict the pose of a dynamic object using a pre-set motion model to obtain a prediction result; perform target detection on the next frame of the lidar point cloud to obtain a new detection box; and associate the prediction result with the new detection box to track the dynamic object.

[0114] In some embodiments, the object detection module 220 is further configured to extract feature points in the current frame image, the feature points including static feature points and dynamic feature points; filter out the dynamic feature points in the current frame image to obtain a visual odometry after removing the dynamic feature points; and use the visual odometry as the initial value for template matching to perform template matching with the local map.

[0115] In some embodiments, the object detection module 220 is further configured to employ optical flow to track feature points of the current frame image based on feature points of the previous frame image, define feature points within the detection box in the current frame image as target feature points; determine the epipolar line of the current frame image based on the fundamental matrix; and determine whether each target feature point is a dynamic feature point based on the distance between each target feature point and the epipolar line.

[0116] In some embodiments, the object detection module 220 is further configured to determine the target feature point as a dynamic feature point if the distance between the target feature point and the epipolar line is greater than a distance threshold; and to determine the target feature point as a static feature point if the distance between the target feature point and the epipolar line is not greater than a distance threshold.

[0117] Those skilled in the art will clearly understand that the apparatus provided in the embodiments of this application can implement the methods provided in the embodiments of this application. The specific working process of the described apparatus and modules can be found in the corresponding processes of the methods in the embodiments of this application, and will not be repeated here.

[0118] In the embodiments provided in this application, the coupling, direct coupling, or communication connection between the modules shown or discussed may be indirect coupling or communication coupling through some interfaces, devices, or modules, and may be electrical, mechanical, or other forms. The embodiments of this application do not impose specific limitations on this.

[0119] Furthermore, the functional modules in the embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module. The integrated modules described above can be implemented in hardware or as software functional modules.

[0120] Please see Figure 19 , Figure 19A schematic diagram of the structure of an autonomous mobile device provided in an embodiment of this application is shown. The autonomous mobile device 300 may include a camera 310, a lidar 320, a memory 330, and a processor 340. The memory 330 stores an application program configured to cause the processor to execute the methods provided in this embodiment when invoked by the processor 340. The lidar 320 is a three-dimensional lidar. It is understood that the lidar 320 may be replaced by other laser detectors, and the autonomous mobile device 300 may be the same as or different from the autonomous mobile device 100. In some embodiments, the autonomous mobile device 300 also integrates an inertial measurement unit.

[0121] The processor 340 may include one or more processing cores. The processor 340 uses various interfaces and lines to connect to various parts within the autonomous mobile device 300, and is used to run or execute instructions, programs, code sets or instruction sets stored in the memory 330, as well as to call and run or execute data stored in the memory 330, and perform various functions and process data of the autonomous mobile device 300.

[0122] The processor 340 can be implemented using at least one of the following hardware forms: Digital Signal Processing (DSP), Field-Programmable Gate Array (FPGA), and Programmable Logic Array (PLA). The processor 340 can integrate one or a combination of several of the following: Central Processing Unit (CPU), Graphics Processing Unit (GPU), and modem. The CPU primarily handles the operating system, user interface, and applications; the GPU is responsible for rendering and drawing the displayed content; and the modem handles wireless communication. It is understood that the modem can also be implemented separately as a communication chip, without being integrated into the processor 340.

[0123] The memory 330 may include random access memory (RAM) or read-only memory (ROM). The memory 330 can be used to store instructions, programs, code, code sets, or instruction sets. The memory 330 may include a program storage area and a data storage area. The program storage area may store instructions for implementing an operating system, instructions for implementing at least one function, instructions for implementing the various method embodiments described above, etc. The data storage area may store data created by the autonomous mobile device 300 during use.

[0124] This application also provides a schematic diagram of a computer-readable storage medium, on which program code is stored, and the program code is configured to execute the method provided in this application when called by a processor.

[0125] Computer-readable storage media may include, but are not limited to, flash memory, electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), hard disk, or other electronic storage devices such as ROM.

[0126] In some embodiments, the computer-readable storage medium includes a non-volatile computer-readable storage medium (Non-TCRSM). The computer-readable storage medium has storage space for program code that performs any of the method steps described above. This program code can be read from or written to one or more computer program products. The program code may be compressed in an appropriate form.

[0127] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A method for filtering and tracking dynamic objects, characterized in that, include: Construct a local map based on keyframes near the current frame's LiDAR point cloud; The current frame of the LiDAR point cloud and the local map are matched within a detection box to detect dynamic objects, and there are objects within the detection box. Filter out the point cloud of dynamic objects from the current frame lidar point cloud and the local map; The dynamic object is tracked, and template matching is no longer performed on the tracked dynamic object.

2. The method according to claim 1, characterized in that, The step of performing template matching between the current frame LiDAR point cloud and the local map within a detection box to detect dynamic objects includes: Target detection is performed on the current frame of the LiDAR point cloud to obtain the detection bounding box; Based on the LiDAR point cloud and local map point cloud within the detection box, determine whether the object within the detection box is a dynamic object.

3. The method according to claim 2, characterized in that, The step of determining whether an object within the detection frame is a dynamic object based on the lidar point cloud and local map point cloud within the detection frame includes: Using an octree to occupy the map, the LiDAR point cloud and local map point cloud within the detection box are discretized into voxels, and each voxel is of one type: dynamic voxel, static voxel, or pseudo-dynamic voxel. The occupancy rate of dynamic voxels within the detection box is determined based on the distance difference between feature points within the voxel. Based on the occupancy rate of dynamic voxels within the detection frame, determine whether the object within the detection frame is a dynamic object.

4. The method according to claim 3, characterized in that, The step of determining the occupancy rate of dynamic voxels within the detection frame based on the distance difference between feature points within a voxel includes: The average distance difference between feature points in the pseudo-dynamic voxel is determined based on the number and location of LiDAR feature points within the same voxel and the number and location of local map feature points. The occupancy rate of dynamic voxels within the detection frame is determined based on the number of dynamic voxels, the number of pseudo-dynamic voxels, and the average value.

5. The method according to claim 4, characterized in that, The step of determining the average distance difference between feature points in the pseudo-dynamic voxel based on the number and location of LiDAR feature points within the same voxel and the number and location of local map feature points includes: Obtain the weights of the dynamic voxels and the pseudo-dynamic voxels; The occupancy rate of dynamic voxels within the detection frame is determined based on the number and weight of the dynamic voxels, the number and weight of the pseudo-dynamic voxels, and the average value.

6. The method according to claim 3, characterized in that, The method of using an octree to occupy the map, discretizing the LiDAR point cloud and the local map point cloud within the detection box into voxels, includes: Using an octree to occupy the map, the LiDAR point cloud and the local map point cloud within the detection box are discretized into voxels; Based on the distribution of LiDAR feature points and local map feature points within each voxel, the type of each voxel is determined to be one of dynamic voxel, static voxel, or pseudo-dynamic voxel.

7. The method according to claim 6, characterized in that, The process of determining the type of each voxel as one of dynamic voxel, static voxel, or pseudo-dynamic voxel based on the distribution of LiDAR feature points and local map feature points within each voxel includes: If a voxel is occupied only by LiDAR feature points or only by local map feature points, the voxel type is determined to be a dynamic voxel. If a voxel is occupied by both lidar feature points and local map feature points, and the distance between lidar feature points and local map feature points is less than a first distance, the type of the voxel is determined to be a static voxel. If a voxel is occupied by both lidar feature points and local map feature points, and the distance between lidar feature points and local map feature points is greater than or equal to the second distance, the voxel type is determined to be a pseudo-dynamic voxel, and the second distance is greater than or equal to the first distance.

8. The method according to claim 3, characterized in that, The step of determining whether an object within a detection frame is a dynamic object based on the occupancy rate of dynamic voxels within the detection frame includes: If the occupancy rate of dynamic voxels within the detection box is greater than the occupancy rate threshold, the object within the detection box is determined to be a dynamic object. If the occupancy rate of dynamic voxels within the detection box is not greater than the occupancy rate threshold, the object within the detection box is determined to be a static object.

9. The method according to claim 1, characterized in that, The tracking of the dynamic object includes: The pose of a dynamic object is predicted using a pre-defined motion model, and the prediction result is obtained. Target detection is performed on the next frame of the LiDAR point cloud to obtain a new detection bounding box; The predicted results are associated with the new detection box to track dynamic objects.

10. The method according to claim 1, characterized in that, The step of performing template matching between the current frame LiDAR point cloud and the local map within the detection box includes: Extract feature points from the current frame image, including static feature points and dynamic feature points; Filter out dynamic feature points in the current frame image to obtain the visual odometry after removing dynamic feature points; The visual odometry is used as the initial value for template matching, and template matching is performed with the local map.

11. The method according to claim 10, characterized in that, Before filtering out dynamic feature points in the current frame image to obtain the visual odometry after removing dynamic feature points, the method further includes: The optical flow method is used to track the feature points of the current frame image based on the feature points of the previous frame image, and the feature points within the detection box in the current frame image are defined as target feature points. Based on the fundamental matrix, determine the epipolar lines of the current frame image; Based on the distance between each target feature point and the epipolar line, determine whether each target feature point is a dynamic feature point.

12. The method according to claim 11, characterized in that, The step of determining whether each target feature point is a dynamic feature point based on the distance between each target feature point and the epipolar line includes: If the distance between the target feature point and the epipolar line is greater than the distance threshold, the target feature point is determined to be a dynamic feature point. If the distance between the target feature point and the epipolar line is not greater than the distance threshold, the target feature point is determined to be a static feature point.

13. A dynamic object filtering and tracking device, characterized in that, include: The map building module is used to build a local map based on keyframes near the current frame's LiDAR point cloud. The object detection module is used to perform template matching between the current frame LiDAR point cloud and the local map within a detection box to detect dynamic objects, wherein there are objects within the detection box; An object filtering module is used to filter out the point cloud of dynamic objects from the current frame lidar point cloud and the local map; The object tracking module is used to track the dynamic object. For the tracked dynamic object, the template matching is no longer performed.

14. An autonomous mobile device, characterized in that, include: The device includes a camera, a lidar sensor, a memory, and a processor, wherein the memory stores an application program configured to cause the processor to perform the method as described in any one of claims 1-12 when invoked by the processor.

15. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores program code that, when invoked by a processor, causes the processor to perform the method as described in any one of claims 1-12.

Images