Multi-motion target detection and trajectory prediction method in field environment

Abstract

The application discloses a method for detecting and predicting trajectories of multiple moving targets in a field environment, which comprises the following steps: point cloud image fusion three-dimensional target detection, three-dimensional multi-target tracking, three-dimensional trajectory prediction, and dynamic SLAM algorithm for establishing a static map.The application has the beneficial effects that the future trajectory of a moving object and a static map can be provided on a self-made field environment dataset, the high accuracy of the point cloud image fusion three-dimensional target detection algorithm, the stable tracking effect of the multi-target tracking algorithm, the precision of the three-dimensional trajectory prediction, and the high preservation rate of static points and the high removal rate of dynamic points of the established static map are verified, and the method can be run in real time on experimental equipment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of trajectory prediction methods, and in particular to a multi-motion target detection and trajectory prediction method in a field environment. BACKGROUND

[0002] With the rapid development of robot technology, it has been widely used in industrial manufacturing, military operations and civilian life. In the actual field environment, the operator can control the ground mobile robot through remote control to complete the task of material transportation. However, this method requires the operator to have rich experience in controlling the ground mobile robot. If the ground mobile robot has the function of automatically following the target object, it will greatly reduce the requirement of the operator's control experience.

[0003] The Chinese invention patent application file with publication number CN115601397A discloses a ship trajectory tracking and prediction method based on a monocular camera, including the following steps: step 1: constructing a target ship image dataset, step 2: dataset annotation and training, step 3: model training result evaluation and analysis, step 4: target ship tracking experiment based on YOLOv5 and Deep Sort algorithm, step 5: target ship visual positioning and data preprocessing, step 6: building an LSTM trajectory prediction model, step 7: selecting and evaluating the time step of the prediction model, step 8: target ship trajectory prediction and verification analysis. The YOLOv5 algorithm is selected to complete the identification of the target ship through the images collected by the camera. Since there are fewer interference objects on the sea surface, the identification of the ship on the sponge through image recognition can achieve good accuracy. However, in the field environment, there are many interference objects with different surface colors and texture features, and it is difficult to meet the requirements of target recognition and trajectory prediction by relying solely on image recognition, with poor accuracy and high target recognition error rate.

[0004] Some papers have implemented target detection and trajectory prediction methods in urban environments for autonomous driving. For example, the article "Fast and furious: Real time end-to-end 3d detection, tracking and motion forecasting with a single convolutional net" published in 2018 in the "Proceedings of the IEEE conference on Computer Vision and Pattern Recognition" completes the tasks of target detection, tracking and trajectory prediction by designing a deep learning neural network, but this method is designed for autonomous driving data sets in urban environments and does not consider rugged terrain conditions, and only uses three-dimensional laser radar point clouds to complete the task, which cannot solve the problem of identifying target objects in interfering objects, causing the tracking effect of multi-target tracking algorithms to be unstable.

[0005] The Chinese invention patent application file with publication number CN114972911A discloses an output data collection and processing method for an autonomous driving perception algorithm model, an electronic device, a storage medium and a vehicle, which solves the problem of how to effectively collect a supplementary training set of a preset scene to improve the performance of the perception algorithm model. The target perception results output by the perception algorithm model are time sequenced to obtain multiple perception frame data, and a judgment is made on whether each frame of perception frame data meets the preset scene. The data collected by the vehicle-mounted sensor within the time window of the perception frame data that meets the preset scene is used as a supplementary training set for the perception algorithm model to train the perception algorithm model, which can more effectively improve the performance of the perception algorithm model. In the process of tracking target objects, moving objects will block each other and also leave a large number of dynamic points in the point cloud map. Simply using point cloud image fusion three-dimensional target detection modules to obtain information to remove dynamic points is not complete, because the target detection algorithm may miss some frames of data of moving objects, which makes it impossible to remove the corresponding dynamic points. The residual error of the feature points to the corresponding plane is too large, resulting in unreasonable estimation of the point cloud frame pose.

[0006] Therefore, the existing target tracking and prediction system in the wild environment still has the problems of poor accuracy of target recognition, unstable multi-target tracking effect and low removal rate of dynamic points. SUMMARY

[0007] In order to solve the above technical problems, the purpose of the present application is to provide a multi-motion target detection and trajectory prediction method in a field environment, which comprises the following steps: point cloud image fusion three-dimensional target detection, three-dimensional multi-target tracking, three-dimensional trajectory prediction, and dynamic SLAM algorithm to establish a static map, the multi-motion target detection and trajectory prediction method in the field environment has the advantages of good target recognition accuracy, stable multi-target tracking effect, and high dynamic point removal rate.

[0008] To achieve the above-mentioned application purposes, the technical solutions adopted by the present application are as follows:

[0009] The multi-motion target detection and trajectory prediction method in the field environment comprises the following steps:

[0010] S1, point cloud image fusion three-dimensional target detection: using a three-dimensional detector to detect point cloud data, the point cloud data is obtained by a point cloud target detection algorithm to obtain point cloud target detection data; using an image sensor to obtain image data, the image data is obtained by an image target detection algorithm to obtain image target detection data; using a Hungarian matching algorithm to realize the matching of the point cloud target detection data and the image target detection data, and outputting matching result data;

[0011] S2, three-dimensional multi-target tracking: creating a corresponding tracking instance according to the matching result data of the point cloud image fusion three-dimensional target detection, constructing a three-dimensional Kalman filter to predict the position of the moving object at the next time, using a Hungarian matching algorithm to complete the matching of the target detection result at the next time and the predicted value, and completing multi-target tracking;

[0012] S3, three-dimensional trajectory prediction: combining a deep learning trajectory prediction algorithm and a Kalman filter algorithm to infer the target motion trajectory, the deep learning trajectory prediction algorithm is based on the historical trajectory of the target in the past period of time to infer the future trajectory of the target, using the Kalman filter algorithm to calculate the error of the future trajectory as a benchmark, when the error of the future trajectory is greater than a preset first threshold value, the future trajectory is invalid, and the Kalman filter algorithm is used to complete the prediction of the three-dimensional trajectory;

[0013] S4, dynamic SLAM algorithm to establish a static map: taking the last point cloud point of the point cloud data as a benchmark, using IMU integral interpolation to obtain the three-dimensional detector pose corresponding to each point cloud point, completing point cloud de-distortion; similarly, obtaining the initial pose of the point cloud data, removing the dynamic points in the point cloud data according to the dynamic object position information obtained by the perception algorithm, and performing grid voxel downsampling, then using ikd-Tree to construct an incremental map, and completing the establishment of a static map.

[0014] This configuration addresses the issue of unstable target detection and tracking caused by numerous interfering objects in the field, effectively ensuring the system's real-time operational stability and achieving stable multi-target tracking performance. It enables the system to predict 3D trajectories under different stages and conditions, allowing for the evaluation of deep learning-based trajectory prediction algorithms to obtain the optimal predicted trajectory, thus achieving high accuracy in target motion trajectory prediction. The system uses the position information of moving objects from the previous frame to remove dynamic objects from the current 3D LiDAR point cloud frame, achieving a high dynamic point removal rate. Parallel computation of the CIA-SSD network process, YOLOv5 process, and SLAM process ensures the stability of the entire system's real-time operation.

[0015] Preferably, step S1 further includes the following step:

[0016] The 3D detector obtains point cloud target detection data through a CIA-SSD network, and the image sensor obtains image target detection data through a YOLOv5 network, calculating the homogeneous coordinates of the target object's center point in the image sensor's coordinate system. in P represents the homogeneous transformation matrix of the 3D detector coordinate system in the image sensor coordinate system. L This represents the homogeneous coordinates of the target object's center point in the 3D detector coordinate system. After obtaining the 2D bounding box of the moving object using the YOLOv5 network, the coordinates p of the center point of the 2D bounding box are calculated. img Then calculate p L With p img The cost matrix is ​​constructed by determining the Euclidean distance between the point cloud target detection data and the image target detection data, and the Hungarian matching algorithm is used to match the point cloud target detection data with the image target detection data.

[0017] This setup enables the matching of CIA-SSD point cloud target detection results with YOLOv5 image target detection results.

[0018] Preferably, step S2 further includes the following step:

[0019] The current position P of the tracked instance is predicted using the Kalman filter algorithm. pre The reasonable observation value P for this tracking instance is obtained through the Hungarian matching algorithm. L .

[0020] This configuration enables real-time updates of the tracking instances, thereby achieving the function of 3D multi-target tracking.

[0021] Preferably, step S2 further includes the following step:

[0022] Let ppre To track the projection of the instance position prediction value on the image, p img The center point of the moving object obtained by the YOLOv5 image target detection network; if p pre Matches p img There are extra p img No match, go to step S2.1; if p pre Matches p L , p img Matches p L Also matches, go to step S2.2; if there is no observation data, go to step S2.3; if p img Does not match p pre The category, discard all observation data at this time;

[0023] S2.1, create a tracking instance for the target, and the tracking instance is in an initial state;

[0024] S2.2, P L As the observation value of the corresponding tracking instance, obtain 3 frames of observation data, and the tracking instance enters a tracking state;

[0025] S2.3, use the prediction value P pre As the current position of the tracking instance, and after 5 times of continuous observation data, delete the tracking instance, and the tracking instance enters a dying state.

[0026] By such a setting: the state of the tracking instance can be used as a basis for making the next operation on the tracking instance, which is conducive to improving the detection, tracking and prediction of the system on the target.

