Low-altitude dynamic target tracking and trajectory prediction device and method

Abstract

The invention relates to a low-altitude dynamic target tracking and trajectory prediction device and method, and the method comprises the following steps: a depth camera collects an RGB image and a depth image of a dynamic target, a visual inertial odometer collects pose information, and transmits the pose information to an upper computer; based on a lightweight target detection algorithm, the upper computer detects the RGB image target to obtain a target position feature and a target shape feature; estimating and predicting a target state based on a Kalman filtering algorithm to obtain target state information and target prediction information; and based on a Hungary algorithm, correlating and matching the detection state information of each target at the current moment with the target prediction state information at the previous moment, updating the target position features and the target shape features, and realizing target state tracking. According to the invention, dynamic target tracking in the environment is realized, the three-dimensional position, speed and acceleration of the target are acquired in real time, and based on the target tracking state, dynamic target space trajectory prediction position prediction based on the target motion prediction equation is carried out.

Description

Technical Field

[0001] The present invention belongs to the field of target tracking, and particularly relates to a device and method for tracking and predicting the trajectory of low-altitude dynamic targets. Background Art

[0002] In recent years, intelligent robots have been widely used in fields such as the Internet of Things (IoT), intelligent transportation, and exploration of unknown environments. During the task execution process, they may be dynamically interfered by flying birds, trees, or other low-altitude interfering objects. A system with the function of tracking and predicting the trajectory of low-altitude dynamic targets can be applied to a wide range of scenarios, such as tracking pedestrians and vehicles in the field of traffic inspection to obtain the target state for monitoring the real-time traffic flow. However, the existing systems with the function of tracking and predicting the trajectory of low-altitude dynamic targets still face many challenges. On the one hand, when the position features and shape features of dynamic targets are prone to change, the target tracking accuracy is easily affected; on the other hand, the problem of poor target tracking and trajectory prediction effects when the environment is changeable. Therefore, it is an urgent task to invent a target tracking and trajectory prediction method that can associate target position and shape features and adapt to environmental requirements. Summary of the Invention

[0003] The present invention provides a method, device, and storage medium for tracking and predicting the trajectory of low-altitude dynamic targets, aiming to solve at least one of the technical problems existing in the prior art.

[0004] The technical solution of the present invention relates to a method for tracking and predicting the trajectory of low-altitude dynamic targets, which is applied to a device for tracking and predicting the trajectory of low-altitude dynamic targets. The device for tracking and predicting the trajectory of low-altitude dynamic targets at least includes a host computer, a depth camera, and a visual inertial odometer. The depth camera and the visual inertial odometer are electrically connected to the host computer respectively. It is characterized in that the method for tracking and predicting the trajectory of low-altitude dynamic targets includes the following steps:

[0005] S100. The depth camera collects the RGB image and depth image of the dynamic target, and the visual inertial odometer collects the pose information, and transmits the RGB image, depth image, and pose information to the host computer;

[0006] S200. Based on the lightweight target detection algorithm Yolo-Fastestv2, the host computer detects the target in the RGB image to obtain the target detection result R t , and the target detection result R t includes target position information and target shape information;

[0007] S300. Based on the Kalman filter algorithm, predict the state of the target at the next moment to obtain the target prediction information, compare the predicted value with the target state information, and correct the target prediction information;

[0008] S400. Based on the Hungarian algorithm, the detection status information of each target at the current moment is associated and matched with the target prediction status information at the previous moment, and the target position feature and the target shape feature are updated to achieve target status tracking.

[0009] Further, the step S200 includes:

[0010] S210. Determine the region of interest (ROI) and the target detection box based on the RGB image, and the region of interest (ROI) is included in the target detection box;

[0011] S220. Based on the target detection box, obtain the target pixel coordinates (u c , v c ) and the target pixel scale (S p , R p ), where u c represents the center point coordinate of the horizontal pixels in the target pixels of the target detection box, v c represents the center point coordinate of the vertical pixels of the target pixels in the target detection box, S p represents the area of the target detection box, and R p represents the aspect ratio of the target detection box;

[0012] S230. Map the region of interest (ROI) to the depth image, obtain the target depth information d, and convert the target pixel coordinates (u c , v c ) into the target world coordinates (X w , Y w , Z w );

[0013] S240. Calculate the target detection result Rt.

[0014] Further, in the step S230, the world coordinates (X w , Y w , Z w ) of the target satisfy:

[0015] M I * M E * [X w , Y w , Z w , 1] T = d * [u, v, 1] T

[0016] where M I is the camera internal parameter matrix, ME is the external camera parameter matrix, d is the target depth information, u represents the target horizontal pixel value, and v represents the target vertical pixel value;

[0017] In the step S240, at time t, the target detection result is:

[0018] R t = {(p i , c i ) | i □ (1, N)}

[0019] where i represents the index of the perceived dynamic target, is the pixel coordinate of the i-th detected dynamic target.

[0020] Furthermore, in the step S200, the target position feature is the intersection over union f iou (i, j) of the target detection box and the target prediction box, and the target shape feature is obtained by calculating the distance between the upper left coordinates and the distance between the lower right coordinates of the target detection box and the target prediction box, as well as the similarity of the width-to-height ratio of each target detection box and the target prediction box. Among them,

[0021] The Euclidean distance d 1 between the upper left coordinates of the target detection box and the target prediction box and the Euclidean distance d 2 between the lower right coordinates are expressed as:

[0022]

[0023] where, is the upper left coordinate of the i-th target detection box, is the lower right coordinate of the i-th target detection box, is the upper left coordinate of the j-th target prediction box, is the lower right coordinate of the j-th target prediction box,

[0024] The Euclidean distance similarity S 1 between the Euclidean distance d 2 between the upper left coordinates and the Euclidean distance d 1 between the lower right coordinates is:

[0025]

[0026] The width-to-height ratio similarity S 2 between the target detection box and the target prediction box is:

[0027]

[0028] where r 1 refers to the width-to-height ratio of the target detection box, and r 2 refers to the width-to-height ratio of the target prediction box;

[0029] Intersection over Union (IoU) of the target detection box and the target prediction box, f iou (i, j) is:

[0030]

[0031] where A i and A j are the areas of the target prediction box and the target detection box respectively, and Ω ij represents the area of the overlapping part of the target prediction box and the target detection box;

[0032] Therefore, for the i-th target prediction box and the j-th target detection box, the value coefficient matrix combining the target position feature and the target shape feature is expressed as:

[0033] ζ ij = a * f iou (i, j) + b * S 1 + c * S 2 , (a + b + c = 1)

[0034] where a, b, and c are all weight parameters with different weight values set according to different scenarios. The higher the weight value, the greater the influence of the corresponding feature on target tracking and matching.

[0035] Furthermore, in the step S300, after obtaining the real-time state information of the target, combined with the target motion prediction equation, the target trajectory is predicted.

[0036] Among them, the target motion is approximated by a uniformly accelerated model, and the scale change is approximated by a first-order differential model;

[0037] Let the initial time be t 0 , the initial position of the dynamic target is the initial velocity is the initial acceleration is Therefore, the position of target i at time t is:

[0038]

[0039] Furthermore, in the step S300,

[0040] For different environments, the requirements for trajectory prediction are different. By modeling the relevant parameters of trajectory prediction and dynamically adjusting the parameters to adapt to the trajectory prediction of different environments, the modeling parameters at least include the target tracking frequency and the trajectory prediction time step;

[0041] Among them, the dynamic target tracking frequency is the frame interval of image processing, that is, the reciprocal of the Kalman filter state transition time;

[0042] The trajectory prediction time step is the time interval calculated each time when predicting the target motion trajectory after obtaining the target position, speed, and acceleration each time.

[0043] Adjust the trajectory prediction time step Δt according to different scenarios, and use the linear interpolation method to adjust the modeling formula of the trajectory prediction time step Δt.

[0044]

[0045] where d max is the maximum target tracking distance, d min is the minimum target tracking distance, t max is the maximum trajectory prediction time step, t min is the maximum trajectory prediction time step, d is the Euclidean distance between the dynamic target and the mobile robot. represents the degree of weather influence, and α represents the adjustment coefficient.

[0046] Furthermore, in the step S300, the target state information includes the target state X k and the observation state Z k , and the target state is represented by the state transition equation, and the observation state is represented by the observation equation as:

[0047]

[0048] where the state transition matrix of the uniformly accelerated model is:

[0049]

[0050] The observation matrix of the uniformly accelerated model is:

[0051]

[0052] where k represents the k-th frame time, dt represents the Kalman filter state transition time interval, W k-1 and V k respectively represent the normal distribution process noise and the normal distribution measurement noise, assumed to be zero-mean noise, X k-1 represents the target state at the previous moment, X k represents the current target state, I 3×3 represents the 3×3 identity matrix, and O 3×3 represents the 3×3 zero matrix;

[0053] By the correlation matching of the target position feature and the target shape feature, the target scale model state transition equation is obtained, and the target scale model state transition equation is:

[0054]

[0055] Among them, represents the scale state of the target at time k, represents the scale state of the target at the previous time, represents the state transition matrix of the target scale, is the process noise of the normal distribution,

[0056] Among them, the state transition matrix of the target scale is:

[0057]

[0058] Observation matrix is:

[0059]

[0060] Furthermore, the step S400 includes:

[0061] S410. Based on the Hungarian algorithm, calculate the intersection over union and shape feature similarity between all target detection boxes and the positions of the target prediction boxes in the previous frame;

[0062] S420. Set different weights to obtain the Hungarian matrix, and use the Hungarian algorithm to maximize the target matching value;

[0063] S430. Set the minimum threshold ζ min Filter the matching results to obtain the best match that meets the requirements, and targets that are not successfully matched will no longer be subject to Kalman filtering.

[0064] Furthermore, the present invention also proposes a low-altitude dynamic target tracking and trajectory prediction device for implementing the low-altitude dynamic target tracking and trajectory prediction method. The low-altitude dynamic target tracking and trajectory prediction device includes:

[0065] Host computer;

[0066] Depth camera, the depth camera is electrically connected to the host computer;

[0067] Visual inertial odometer, the visual inertial odometer is electrically connected to the host computer.

[0068] Furthermore, the present invention also proposes a computer-readable storage medium, on which program instructions are stored, and when the program instructions are executed by a processor, the low-altitude dynamic target tracking and trajectory prediction method is implemented.

[0069] Compared with the existing technology, the present invention has the following characteristics.

[0070] The present invention provides a device and method for tracking and trajectory prediction of low-altitude dynamic targets. The method integrates hardware such as RGBD cameras and computing platforms on small unmanned aerial vehicles or other mobile robots. By deploying target tracking and trajectory prediction methods, dynamic target tracking in the environment is achieved, and the three-dimensional position, speed, and acceleration of the target are obtained in real time. Based on the target tracking state, position prediction of the dynamic target space trajectory is performed based on the target motion prediction equation. The method is integrated into a system that supports tracking and trajectory prediction of dynamic obstacles such as flying birds and pedestrians, providing guarantee for the safe operation of mobile robots in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 It is a flowchart of the method for tracking and trajectory prediction of low-altitude dynamic targets.