[0027] As a preferred, in the step S3, further comprising the following steps:

[0028] The deep learning trajectory prediction algorithm uses a memory-enhanced neural network, uses a historical trajectory encoder network to extract historical trajectory features hi in the historical trajectory, uses a future trajectory encoder network to extract future trajectory features fi in the future trajectory, and splices hi and fi features., input to the decoder network to restore the future trajectory of the moving object.

[0029] By such a setting: the accuracy of the memory-enhanced neural network trajectory prediction can be ensured.

[0030] As a preferred, in the step S2, further comprising the following steps:

[0031] Use a three-dimensional Kalman filter to estimate the acceleration, speed and position of the tracking instance.

[0032] By such a setting: it is conducive to further monitoring and analyzing the state of the tracking instance.

[0033] Preferably, step S3 further includes the following step:

[0034] Using a three-dimensional Kalman filter, the acceleration, velocity, and position of the tracked instance are estimated to calculate the trajectory points over a future time period. With trajectory points Based on the baseline, the trajectory point error of several future trajectories over a period of time is calculated, and the future trajectory with the smallest error is selected as the optimal trajectory. When the error of the optimal trajectory is greater than a preset first threshold, the future trajectory is invalid.

[0035] This setting further ensures the accuracy of future trajectories, achieving the advantage of high accuracy in predicting target motion trajectories.

[0036] Preferably, step S4 further includes the following step:

[0037] The pose change between two adjacent frames of 3D LiDAR point cloud is obtained by IMU pre-integration. At the same time, the pose change can be used as a constraint to construct a factor graph. After the factor graph is optimized, the corresponding IMU zero bias value and other state variables can be obtained, and the IMU initialization is completed.

[0038] This setting ensures the accuracy of point cloud distortion removal during pose interpolation based on IMU integration.

[0039] Preferably, step S4 further includes the following step:

[0040] After constructing an incremental map using ikd-Tree, the feature points in the point cloud data are selected from the nearest map points to the incremental map constructed by ikd-Tree. It is then determined whether the map points are planar. If they are planar, the residual between the feature point and the planar plane containing the map points is calculated.

[0041] With this setup, the pose of the current 3D detector can be estimated by calculating the residual, ensuring the positioning accuracy of the entire system.

[0042] Preferably, step S4 further includes the following step:

[0043] calculate

[0044] in This represents the coordinates of the i-th feature point in the point cloud frame. express If the residual S to the nearest plane in the incremental map is greater than a preset second threshold, then the feature point is removed.

[0045] With this setting, it is possible to determine whether a feature point is noise, achieving the advantages of high dynamic point removal rate and preserving static points as much as possible.

[0046] Compared with the prior art, the present invention has achieved beneficial technical effects:

[0047] 1. Point cloud target detection data obtained by the 3D LiDAR and image target detection data obtained by the camera are fused and matched using the Hungarian matching algorithm, and the matching result data can be output. Simultaneously, parallel computation of the CIA-SSD point cloud target detection process and the YOLOv5 image target detection process is achieved, solving the problem of unstable target detection and tracking caused by numerous interference objects in the field environment. This effectively ensures the stability of the system's real-time operation, achieving high detection accuracy while improving the stability of multi-target tracking.

[0048] 2. Deep learning-based trajectory prediction algorithms require the historical trajectory of a moving object over a certain period to make inferences. If the number of historical trajectory points is insufficient, the deep learning neural network will be unable to extract effective features. However, in actual system operation, the number of historical trajectory points is often insufficient in the initial stage of tracking instance creation. In this case, the Kalman filter algorithm is used to complete 3D trajectory prediction, enabling the system to predict 3D trajectories under different stages and conditions. Furthermore, the Kalman filter algorithm has relatively small errors in predicting the trajectory of a moving object over short periods, allowing for the evaluation of the prediction results from deep learning-based trajectory prediction algorithms to obtain the optimal predicted trajectory, thus achieving the advantage of high accuracy in predicting the target motion trajectory.

[0049] 3. Simply using point cloud image fusion to obtain information from a 3D target detection module to remove dynamic points is incomplete, because the target detection algorithm may miss moving objects in a certain frame of data, thus failing to remove the corresponding dynamic points. To address this issue, this invention designs a dynamic object information collection strategy. On the one hand, it obtains the target detection results from the previous frame of data; on the other hand, it obtains the corresponding position estimates from tracking instances in the previous frame that did not have matching observations but were not deleted. Since the relative speed between the ground mobile robot and the moving object is not large during the tracking process of the ground mobile robot, the position change of the moving object between adjacent data frames is not significant. The position information of the moving object in the previous frame can be used to complete the removal of dynamic objects in the current 3D LiDAR point cloud frame, achieving the advantage of a high dynamic point removal rate. Parallel computation of the CIA-SSD network process, YOLOv5 process, and SLAM process is implemented to ensure the stability of the entire system's real-time operation.

[0050] 4. This invention, based on a self-made field environment dataset, can provide the future trajectory of moving objects and establish a static map. It verifies the high accuracy of the point cloud image fusion 3D target detection algorithm, the stable tracking effect of the multi-target tracking algorithm, the accuracy of 3D trajectory prediction, and the high retention rate of static points and the high removal rate of dynamic points in the established static map. It can run in real-time on experimental equipment. This system is of great significance for solving the problem of tracking target objects with ground mobile robots. Attached Figure Description

[0051] Figure 1 This is a schematic diagram of the system flow of the multi-moving target detection and trajectory prediction method in the field environment in this embodiment of the invention;

[0052] Figure 2 This is a schematic diagram of the matching strategy and tracking instance lifecycle management process in an embodiment of the present invention;

[0053] Figure 3 This is a schematic diagram of the tracking instance state transition process in an embodiment of the present invention;

[0054] Figure 4 This is a schematic diagram of the structure of the historical trajectory encoder network, the future trajectory encoder network, and the decoder network in an embodiment of the present invention;

[0055] Figure 5 This is a flowchart illustrating the dynamic SLAM algorithm in an embodiment of the present invention. Detailed Implementation

[0056] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the embodiments. However, the scope of protection of this invention is not limited to the specific embodiments described below.

[0057] refer to Figure 1 A method for multi-moving target detection and trajectory prediction in field environments is used to detect objects around a ground mobile robot, predict relative motion trajectories, and build static maps. The method includes the following steps:

[0058] S1. Point Cloud Image Fusion for 3D Target Detection: Point cloud data is obtained using a 3D detector. This point cloud data is then processed by a point cloud target detection algorithm to obtain point cloud target detection data. The point cloud data is the raw data obtained by the 3D detector. Image data is obtained using an image sensor. This image data is then processed by an image target detection algorithm to obtain image target detection data. A Hungarian matching algorithm is used to match the point cloud target detection data with the image target detection data, and the matching result is output. The 3D detector is a 3D LiDAR, and the image sensor is a camera.

[0059] The 3D detector acquires point cloud target detection data through the CIA-SSD network, and the image sensor uses YOLO... v 5. The network obtains image target detection data, and calculates the coordinates of the center point of the moving object in the image sensor coordinate system using formula ①:

[0060]

[0061] Calculate the homogeneous coordinates of the target object in the image coordinate system using formula ②:

[0062]

[0063] Let p L = (u, v) represents the projection of the center point of the moving object onto the image obtained from the CIA-SSD network, where (x C y C , z C , 1) represents the homogeneous coordinates of the center point of the target object in the image sensor coordinate system. P represents the homogeneous transformation matrix of the 3D detector coordinate system in the image sensor coordinate system. L Let (u, v, 1) represent the homogeneous coordinates of the target object's center point in the 3D detector coordinate system, and (u, v, 1) represent the homogeneous coordinates of the target object's center point in the image coordinate system. K represents the intrinsic parameter matrix of the monocular camera. After obtaining the 2D bounding box of the moving object using the YOLOv5 network, the coordinates p of the center point of the 2D bounding box are calculated. img Then calculate p L With p img The cost matrix was constructed by equating the Euclidean distance between the points and images. The Hungarian matching algorithm was used to match the point cloud object detection data with the image object detection data, thus achieving the matching of CIA-SSD point cloud object detection results with YOLOv5 image object detection results.

[0064] S2, 3D multi-target tracking: Based on the matching results of 3D target detection fused with point cloud images, corresponding tracking instances are created. A 3D Kalman filter is constructed to predict the position of the moving object at the next moment. The Hungarian matching algorithm is used to match the target detection result with the predicted value at the next moment, thus completing multi-target tracking.

[0065] Since the moving object is moving on rugged terrain, it is necessary to model the moving object using three-dimensional kinematics. Taking the position, velocity, and acceleration of the tracked instance as state variables, the following state equation ③ is obtained based on the kinematic relationships:

[0066]

[0067] Among them (P) t v t at ) represents the position, velocity, and acceleration of the tracked instance, Δt represents the time interval, and Q represents the process noise, which follows a Gaussian distribution;

[0068] Since only the position of the target object can be obtained in point cloud image fusion for 3D target detection, the observation equation ④ is established as follows:

[0069]

[0070] Among them, P w This represents the position of the tracking instance in the map coordinate system, and R represents the observation noise, which follows a Gaussian distribution.