[0072] Figure 2 It is a flowchart of detecting RGB image targets and obtaining target position features and target shape features based on a lightweight target detection algorithm in the method for tracking and trajectory prediction of low-altitude dynamic targets.

[0073] Figure 3 It is a schematic block diagram of the process in the method for tracking and trajectory prediction of low-altitude dynamic targets.

[0074] Figure 4 It is a schematic diagram of the association between target position features and shape features in the method for tracking and trajectory prediction of low-altitude dynamic targets.

[0075] Figure 5 It is a comparison diagram of the estimated value of the target three-dimensional position and the detected value by the motion capture system in the method for tracking and trajectory prediction of low-altitude dynamic targets.

[0076] Figure 6 It is a comparison diagram of the predicted trajectory value of the three-dimensional trajectory of the small ball target and the detected value by the motion capture system in the method for tracking and trajectory prediction of low-altitude dynamic targets. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0077] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0078] The concept, specific structure, and technical effects of the present invention will be clearly and completely described below in combination with the embodiments and the accompanying drawings to fully understand the objectives, solutions, and effects of the present invention.

[0079] It should be noted that, unless otherwise specified, when a feature is referred to as "fixed" or "connected" to another feature, it can be directly fixed or connected to the other feature, or indirectly fixed or connected to the other feature. The singular forms of "a", "the", and "said" used in this article are also intended to include the plural forms, unless the context clearly indicates otherwise. In addition, unless otherwise defined, all technical and scientific terms used in this article have the same meaning as commonly understood by those skilled in the technical field of this technology. The terms used in the description of this article are only for describing specific embodiments and are not intended to limit the present invention. The term "and / or" used in this article includes any combination of one or more of the related listed items.

[0080] It should be understood that although the terms first, second, third, etc. may be used in this disclosure to describe various elements, these elements should not be limited to these terms. These terms are only used to distinguish elements of the same type from each other. For example, without departing from the scope of this disclosure, the first element may also be referred to as the second element, and similarly, the second element may also be referred to as the first element. The use of any and all examples or exemplary language ("for example", "such as", etc.) provided in this article is only intended to better illustrate the embodiments of the present invention and will not impose a limitation on the scope of the present invention unless otherwise required. In addition, the industry term "pose" used in this article refers to the position and orientation of a certain element relative to a spatial coordinate system.

[0081] Referring to Figures 1 to 6 , an embodiment of the present invention provides a low-altitude dynamic target tracking and trajectory prediction method, which is applied to a low-altitude dynamic target tracking and trajectory prediction device. The low-altitude dynamic target tracking and trajectory prediction device at least includes a host computer, a depth camera, and a visual inertial odometer. The depth camera and the visual inertial odometer are respectively electrically connected to the host computer. Referring to Figure 1 and Figure 3 , the low-altitude dynamic target tracking and trajectory prediction method includes the following steps:

[0082] S100. The depth camera collects the RGB image and depth image of the dynamic target, and the visual inertial odometer collects pose information, and transmits the RGB image, depth image, and pose information to the host computer;

[0083] S200. Based on the lightweight target detection algorithm Yolo-Fastestv2, the host computer detects the target in the RGB image and obtains the target detection result R t , the target detection result R t includes target position information and target shape information;

[0084] S300. Based on the Kalman filtering algorithm, predict the state of the target at the next moment to obtain target prediction information, compare the predicted value with the target state information, and correct the target prediction information.

[0085] S400. Based on the Hungarian algorithm, associate and match the detection state information of each target at the current moment with the target prediction state information at the previous moment, update the target position feature and the target shape feature, and achieve target state tracking.

[0086] Compared with the existing technologies, the present invention has the following characteristics.

[0087] The present invention provides a low-altitude dynamic target tracking and trajectory prediction device and method. This method integrates hardware such as RGBD cameras and computing platforms on small unmanned aerial vehicles or other mobile robots. By deploying target tracking and trajectory prediction methods, it realizes the tracking of dynamic targets in the environment and obtains the three-dimensional position, speed, and acceleration of the target in real time. And based on the target tracking state, it conducts dynamic target space trajectory prediction position prediction based on the target motion prediction equation. Integrate the method into a system, and this system supports the tracking and trajectory prediction of dynamic obstacles such as flying birds and pedestrians, providing guarantee for the safe operation of mobile robots in complex environments.

[0088] Specifically, the present invention discloses a low-altitude dynamic target tracking and trajectory prediction method. This method starts from target detection, uses a mobile robot equipped with an RGBD camera to represent the position and shape of potential targets, and uses the target position and shape representation results for state prediction by the Kalman filter. Secondly, the Hungarian algorithm is used to associate the position and shape features of the target to obtain the real-time three-dimensional position of the target and achieve target tracking. At the same time, parametric modeling is carried out on trajectory prediction-related parameters such as target tracking frequency and trajectory prediction time step, and the parameters are adjusted according to different environments. And based on the real-time position, speed, and acceleration information of target tracking, effective trajectory prediction of the target in different environments is realized. The present invention improves the target tracking accuracy in complex environments by associating the position and shape features of the target; and improves the target trajectory prediction accuracy and effect in the case of environmental changes by modeling the trajectory prediction-related parameters, and is applicable to dynamically changing scenarios.

[0089] In a specific embodiment, a mobile robot is equipped with an RGBD camera as a sensing device. Through a lightweight target detection algorithm and the Kalman filter, the detection state and prediction state of the target are represented, including a target position feature representation and a target shape feature representation. And through the Hungarian algorithm, the target position and target shape features are associated and matched to obtain the real-time spatial position of target tracking. At the same time, parametric modeling is carried out on the target tracking frequency and the trajectory prediction time step to realize dynamic parameter adjustment according to different environments, and then based on the target motion prediction equation, the spatial trajectory position prediction of the dynamic target is carried out.