[0071] Since moving objects within the sensor's detection range often exhibit relatively consistent speeds during target object tracking, this invention uses the average speed v of the moving object... avg As the initial velocity of the newly discovered tracking instance, and then after the tracking instance has obtained 3 observations and accumulated 3 frames of historical trajectory points, a new velocity estimate v is obtained through the least squares algorithm. ls Calculate v ls Velocity estimates compared to Kalman filtering algorithm The difference Δv. If the magnitude of Δv is greater than the preset third threshold, then v is used. ls Update the tracking instance state and use it for subsequent Kalman filter algorithm iterations; implement the estimation of the tracking instance's acceleration, velocity, and position using a 3D Kalman filter;

[0072] The current position P of the tracked instance is predicted using the Kalman filter algorithm. pre The reasonable observation value P for this tracking instance is obtained through the Hungarian matching algorithm. L This enables real-time updates of the tracking instances, thereby achieving the function of three-dimensional multi-target tracking;

[0073] Matching strategies and algorithmic frameworks for tracking instance lifecycle management, such as Figure 2 As shown, where p pre To track the projection of the instance location prediction onto the image, p img Let p be the center point of the moving object obtained by the YOLOv5 image object detection network; pre With p img There are extra p img If no match is found, then condition 1 is met and proceed to step s2.1; if pre With p L p img With p L If it also matches, then condition 2 is met and proceed to step S2.2; if there is no observation data, then condition 3 is met and proceed to step S2.3; if p imgWith p pre If the category does not match, all observation data at this time will be discarded; the tracking instance lifecycle management strategy is as follows, and the state transitions of the tracking instance are as follows: Figure 3 As shown, the states of a tracking instance mainly include the initial state, the tracking state, and the extinction state;

[0074] S2.1 Create a tracking instance for the target, which is in an initial state;

[0075] s2.2, P L As the observation value of the corresponding tracking instance, the tracking instance enters the tracking state after obtaining 3 frames of observation data;

[0076] S2.3, Using the predicted value P pre As the current position of the tracking instance, if no observation data is obtained for 5 consecutive times, the tracking instance is deleted and enters the extinction state.

[0077] S3, 3D trajectory prediction: Combine deep learning trajectory prediction algorithm with Kalman filter algorithm to infer the target's motion trajectory. The deep learning trajectory prediction algorithm infers the target's future trajectory based on the target's historical trajectory over a period of time. The Kalman filter algorithm is used as a benchmark to calculate the error of the future trajectory. When the error of the future trajectory is greater than a preset first threshold, the future trajectory is invalid. The Kalman filter algorithm is then used to complete the prediction of the 3D trajectory.

[0078] The deep learning trajectory prediction algorithm employs a memory-enhanced neural network. It uses a historical trajectory encoder network to extract historical trajectory features *hi* and a future trajectory encoder network to extract future trajectory features *fi*. The *hi* and *fi* features are concatenated and input into a decoder network to reconstruct the future trajectory of the moving object. The network structure diagrams of the encoder and decoder networks are shown below. Figure 4 As shown, the encoder network is divided into historical trajectory encoder network and future trajectory encoder network. They have the same network structure, but the data they learn are different.

[0079] The controller of the memory-enhanced neural network is composed of a fully connected neural network, and its training process uses an encoder network and a decoder network. First, a portion of the training data is selected and processed by the historical trajectory encoder network and the future trajectory encoder network to obtain historical trajectory feature memories and future trajectory feature memories. Then, formal training begins. The historical trajectory feature encoder obtains historical trajectory features h. Cosine distance is used to find the five historical trajectory feature vectors most similar to h in the historical trajectory feature memory, and further, the corresponding five future trajectory feature vectors are found. h is concatenated with these five future trajectory feature vectors, and the decoder network obtains five future trajectories. The error rate of the predicted trajectory is calculated using the following formula ⑤:

[0080]

[0081] in It is an indicator function, when the predicted 3D trajectory points With truth value P F Error within a certain threshold Th t It is 1 within the specified range, and 0 otherwise. The threshold Th... t As the prediction time increases, Th is set. 1s =0.5m,Th 2s =1.0mn,Th 3s =1.5m,Th 4s =2.0m.

[0082] Select the maximum error rate E among the 5 predicted trajectories. r ate_max Loss function L used to construct a memory-enhancing neural network controller c As shown in equation ⑥ below:

[0083] L c =E rate_max (1-P(w))+(1-E rate_max )P(w) ⑥

[0084] Where P(w) represents the probability of the controller output being written into memory. When P(w) > 0.5, the corresponding historical trajectory features and future trajectory features are written into memory as a memory. While training the controller, the historical trajectory feature memory and future trajectory feature memory are continuously increased for network inference. During the training of the memory-enhanced neural network, noise is randomly added to the historical trajectory points as a data augmentation method to improve the network's generalization ability and avoid overfitting.

[0085] During the inference process, five corresponding future trajectory features are selected for memorization. Then, the decoder outputs five future trajectories. These five trajectories are then evaluated, and the optimal future trajectory is selected as the final result. This invention utilizes a three-dimensional Kalman filter to estimate the acceleration, velocity, and position of the tracked instance, calculating the trajectory point for the next second. With trajectory points Based on the baseline, the trajectory point error of 1 second is calculated among several future trajectories. The future trajectory with the smallest error is selected as the optimal trajectory. When the error of the optimal trajectory is greater than the preset first threshold, the future trajectory is invalid, and the Kalman filter algorithm is used to complete the 3D trajectory prediction.

[0086] S4. Dynamic SLAM Algorithm for Static Map Building: Using the last point in the point cloud data as a reference, IMU integral interpolation is used to obtain the 3D detector pose corresponding to each point, completing point cloud distortion removal. Similarly, the initial pose of the point cloud data is obtained, and dynamic points in the point cloud data are removed based on the dynamic object position information obtained by the perception algorithm. Raster voxel downsampling is then performed, and an incremental map is constructed using ikd-Tree, completing the static map building. The SLAM algorithm flowchart is as follows: Figure 5 As shown, IMU pre-integration is used to obtain the pose change between two adjacent frames of 3D LiDAR point clouds. This pose change can also serve as a constraint to construct a factor map. After optimizing the factor map, the corresponding IMU zero bias and other state variables can be obtained, completing the IMU initialization. The points in the 3D LiDAR point cloud are not all scanned at the same time. During the 3D LiDAR scanning of the surrounding environment, due to the movement of the ground-based mobile robot, the 3D LiDAR coordinate system corresponding to the point cloud points scanned at different times may not be in the same pose, leading to distortion of the 3D LiDAR point cloud. This invention uses a pose interpolation method based on IMU integration to remove motion distortion from the 3D LiDAR point cloud. Taking the last point cloud point of the 3D LiDAR point cloud frame as a reference, IMU integration interpolation is used to obtain the 3D LiDAR pose corresponding to each point cloud point, thus completing the point cloud distortion removal.

[0087] After distortion correction of the 3D LiDAR point cloud, the initial pose of the 3D LiDAR point cloud frame is obtained using IMU integral pose interpolation. Based on the dynamic object position information obtained by the perception algorithm, dynamic points in the 3D LiDAR point cloud are removed, and raster voxel downsampling is performed. This completes the 3D LiDAR point cloud frame processing. After constructing an incremental map using ikd-Tree, the five nearest map points between the feature point in the point cloud data and the incremental map constructed by ikd-Tree are selected. It is then determined whether these five map points form a plane; if so, the residual between the feature point and the plane containing the five map points is calculated.

[0088] If dynamic points are not removed, the residuals from their feature points to the corresponding plane are usually quite large. Using these feature points to estimate the point cloud frame pose is unreasonable. Therefore, the corresponding values ​​are calculated using the following formula 7.

[0089]

[0090] in This represents the coordinates of the i-th feature point in the point cloud frame. express If the residual from the nearest plane to the incremental map is greater than the preset second threshold, then the residual value of the feature point to the nearest plane is considered unreasonable, and the feature point is considered to be a dynamic point or a noise point. The feature point is then removed to achieve the removal of dynamic points.

[0091] This embodiment has the following advantages:

[0092] The point cloud target detection data obtained by the 3D LiDAR and the image target detection data obtained by the camera are fused and matched using the Hungarian matching algorithm, and the matching result data can be output. This enables parallel computing of the CIA-SSD point cloud target detection process and the YOLOv5 image target detection process, solving the problem of unstable target detection and tracking caused by many interference objects in the field environment. It can effectively ensure the stability of the system's real-time operation, and improve the stability of multi-target tracking while achieving high detection accuracy.

[0093] Deep learning-based trajectory prediction algorithms require the historical trajectory of a moving object over a certain period to make inferences. If the number of historical trajectory points is insufficient, the deep learning neural network will be unable to extract effective features. However, in actual system operation, the number of historical trajectory points is often insufficient in the initial stage of tracking instance creation. In this case, the Kalman filter algorithm is used to perform 3D trajectory prediction, enabling the system to predict 3D trajectories at different stages and under different conditions. Furthermore, the Kalman filter algorithm has relatively small errors in predicting the trajectory of a moving object over short periods, allowing for the evaluation of the prediction results from deep learning-based trajectory prediction algorithms to obtain the optimal predicted trajectory, thus achieving the advantage of high accuracy in predicting the target motion trajectory.