[0090] Further, referring to Figure 2 , the step S200 includes:

[0091] S210. Determine the region of interest (ROI) and the target detection box based on the RGB image, where the region of interest (ROI) is included within the target detection box;

[0092] S220. Obtain the target pixel coordinates (u c , v c ) and the target pixel scale (S p , R p ) based on the target detection box, where u c represents the central point coordinate of the horizontal pixels in the target pixels of the target detection box, v c represents the central point coordinate of the vertical pixels in the target pixels of the target detection box, S p represents the area of the target detection box, and R p represents the aspect ratio of the target detection box;

[0093] S230. Map the region of interest (ROI) to the depth image, obtain the target depth information d, and convert the target pixel coordinates (u c , v c ) into the target world coordinates (X w , Y w , Z w );

[0094] S240. Calculate the target detection result Rt.

[0095] Specifically, in the step S200, the target position feature representation is through the intersection over union of the detection box and the prediction box; the target shape feature representation is by calculating the distances between the upper left coordinates and the lower right coordinates of the detection box and the prediction box, as well as the aspect ratio similarity between each detection box and the prediction box, and the prediction box is calculated based on the detection box through the Kalman filter transition matrix.

[0096] Further, referring to Figure 2 and Figure 4 , in the step S230, the world coordinates (X w , Y w , Z w ) of the target satisfy:

[0097] M I * M E * [X w , Y w , Zw , 1] T = d * [u, v, 1] T

[0098] Among them, M I is the camera internal parameter matrix, M E is the camera external parameter matrix, d is the target depth information, u represents the target horizontal pixel value, and v represents the target vertical pixel value;

[0099] In the step S240, at time t, the target detection result is:

[0100] R t = {(p i , c i ) | i ∈ (1, N)}

[0101] Among them, i represents the index of the perceived dynamic target, is the pixel coordinate of the i-th detected dynamic target.

[0102] Furthermore, in the step S200, the target position feature is the intersection over union ratio f iou (i, j) of the target detection box and the target prediction box, and the target shape feature is obtained by calculating the distance between the upper left coordinates and the distance between the lower right coordinates of the target detection box and the target prediction box, as well as the aspect ratio similarity of each target detection box and the target prediction box. Among them,

[0103] The Euclidean distance d 1 between the upper left coordinates of the target detection box and the target prediction box and the Euclidean distance d 2 between the lower right coordinates are expressed as:

[0104]

[0105] Among them, is the upper left coordinate of the i-th target detection box, is the lower right coordinate of the i-th target detection box, is the upper left coordinate of the j-th target prediction box, is the lower right coordinate of the j-th target prediction box,

[0106] The Euclidean distance similarity S 1 between the Euclidean distance d 2 between the upper left coordinates and the Euclidean distance d 1 between the lower right coordinates is:

[0107]

[0108] The aspect ratio similarity S 2 between the target detection box and the target prediction box is:

[0109]

[0110] Among them, r 1 refers to the aspect ratio of the target detection box, and r 2 refers to the aspect ratio of the target prediction box;

[0111] The intersection over union f of the target detection box and the target prediction box iou (i, j) is:

[0112]

[0113] Among them, A i and A j are the areas of the target prediction box and the target detection box respectively, and Ω ij represents the area of the overlapping part of the target prediction box and the target detection box;

[0114] Therefore, for the i-th target prediction box and the j-th target detection box, the value coefficient matrix combining the target position feature and the target shape feature is expressed as:

[0115] ζ ij = a * f iou (i, j) + b * S 1 + c * S 2 , (a + b + c = 1)

[0116] Among them, a, b, and c are all weight parameters with different weight values set according to different scenarios. The higher the weight value, the greater the influence of the corresponding feature on target tracking and matching.

[0117] Furthermore, referring to Figure 1 , in step S300, after obtaining the real-time state information of the target, combined with the target motion prediction equation, the trajectory of the target is predicted.

[0118] Among them, the target motion is approximated by a uniformly accelerated model, and the scale change is approximated by a first-order differential model;

[0119] Let the initial time be t 0 , the initial position of the dynamic target is the initial velocity is the initial acceleration is Therefore, the position of target i at time t is:

[0120]

[0121] Specifically, a dynamic target motion model is a mathematical model used to describe the motion state of a target in space and its changes over time. In dynamic target tracking and prediction, motion models are usually adopted to predict the motion trajectory of a target. Common motion models include the uniform motion model, the uniformly accelerated motion model, and the non-linear motion model, etc. Through the dynamic target motion model, the future position and velocity of a target can be estimated based on its historical state, thereby improving the tracking accuracy. In these models, the state usually includes position, velocity, and acceleration, and the state changes are described using motion equations. This model is widely applied in fields such as autonomous driving and intelligent robot navigation, helping the system to achieve real-time tracking of targets and trajectory prediction in complex environments.

[0122] The observation model is another important component in dynamic target tracking. It is used to describe how the system extracts information from the observation data to infer the state of the target. Usually, the state of the target is observed through sensors (such as cameras, radars, etc.), but these observation data often contain noise and errors. The role of the observation model is to establish the relationship between the observed values and the true state of the target, usually expressed in a linear or non-linear manner. By combining the observation model with the motion model, filtering algorithms can be used to optimize the estimation of the target state, reduce the influence of noise, and thus obtain more accurate target state information.