[0094] Simply using point cloud image fusion to obtain information from a 3D target detection module to remove dynamic points is incomplete, because the target detection algorithm may miss moving objects in a certain frame of data, thus failing to remove the corresponding dynamic points. To address this issue, this invention designs a dynamic object information collection strategy. On the one hand, it obtains the target detection results from the previous frame of data; on the other hand, it obtains the corresponding position estimates from tracking instances in the previous frame that did not have matching observations but were not deleted. Since the relative speed between the ground mobile robot and the moving object is not large during the tracking process of the ground mobile robot, the position change of the moving object between adjacent data frames is small. The position information of the moving object in the previous frame can be used to complete the removal of dynamic objects in the current 3D LiDAR point cloud frame, achieving the advantage of a high dynamic point removal rate. Parallel computation of the CIA-SSD network process, YOLOv5 process, and SLAM process is implemented to ensure the stability of the entire system's real-time operation.

[0095] This invention, using a self-made field environment dataset, can provide the future trajectory of moving objects and create a static map. It verifies the high accuracy of the point cloud image fusion 3D target detection algorithm, the stable tracking effect of the multi-target tracking algorithm, the accuracy of 3D trajectory prediction, and the high retention rate of static points and the high removal rate of dynamic points in the created static map. It can run in real-time on experimental equipment (AMD Ryzen 93950x CPU, NVIDIA GeForce RTX 3080 GPU). This system is of great significance for solving the problem of tracking target objects with ground mobile robots.

[0096] By obtaining the corresponding matching value through the Hungarian matching algorithm, and further judging the status of the tracking instance through the tracking instance lifecycle management strategy, the status of the tracking instance can be used as the basis for judging the next operation of the tracking instance, which is conducive to improving the system's detection, tracking and prediction of the target.

[0097] By training a historical trajectory encoder network that extracts historical trajectory features from historical trajectories, a future trajectory encoder network that extracts future trajectory features from future trajectories, and a future trajectory encoder network that decodes and restores future trajectory features to future trajectories, the accuracy of trajectory prediction by the memory-enhanced neural network can be guaranteed.

[0098] By initializing the IMU, the accuracy of point cloud distortion removal can be guaranteed through pose interpolation based on IMU integration.

[0099] Using a three-dimensional Kalman filter to estimate the acceleration, velocity, and position of the tracking instance can provide further estimation data for the tracking instance, which is beneficial for further monitoring and analysis of the tracking instance's state.

[0100] By evaluating the future trajectory using a three-dimensional Kalman filter, the accuracy of the future trajectory can be further guaranteed, achieving the advantage of high accuracy in predicting the target motion trajectory.

[0101] By calculating the residuals, it is possible to determine whether the residual values ​​from feature points in point cloud data to the nearest plane of the incremental map are reasonable. Based on this, it is possible to determine whether the feature point is a dynamic noise point and remove it, thereby improving the removal rate of dynamic points and ensuring the stability of the entire system's real-time operation. It can also complete the pose estimation of the current 3D detector, ensuring the positioning accuracy of the entire system.

[0102] When dynamic points are not removed, the residuals from their feature points to the corresponding plane are usually quite large. Using these feature points to estimate the pose of the point cloud frame is unreasonable. Therefore, the corresponding value is calculated using the following formula 7. If this value is greater than the second threshold, the residual value from the feature point to the nearest plane is considered unreasonable, and the feature point is either a dynamic point or noise, which needs to be removed. Directly removing all point cloud points near the location of dynamic objects will obviously remove some ground points, resulting in holes and other issues in the ground area. This invention solves this problem by using an incremental map constructed with an ikd-Tree, using the next 3D LiDAR point cloud keyframe to fill in these holes, thereby achieving the goal of preserving as many static points as possible. This achieves the advantages of a high dynamic point removal rate and the ability to preserve as many static points as possible.

[0103] Based on the disclosure and teachings of the foregoing specification, those skilled in the art can make changes and modifications to the above embodiments. Therefore, the present invention is not limited to the specific embodiments disclosed and described above, and some modifications and changes to the invention should also fall within the protection scope of the claims of the present invention. Furthermore, although some specific terms are used in this specification, these terms are only for convenience of explanation and do not constitute any limitation on the invention.

Claims