[0123] The Kalman filter algorithm is an optimal recursive state estimation algorithm for linear dynamic systems. It estimates the system state under noisy conditions by combining the motion model and the observation model of the target. This algorithm proceeds in two steps: prediction and update. First, it predicts the state of the target at the next moment based on the motion model; then, it corrects the predicted state by combining the current observed values to obtain a more accurate estimation. The Kalman filter is highly efficient in dealing with real-time dynamic target tracking problems, especially suitable for linear systems and Gaussian noise environments. In practical applications, algorithms such as the Extended Kalman Filter and the Unscented Kalman Filter have also been derived to handle non-linear systems.

[0124] The uniformly accelerated motion model is a model that describes the motion state of an object under uniformly accelerated motion conditions. In this model, the acceleration of the target remains constant, and its motion trajectory can be predicted based on the initial position, initial velocity, and acceleration of the target. In dynamic target tracking, the uniformly accelerated motion model is often used to handle motion scenarios with constant acceleration, such as the motion of vehicles in some autonomous driving scenarios. Under this model, the position and velocity of the target change linearly with time and can be calculated using formulas. Therefore, it provides a simple and effective basic model for motion trajectory prediction and is applicable when the target is in an accelerated motion state.

[0125] Scale variation refers to the change in the physical size of the target or the size of the detection box in the image sequence. In dynamic target tracking, scale variation is an important influencing factor because factors such as the distance of the target, the change in the camera angle, etc. will cause the size of the target in the image to change. To accurately track the target, scale variation must be effectively handled. Common methods include using a multi-scale detection framework or dynamically adjusting the size of the target through the model. In addition, scale variation may also affect the target tracking accuracy, especially when the target moves rapidly or the distance changes significantly, so filtering algorithms or other techniques are needed to compensate for scale variation.

[0126] Given that different environments have different requirements for trajectory prediction, by modeling the relevant parameters of trajectory prediction and dynamically adjusting the parameters to adapt to the trajectory prediction in different environments. The modeling parameters include but are not limited to the target tracking frequency and the trajectory prediction time step. Among them, the dynamic target tracking frequency refers to the frame interval for image processing, that is, the reciprocal of the Kalman filter state transition time. The trajectory prediction time step refers to the time interval for each calculation when predicting the target motion trajectory after obtaining the target position, speed, and acceleration each time. The target tracking frequency and the trajectory prediction time step can be preset according to the task scenario or dynamically adjusted according to environmental perception. The following lists the adjustment strategies in some scenarios.

[0127] 1) Different target scales: When tracking small and fast-moving targets such as flying birds and flying balls, high-precision trajectory prediction of dynamic targets is required. By adjusting the target tracking frequency and the trajectory prediction time step, target tracking is performed on each frame of the image to achieve high-frequency updates of the target tracking points and trajectory prediction points.

[0128] 2) Different target distances: When the target is far from the mobile robot, make a farther trajectory prediction for the target, and the flight trajectory of the dynamic target can be pre-judged to prepare for the subsequent robot path planning.

[0129] 3) Weather conditions: When the weather affects the trajectory prediction performance of the dynamic target, it is necessary to increase the target tracking frequency to update the target state information at a high frequency, and increase the target trajectory prediction time step to avoid the weather affecting the acquisition of multi-frame data, thereby affecting the tracking and prediction effect of the dynamic target.

[0130] In the present invention, the trajectory prediction method based on parametric modeling can dynamically adjust the target tracking frequency and the trajectory prediction time step according to different external environments, so as to adapt to the trajectory prediction requirements in the case of environmental changes and achieve effective trajectory prediction of dynamic targets in the case of changing environments.

[0131] Further, referring to Figure 1 , in step S300,

[0132] For different environments, the requirements for trajectory prediction are different. By modeling the relevant parameters of trajectory prediction and dynamically adjusting the parameters to adapt to the trajectory prediction in different environments, the modeling parameters at least include the target tracking frequency and the trajectory prediction time step;

[0133] Among them, the dynamic target tracking frequency is the frame interval for image processing, that is, the reciprocal of the Kalman filter state transition time;

[0134] The trajectory prediction time step is the time interval for each calculation when predicting the target motion trajectory after obtaining the target position, speed, and acceleration each time;

[0135] Adjust the trajectory prediction time step Δt according to different scenarios, and use linear interpolation to adjust the modeling formula of the trajectory prediction time step Δt.

[0136]

[0137] where d max is the maximum target tracking distance, d min is the minimum target tracking distance, t max is the maximum trajectory prediction time step, t min is the maximum trajectory prediction time step, d is the Euclidean distance between the dynamic target and the mobile robot. represents the degree of weather influence, and α represents the adjustment coefficient.

[0138] Specifically, parametric modeling is a model construction for the target tracking frequency and the trajectory prediction time step. For different environments, the target tracking frequency and the trajectory prediction time step can be set in advance or dynamically adjusted according to environmental perception. Parameters refer to indicators that can affect the accuracy of target tracking and trajectory prediction, prediction distance, etc., including but not limited to the target tracking frequency and the trajectory prediction time step. The target tracking frequency refers to the frame interval for image processing, that is, the reciprocal of the Kalman filter state transition time. The trajectory prediction time step refers to the time interval for each calculation when predicting the target motion trajectory after obtaining the target position, speed, and acceleration each time. Dynamically adjusting the target tracking frequency and the trajectory prediction time step means, on the one hand, setting the target tracking frequency and the trajectory prediction time step in advance according to the task scenario; on the other hand, adjusting the target tracking frequency and the trajectory prediction time step to adapt to the requirements of tracking and trajectory prediction under different target distances and sizes by detecting information such as the target distance and size through sensors. For example, when detecting a small target or a fast-moving target, adjust the preset target tracking frequency value to reduce the Kalman filter state transition time, thereby increasing the target tracking frequency; when predicting the trajectory of dynamic targets at different distances, use linear interpolation to adjust the trajectory prediction time step, and the trajectory prediction time step is represented by Δt.

[0139] Further, in the step S300, the target state information includes the target state X k and the observed state Z k , and the target state is represented by the state transition equation, and the observed state is represented by the observation equation as follows:

[0140]

[0141] Among them, the state transition matrix of the uniform acceleration model is:

[0142]

[0143] The observation matrix of the uniform acceleration model is:

[0144]

[0145] Among them, k represents the k-th frame time, dt represents the Kalman filter state transition time interval, W k-1 and V k respectively represent the normal distribution process noise and the normal distribution measurement noise, assumed to be zero-mean noise, X k-1 represents the target state at the previous moment, X k represents the current target state, I 3×3 represents a 3×3 identity matrix, and O 3×3 represents a 3×3 zero matrix;

[0146] By the correlation matching of the target position feature and the target shape feature, the target scale model state transition equation is obtained, and the target scale model state transition equation is:

[0147]

[0148] Among them, represents the scale state of the target at time k, represents the scale state of the target at the previous moment, represents the state transition matrix of the target scale, is the normal distribution process noise,

[0149] Among them, the state transition matrix of the target scale is:

[0150]

[0151] The observation matrix is:

[0152]

[0153] Specifically, the target state information X k is an internal representation of physical quantities such as the true position, velocity, and acceleration of the target in the Kalman filter algorithm. These state variables constitute a complete description of the target motion in the system and are mainly used to predict the future position of the target. The target state information X k describes the position, velocity, and acceleration of the target at time k. Through the Kalman filter, the system is based on the target state information X k―1 at the previous time and the state transition matrix A to predict the target state information X k at the next time.

[0154] The observation state Z k refers to the observation data of the target directly obtained through sensors, which contains the target information obtained from sensors (such as RGBD cameras). The observation state Z k usually contains the following information: pixel coordinates, velocity information, scale information, etc., all of which are the values of the target detection result R obtained in step S200 t .

[0155] Therefore, the target state X k and the observation state Z k are both necessary parameters in the Kalman filter. Through the Kalman filter, the target state X k will be corrected according to the observation state Z k so that the system can accurately predict and track the motion of the target.

[0156] Furthermore, referring to Figure 1 and Figure 3 , the step S400 includes:

[0157] S410. Based on the Hungarian algorithm, calculate the intersection over union and shape feature similarity between all target detection boxes and the target prediction box positions in the previous frame;

[0158] S420. Set different weights to obtain the Hungarian matrix, and use the Hungarian algorithm to maximize the target matching value;

[0159] S430. Set the minimum threshold ζ min to filter the matching results, obtain the best match that meets the requirements, and targets that do not match successfully will no longer be subject to the Kalman filter.

[0160] Referring to Figure 5, which is an application example of the method of the present invention, shows the comparison between the three-dimensional trajectory tracking prediction value of the target ball based on the method of the present invention and the tracking detection value of the target ball by the motion capture system. It contains three small graphs, each of which visually analyzes different dimensions of the trajectory prediction. The first three small graphs respectively show the comparison between the tracking prediction trajectory of the target ball based on the algorithm and the tracking detection trajectory realized by the motion capture system in the X, Y, and Z axis directions. The horizontal axis represents the time axis, and the vertical axis represents the coordinate values in each direction. The motion capture system is an optical motion capture system built by NOKOV (Metric) Technology. It is a space of 10*10*3m surrounded by more than 20 cameras, with a positioning accuracy of sub-millimeter level. Therefore, it has high reference value for the real position of the ball in space. Figure 5 It aims to show the error degree between the two and verify the prediction accuracy of the method in the XYZ axis directions. The closer the curves are, the better the prediction effect of the system.

[0161] Specifically, Figure 5 It represents the comparison between the target tracking result and the real position of the target detected by the motion capture system. The specific implementation process is to set the correlation weight of the target position and shape features. When the target is detected, the target detection box is obtained, and at the same time, the target prediction box is obtained through Kalman filtering. The target position features and shape features of the target detection box and the target prediction box are represented, and the position feature similarity and shape feature similarity between the target detection box and the prediction box are calculated respectively. The position and shape features are associated and matched, and the targets with successful matching are updated in status to achieve the acquisition of the target three-dimensional position. The specific experimental process is as follows: The tennis ball is towed by a transparent thin string and swayed in front of the perspective of the robot camera. The tennis ball is detected and tracked by the target tracking method in the present invention to obtain the target three-dimensional position. Through the analysis of the example data, the average position error of the target tracking by the present invention is 3.4 cm.

[0162] Referring to Figure 6 , which is an application example of the method of the present invention, shows the comparison between the three-dimensional trajectory tracking prediction value of the target ball based on the method of the present invention and the tracking detection value of the target ball by the motion capture system. It contains six small graphs. The first three small graphs respectively show the comparison between the tracking prediction trajectory of the target ball based on the algorithm and the tracking detection trajectory realized by the motion capture system in the X, Y, and Z axis directions when the trajectory prediction time step is 1 second. The last three small graphs respectively show the comparison between the tracking prediction trajectory of the target ball based on the algorithm and the tracking detection trajectory realized by the motion capture system in the X, Y, and Z axis directions when the trajectory prediction time step is 2 seconds. The horizontal axis represents the time axis, and the vertical axis represents the coordinate values in each direction. Figure 6The six small figures are mainly used to verify the trajectory prediction accuracy of the system in the X, Y, and Z directions at different time steps. Whether the time step is 1 second or 2 seconds, the system can maintain high accuracy in three-dimensional space, indicating that the invention has good robustness and can meet the requirements in different environments.