1. A method for detecting and predicting the trajectory of multiple moving targets in a field environment, characterized in that, Includes the following steps: S1. Point cloud image fusion for 3D target detection: Point cloud data is obtained by using a 3D detector, and the point cloud data is processed by a point cloud target detection algorithm to obtain point cloud target detection data; image data is obtained by using an image sensor, and the image data is processed by an image target detection algorithm to obtain image target detection data; the Hungarian matching algorithm is used to match the point cloud target detection data with the image target detection data, and the matching result data is output. S2, 3D multi-target tracking: Based on the matching results of 3D target detection fused with point cloud images, corresponding tracking instances are created. A 3D Kalman filter is constructed to predict the position of the moving object at the next moment. The Hungarian matching algorithm is used to match the target detection result with the predicted value at the next moment, thus completing multi-target tracking. S3. Three-dimensional trajectory prediction: Combine deep learning trajectory prediction algorithm with Kalman filter algorithm to infer the target's motion trajectory. The deep learning trajectory prediction algorithm infers the target's future trajectory based on the target's historical trajectory over a period of time. The Kalman filter algorithm is used as a benchmark to calculate the error of the future trajectory. When the error of the future trajectory is greater than a preset first threshold, the future trajectory is invalid. The Kalman filter algorithm is then used to complete the prediction of the three-dimensional trajectory. S4. Dynamic SLAM algorithm to build static map: Using the last point in the point cloud data as the reference, IMU integral interpolation is used to obtain the 3D detector pose corresponding to each point in the point cloud, thus completing the point cloud distortion removal; similarly, the initial pose of the point cloud data is obtained, and dynamic points in the point cloud data are removed according to the dynamic object position information obtained by the perception algorithm, and grid voxel downsampling is performed. Then, ikd-Tree is used to build an incremental map to complete the establishment of the static map. Step S2 further includes the following steps: The current position of the tracked instance is predicted using the Kalman filter algorithm. Reasonable observations for this tracking instance are obtained using the Hungarian matching algorithm. ; set up To track the projection of the instance location prediction values ​​onto the image, The center point of the moving object obtained by the YOLOv5 image object detection network; if and There are excess If no match is found, proceed to step S2.1; if and , and If a match is found, proceed to step S2.2; if no observation data is found, proceed to step S2.3; if... and If the categories do not match, all observation data at this time will be discarded; S2.1 Create a tracking instance for the target, which is in an initial state; S2.2, As the observation value of the corresponding tracking instance, the tracking instance enters the tracking state after obtaining 3 frames of observation data; S2.3 Using predicted values As the current position of the tracking instance, if no observation data is obtained for 5 consecutive times, the tracking instance is deleted and enters the extinction state.

2. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 1, characterized in that, Step S1 further includes the following steps: The 3D detector obtains point cloud target detection data through a CIA-SSD network, and the image sensor obtains image target detection data through a YOLOv5 network, calculating the homogeneous coordinates of the target object's center point in the image sensor's coordinate system. ,in This represents the homogeneous transformation matrix of the 3D detector coordinate system in the image sensor coordinate system. This represents the homogeneous coordinates of the target object's center point in the 3D detector coordinate system. After obtaining the 2D bounding box of the moving object using the YOLOv5 network, the coordinates of the center point of the 2D bounding box are calculated. Then obtain and The cost matrix is ​​constructed by determining the Euclidean distance between the point cloud target detection data and the image target detection data, and the Hungarian matching algorithm is used to match the point cloud target detection data with the image target detection data.

3. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 1, characterized in that, Step S3 further includes the following steps: The deep learning trajectory prediction algorithm employs a memory-enhanced neural network and uses a historical trajectory encoder network to extract historical trajectory features from historical trajectories. Future trajectory features are extracted from future trajectories using a future trajectory encoder network. ,Will and The features are concatenated and input into the decoder network to recover the future trajectory of the moving object.

4. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 3, characterized in that, Step S2 further includes the following steps: The acceleration, velocity, and position of the tracked instance are estimated using a three-dimensional Kalman filter.

5. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 4, characterized in that, Step S3 further includes the following steps: Using a three-dimensional Kalman filter, the acceleration, velocity, and position of the tracked instance are estimated to calculate the trajectory points over a future time period. , with trajectory points Based on the baseline, the trajectory point error of a certain period of time in the future trajectory predicted by several memory-enhanced neural networks is calculated, and the future trajectory with the smallest error is selected as the optimal trajectory. When the error of the optimal trajectory is greater than a preset first threshold, the future trajectory is invalid.

6. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 1, characterized in that, Step S4 also includes the following steps: The pose change between two adjacent frames of 3D LiDAR point cloud is obtained by IMU pre-integration. At the same time, the pose change can be used as a constraint to construct a factor graph. After the factor graph is optimized, the corresponding IMU zero bias state variable can be obtained, and the IMU initialization is completed.

7. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 1, characterized in that, Step S4 also includes the following steps: After constructing an incremental map using ikd-Tree, the feature points in the point cloud data are selected from the nearest map points to the incremental map constructed by ikd-Tree. It is then determined whether the map points are planar. If they are planar, the residual between the feature point and the planar plane containing the map points is calculated.

8. The method for detecting and predicting the trajectory of multiple moving targets in a field environment according to claim 7, characterized in that, Step S4 also includes the following steps: calculate , in This represents the coordinates of the i-th feature point in the point cloud frame. express If the residual S to the nearest plane in the incremental map is greater than a preset second threshold, then the feature point is removed.

Images