[0163] Specifically, Figure 6 It represents the comparison between the predicted result of the target trajectory and the actual position of the target detected by the motion capture system. The specific implementation process is as follows: Throw a tennis ball towards the drone, and obtain the three-dimensional position, velocity, and acceleration information of the target through the target tracking method based on position and shape features in the present invention. At the same time, by setting the trajectory prediction time steps of 1 s and 2 s to simulate the trajectory prediction time step adjustment strategy in different environments, obtain the predicted positions of the target at different trajectory prediction time steps, and conduct error comparison and analysis. The experiment shows that the system maintains good accuracy at different trajectory prediction time steps and can meet different trajectory prediction requirements in different environments.

[0164] Furthermore, referring to Figure 1 the present invention also proposes a low-altitude dynamic target tracking and trajectory prediction device for implementing the low-altitude dynamic target tracking and trajectory prediction method. The low-altitude dynamic target tracking and trajectory prediction device includes:

[0165] A host computer;

[0166] A depth camera, which is electrically connected to the host computer;

[0167] A visual inertial odometer, which is electrically connected to the host computer.

[0168] Furthermore, the present invention also proposes a computer-readable storage medium, on which program instructions are stored. When the program instructions are executed by a processor, the low-altitude dynamic target tracking and trajectory prediction method is implemented.

[0169] It should be recognized that the method steps in the embodiments of the present invention can be implemented or carried out by computer hardware, a combination of hardware and software, or computer instructions stored in a non-transitory computer-readable memory. The method can use standard programming techniques. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, if necessary, the program can be implemented in assembly or machine language. In any case, the language can be a compiled or interpreted language. In addition, for this purpose, the program can run on a dedicated integrated circuit programmed for this purpose.

[0170] In addition, the operations of the processes described herein can be performed in any suitable order, unless otherwise indicated herein or otherwise clearly contradicted by the context. The processes described herein (or variations and / or combinations thereof) can be performed under the control of one or more computer systems configured with executable instructions and can be implemented as code (e.g., executable instructions, one or more computer programs, or one or more applications) executed jointly on one or more processors, by hardware, or by a combination thereof. The computer program includes a plurality of instructions executable by one or more processors.

[0171] Further, the method can be implemented in any type of computing platform operably connected to a suitable one, including but not limited to personal computers, minicomputers, mainframes, workstations, network or distributed computing environments, separate or integrated computer platforms, or communication with charged particle tools or other imaging devices, etc. Aspects of the present invention can be implemented in machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into the computing platform, such as a hard disk, optical read and / or write storage medium, RAM, ROM, etc., such that it can be read by a programmable computer and can be used to configure and operate the computer to perform the processes described herein when the storage medium or device is read by the computer. In addition, the machine-readable code, or portions thereof, can be transmitted via a wired or wireless network. When such media includes instructions or programs that implement the above-described steps in combination with a microprocessor or other data processor, the inventions described herein include these and other different types of non-transitory computer-readable storage media. When programmed according to the methods and techniques of the present invention, the present invention can also include the computer itself.

[0172] The computer program can be applied to the input data to perform the functions described herein, thereby transforming the input data to generate output data stored in non-volatile memory. The output information can also be applied to one or more output devices such as a display. In a preferred embodiment of the present invention, the transformed data represents physical and tangible objects, including a specific visual depiction of the physical and tangible objects generated on the display.

[0173] As described above, only the preferred embodiments of the present invention are given, and the present invention is not limited to the above-described embodiments. As long as the same means are used to achieve the technical effects of the present invention, any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention. Within the scope of protection of the present invention, its technical solutions and / or implementation manners can have various different modifications and changes.

Claims

1. A low-altitude dynamic target tracking and trajectory prediction method, applied to a low-altitude dynamic target tracking and trajectory prediction device, the low-altitude dynamic target tracking and trajectory prediction device at least includes a host computer, a depth camera and a visual inertial odometer, the depth camera and the visual inertial odometer are electrically connected to the host computer respectively, characterized in that: The low-altitude dynamic target tracking and trajectory prediction method comprises the following steps: S100, the depth camera collects RGB images and depth images of the dynamic target, the visual inertial odometer collects posture information, and transmits the RGB image, depth image and posture information to the host computer; S200, based on a lightweight target detection algorithm, the host computer detects the target in the RGB image and obtains the target detection result R t , the target detection result R t Including target location information and target shape information; S300, based on the Kalman filter algorithm, predicting the state of the target at the next moment to obtain target prediction information, comparing the predicted value with the target state information and correcting the target prediction information; S400, based on the Hungarian algorithm, associate and match the detection state information of each target at the current moment with the target prediction state information at the previous moment, update the target position features and target shape features, and realize target state tracking.

2. The low-altitude dynamic target tracking and trajectory prediction method according to claim 1, characterized in that: The step S200 includes: S210, determining a region of interest and a target detection frame based on the RGB image, wherein the region of interest is contained in the target detection frame; S220, based on the target detection frame, obtain the target pixel coordinates (u c ,v c ) and the target pixel scale (S p ,R p ), where u c Represents the coordinates of the center point of the horizontal pixel in the target pixel of the target detection box, v c Represents the center point coordinates of the target pixel in the target detection box, S p Represents the area of ​​the target detection box, R p Indicates the aspect ratio of the target detection box; S230, mapping the region of interest to the depth image, obtaining target depth information d, and converting the target pixel coordinates (u c ,v c ) is converted to the target world coordinate (X w ,Y w ,Z w ); S240, calculating the target detection result Rt.

3. The low-altitude dynamic target tracking and trajectory prediction method according to claim 2 is characterized in that: In step S230, the world coordinates (X w ,Y w ,Z w )satisfy: M I *M E *[X w ,Y w ,Z w ,1] T =d*[u,v,1] T Among them, M I is the camera intrinsic parameter matrix, M E is the camera extrinsic matrix, d is the target depth information, u represents the target horizontal pixel value, and v represents the target vertical pixel value; In step S240, at time t, the target detection result is: Where i represents the perceived dynamic target index, For the The pixel coordinates of the detected dynamic targets.

4. The low-altitude dynamic target tracking and trajectory prediction method according to claim 1, characterized in that: In step S200, the target position feature is the intersection-over-intersection ratio f of the target detection frame and the target prediction frame. iou (i, j), the target shape feature is obtained by calculating the distance between the upper left corner coordinates and the lower right corner coordinates of the target detection box and the target prediction box, as well as the similarity of the aspect ratio between each target detection box and the target prediction box, where The Euclidean distance d1 between the upper left corner coordinates of the target detection box and the target prediction box and the Euclidean distance d2 between the lower right corner coordinates are expressed as: in, is the coordinate of the upper left corner of the i-th target detection box, is the coordinate of the lower right corner of the i-th target detection box, is the coordinate of the upper left corner of the j-th target prediction box, is the coordinate of the lower right corner of the j-th target prediction box, The Euclidean distance similarity S1 between the upper left corner coordinates d1 and the lower right corner coordinates d2 is: The aspect ratio similarity S2 between the target detection box and the target prediction box is: Among them, r1 refers to the aspect ratio of the target detection box, and r2 refers to the aspect ratio of the target prediction box; The intersection of the target detection box and the target prediction box is f iou (i,j) is: Among them, A i and A j are the areas of the target prediction box and the target detection box, respectively, ij Represents the area of ​​the overlapping part of the target prediction box and the target detection box; Therefore, for the i-th target prediction box and the j-th target detection box, the value coefficient matrix of the joint target position feature and target shape feature is expressed as: ζ ij =a*f iou (i,j)+b*S1+c*S2,(a+b+c=1) Among them, a, b, and c are weight parameters with different weight values ​​set according to different scenarios. The higher the weight value, the greater the influence of the corresponding feature on target tracking and matching.

5. The low-altitude dynamic target tracking and trajectory prediction method according to claim 1, characterized in that: In step S300, after obtaining the real-time state information of the target, the trajectory of the target is predicted by combining the target motion prediction equation. Among them, the target motion is approximated by the uniform acceleration model, and the scale change is approximated by the first-order differential model; Assume that at the initial time t0, the initial position of the dynamic target is The initial speed is The initial acceleration is Therefore, the position of target i at time t is for:

6. The low-altitude dynamic target tracking and trajectory prediction method according to claim 5, characterized in that: In step S300, Different environments have different requirements for trajectory prediction. By modeling trajectory prediction related parameters and dynamically adjusting the parameters, the trajectory prediction in different environments can be adapted. The modeling parameters at least include the target tracking frequency and the trajectory prediction time step. Among them, the dynamic target tracking frequency is the frame interval of image processing, that is, the inverse of the Kalman filter state transition time; The trajectory prediction time step is the time interval between each calculation when predicting the target motion trajectory after obtaining the target position, velocity and acceleration once; According to different scenarios, the trajectory prediction time step Δt is adjusted, and the trajectory prediction time step Δt modeling formula is adjusted using linear interpolation method. Among them, d max is the maximum target tracking distance, d min is the minimum target tracking distance, t max is the maximum trajectory prediction time step, t min is the maximum trajectory prediction time step, d is the Euclidean distance between the dynamic target and the mobile robot, It indicates the degree of weather influence, and α indicates the adjustment coefficient.

7. The low-altitude dynamic target tracking and trajectory prediction method according to claim 1, characterized in that: In step S300, the target state information includes the target state X k and the observed state Z k , target state Expressed by the state transfer equation, the observed state The observation equation is expressed as: Among them, the state transfer matrix of the uniform acceleration model is for: Observation matrix of uniform acceleration model for: Where k represents the kth frame time, dt represents the Kalman filter state transfer time interval, W k-1 and V k They represent the normal distribution process noise and normal distribution measurement noise respectively, assuming zero mean noise, X k-1 represents the target state at the previous moment, X k Indicates the current state of the target, I 3×3 represents the 3×3 identity matrix, O 3×3 represents a 3×3 zero matrix; By associating and matching the target position features and the target shape features, the target scale model state transfer equation is obtained, and the target scale model state transfer equation is: in, represents the scale state of the target at time k, Indicates the scale state of the target at the previous moment, represents the state transition matrix of the target scale, is the normally distributed process noise, Among them, the state transfer matrix of the target scale is for: Observation Matrix for:

8. The low-altitude dynamic target tracking and trajectory prediction method according to claim 1, characterized in that: The step S400 includes: S410, based on the Hungarian algorithm, by calculating the intersection-over-union ratio of positions of all target detection frames and the target prediction frames of the previous frame and the similarity of shape features; S420, setting different weights to obtain a Hungarian matrix, and using the Hungarian algorithm to maximize the target matching value; S430, setting the minimum threshold value ζ min The matching results are filtered to obtain the best match that meets the requirements, and the targets that are not successfully matched are no longer Kalman filtered.

9. A low-altitude dynamic target tracking and trajectory prediction device, used to implement the low-altitude dynamic target tracking and trajectory prediction method according to any one of claims 1 to 8, characterized in that: The low-altitude dynamic target tracking and trajectory prediction device comprises: Host computer; A depth camera, the depth camera is electrically connected to the host computer; The visual inertial odometer is electrically connected to the host computer. 10 . A computer-readable storage medium having program instructions stored thereon, wherein the program instructions are executed by a processor to implement the method according to claim 1 .

Images