Object motion estimation and tracking method and device based on adaptive Kalman filtering

Abstract

The invention discloses an object motion estimation and tracking method and device based on adaptive Kalman filtering, and the method comprises the steps: carrying out the kinematics analysis based on the wind resistance, rotation and Magnus effect of a flying sphere, and precisely building a motion equation of the flying sphere; secondly, designing an adaptive Kalman filtering algorithm to solve a motion equation, namely extracting convolution features through an FCOS algorithm, realizing flying ball dynamic tracking based on a KCF tracker, reconstructing a target three-dimensional pose through a camera projection model, inputting the target three-dimensional pose into an adaptive Kalman filter, realizing flying ball motion equation solving, and improving robustness of measurement noise and modeling errors; and finally, aiming at the problem of autonomous view loss, designing a PWC-Net dense optical flow method to realize autonomous view dynamic maintenance, supporting rapid and accurate self-sensing motion estimation and tracking of the flying sphere, and effectively overcoming the loss rate and hysteresis of the tracking of the flying sphere.

Description

Technical Field

[0001] The present invention belongs to the field of motion object perception and tracking technology, and in particular relates to a method and device for object motion estimation and tracking based on adaptive Kalman filtering. Background Art

[0002] Computer vision-based research on moving targets is crucial for a variety of applications, including robotics, intelligent driving, and hazardous operations. Within this context, the perception and tracking of moving targets has garnered widespread attention. This focus is on achieving object detection, tracking, and real-time prediction. Object detection uses detection algorithms to locate and classify objects in the field of view. Object tracking aims to continuously acquire information such as the target's position. Trajectory prediction aims to obtain a pre-determined understanding of the target's subsequent motion state and information, enabling interaction with the target.

[0003] During the research, the following problems were found:

[0004] (1) Traditional moving object perception and tracking methods use too few parameters in the motion equations established for the target, and the modeling is too rough, resulting in low prediction accuracy;

[0005] (2) Predefined anchor boxes are often used in target detection, which makes the detection process complicated and reduces the real-time performance and flexibility of detection;

[0006] (3) The tracker algorithm is highly complex, has poor real-time performance, and consumes large amounts of computing and memory resources;

[0007] (4) The prediction accuracy is poor in nonlinear time-varying scenarios. Summary of the Invention

[0008] To address the shortcomings of the existing technology, achieve self-perceived motion estimation and tracking of flying objects, and improve tracking efficiency and accuracy, the present invention adopts the following technical solutions:

[0009] The object motion estimation and tracking method based on adaptive Kalman filtering includes the following steps:

[0010] Step 1: Perform kinematic analysis on the object and construct the object's motion equation;

[0011] Step 2: Use the FCOS (Fully Convolutional One-stage Object Detector) algorithm to extract and match features of moving objects, perform bounding box regression and center prediction at each scale to complete target detection of moving objects;

[0012] Step 3: Based on the KCF (Kernelized Correlation Filter) tracker, matching is performed in the target area. The target detection result of the moving object is used as the initial detection boundary. The features of the area are extracted and the position with the maximum response is found as the target moving object to track the moving object in real time.

[0013] Step 4: Obtain the state vector of the target moving object; use the depth camera intrinsic parameters to convert the center of mass of the moving object from image coordinates to actual three-dimensional coordinates, obtain the position, velocity and acceleration information of the target moving object, and obtain the angular velocity through the micro gyroscope on the moving object;

[0014] Step 5: Input the state vector of the target moving object into the adaptive Kalman filter, solve the motion equation of the moving object, output the predicted target moving object motion information, and estimate and optimize the object's motion trajectory based on this information, thereby achieving the modeling and prediction of the position, velocity and acceleration of the flying target;

[0015] Step 6: Through the Pyramidal, Warping, and Cost Volume Net (PWC-Net) dense optical flow method, the optical flow information is obtained and input into the KCF tracker to achieve autonomous dynamic field of view maintenance and handle the problem of target field of view loss caused by occlusion, rapid motion or target change. The obtained optical flow information is used to solve the problem of target moving object loss through optical flow consistency.

[0016] Furthermore, the object motion equation constructed in step 1 is a nonlinear differential equation of the object:

[0017]

[0018] F g =mg

[0019]

[0020] F m =S(ω×v)

[0021] Among them, m represents the mass of the object, r represents the position vector of the object, and t represents the time. Indicates the time derivative of the position vector, F g Indicates the gravity of the object, F d Indicates the air resistance of the object, F m represents the Magnus force, g represents the gravitational acceleration vector, C drepresents the drag coefficient, ρ represents the air density, A represents the obstructed (windward) area of ​​the object, v represents the velocity vector of the object, S represents a constant related to the surface roughness of the object and the air density, ω represents the angular velocity vector of the object, and v represents the velocity vector of the object. Substituting the above forces into Newton's second law and decomposing the acceleration a into differential equations of position and velocity, we obtain the nonlinear differential equation for the moving object:

[0022]

[0023] That is, the corresponding motion equation is obtained:

[0024] a=g+a drag +a magnus

[0025] a drag =-c d ∥v∥v

[0026] a magnus =c m (ω×v)

[0027] Among them, a drag is the equivalent acceleration of air resistance, a magnus represents the Magnus effect equivalent acceleration, represents the air resistance coefficient, represents the Magnus effect coefficient. Considering the above three forces, the system has nonlinearity and time variability.

[0028] Furthermore, in step 2, the target detection FCOS algorithm is used to directly predict the target bounding box at each pixel position in the image. First, the target moving object image and feature extraction are input. The input image is passed through the pyramid network (FPN) to extract the feature map F. In the mentioned scenario, only the target bounding box needs to be predicted:

[0029] (l,t,r,b)=f reg (F)

[0030] Among them, l represents the left distance, t represents the upper distance, r represents the right distance, b represents the lower distance, and f reg Convolutional layer representing the regression branch.

[0031] Furthermore, in step 2, in order to solve the problem of blurred boundaries of the target flying sphere image, the center degree C is used to measure the distance between the predicted point and the center of the moving object:

[0032]

[0033] Among them, min(,) represents the minimum operation, max(,) represents the maximum operation, the value of the center degree is between 0 and 1, and the higher the center degree, the closer the predicted point is to the target center. Regression prediction is performed on each pixel position of the feature map, and the prediction bounding box with high confidence is retained through threshold screening, and duplicates are removed to obtain the final detection result y.

[0034] Furthermore, in step 3, the KCF tracker aims to find a filter f such that the input image patch x i The correlation with the detection result y of the target moving object is the largest, and the objective function is expressed as:

[0035]

[0036] in, <f,x i > represents the filter f and the input image x i The convolution result, y i Represents the detection result of the i-th target moving object, λ represents the regularization parameter, and the KCF tracker uses Fourier transform to convert the convolution operation into the frequency domain. In order to speed up the calculation, the objective function is converted to:

[0037]

[0038] in, and They represent the filter f ball , test results y i and image patch x i Fourier transform of the kernel matrix, Represents element-wise multiplication, and the solution to the filter is obtained:

[0039]

[0040] Choose a Gaussian kernel function:

[0041]

[0042] Through Fourier transform, the kernel matrix is ​​expressed in the frequency domain as:

[0043]

[0044] Among them, x represents the feature vector of the image patch used for tracking in the current frame, z represents the template feature vector of the target model, σ represents the bandwidth parameter, represents Fourier transform;

[0045] In each frame, the KCF tracker updates the filter and uses the target position of the current frame and the new image patch to update the model. The update formula is:

[0046]

[0047] Among them, η represents the learning rate, represents the filter of the previous frame, The filter representing the current frame is solved by the frequency domain representation of the convolution operation. The position of the target moving object is calculated by the following formula:

[0048]

[0049] in, Represents the inverse Fourier transform, the position of the maximum value in the response graph is the position of the moving object, and the corresponding moving object target bounding box (l r ,t r ,r r ,b r ).

[0050] Furthermore, in step 4, a depth image is acquired and the depth data of the moving object is extracted from it. The center of mass of the object is calculated based on the average position of the object's pixels. The center of mass is converted from image coordinates to actual three-dimensional coordinates using the camera's intrinsic parameters. The position data of the object is continuously sampled over a period of time, and the speed is calculated based on the rate of change of the position:

[0051]

[0052] Calculate the acceleration from the rate of change of velocity:

[0053]

[0054] Among them, r represents the position vector of the target moving object, v represents the velocity vector of the target moving object, a represents the acceleration vector of the target moving object, k represents the current state, and Δt represents the discrete time step.

[0055] Furthermore, in step 5, the trajectory of the target moving object is updated, and its discrete motion equation is as follows:

[0056]

[0057] The target motion bitmap position, velocity and acceleration information obtained in step 4 are used as the input of the adaptive Kalman filter, and the initial state vector x0 = [r0, v0] is set. T , where r0 represents the initial position, v0 represents the initial velocity, the initial control input u0 = a0, a0 represents the initial acceleration, the discrete time step is Δt, and the state transfer matrix F and control matrix B are:

[0058]

[0059] Use the state transfer matrix and control matrix to predict the state and covariance at the next moment and predict the state vector x k+1∣k and the covariance matrix P k+1∣k for:

[0060] x k+1∣k =Fx k +Bu k

[0061] P k+1∣k =FP k F T +Q k

[0062] Among them, u k =a k represents the control input, Q k is the process noise covariance matrix, initially the unit matrix Q0=I 6×6 , the covariance matrix is ​​initially set to the identity matrix P0 = I 6×6 ;

[0063] Combine the observations and Kalman gain to update the state and covariance, and calculate the Kalman gain K k :

[0064] K k =P k+1∣k H T (HP k+1∣k H T +R k ) -1

[0065] Update the state vector x k+1 and the covariance matrix P k+1 :

[0066] y k =z k -Hx k+1∣k

[0067] x k+1 =x k+1∣k +K k y k

[0068] P k+1 =(IK k H)P k+1∣k

[0069] Among them, z k Represents the observation value, H represents the observation matrix, and the observed residual y k Dynamically adjust the process noise covariance matrix Q k and the observation noise covariance matrix R k, adapt to system changes:

[0070]

[0071] Among them, α represents the equilibrium parameter;

[0072] Then according to the object motion equation in step 1, u k+1 To update:

[0073]

[0074] Among them, g represents the gravitational acceleration vector, c d represents the drag coefficient, v k+1 represents the updated velocity vector of the moving object, S represents a constant related to the surface roughness of the object and the air density, m represents the mass of the object, ω k+1 Represents the updated angular velocity vector of the moving object;

[0075] According to the modified Kalman gain K k , observation residual y k To update the predicted subsequent state vector x and control input u:

[0076]

[0077] Among them, the updated initial vector is the state vector N represents the discrete time until the end of the motion state, n=1,2,3...N.

[0078] Furthermore, in step 6, CNN is used to extract pyramid features from the two images I1 and I2 of the detected target moving object:

[0079]

[0080] in, and Represents the feature map at layer l, L represents the number of pyramid levels, from high resolution to low resolution, at each pyramid level l, the optical flow w estimated by the previous level is used l+1 Features of the current level To make a distortion:

[0081]

[0082] Align the features of the second image to the features of the first image to reduce the displacement difference between the images, where x represents the pixel coordinates in the image. After distortion, calculate the cost volume:

[0083]

[0084] Among them, d represents the displacement hypothesis. The matching cost under different displacement hypotheses is expressed by comparing the similarity between the distorted features and the features of the first image. A small convolutional network is used to process the cost volume and refine the optical flow estimation layer by layer. The optical flow estimation is expressed as:

[0085] w l =ConvNet(C l ,w l+1 )

[0086] Among them, w l represents the optical flow estimation at layer l, w l+1 Represents the optical flow estimation result of the previous layer, and further refines the optical flow estimation result through the context network:

[0087]

[0088] The optical flow information is input into the KCF tracker. When target loss occurs due to occlusion, rapid motion or target change, strategies such as optical flow consistency detection, re-detection mechanism, multi-frame backtracking and multi-feature fusion are used to handle target loss.

[0089] An object motion estimation and tracking device based on adaptive Kalman filtering, including a kinematic equation construction module, a target detection module, a real-time tracking module, a state acquisition module, a prediction module, and an optical flow information generation module;

[0090] The kinematic equation construction module performs kinematic analysis on the object and constructs the object's motion equation;

[0091] The object detection module uses the FCOS (Fully Convolutional One-stage Object Detector) algorithm to extract and match features of moving objects, and performs bounding box regression and center prediction at each scale to complete the detection of moving objects.

[0092] The real-time tracking module performs matching in the target area based on the KCF (Kernelized Correlation Filter) tracker, uses the target detection result of the moving object as the initial detection boundary, extracts the features of the area, and finds the position with the maximum response as the target moving object to track the moving object in real time;

[0093] The state acquisition module is used to obtain the state vector of the target moving object; using the depth camera intrinsic parameters to convert the center of mass of the moving object from image coordinates to actual three-dimensional coordinates, obtain the position, velocity and acceleration information of the target moving object, and obtain the angular velocity through the micro gyroscope on the moving object;

[0094] The prediction module inputs the state vector of the target moving object into the adaptive Kalman filter, solves the motion equation of the moving object, outputs the predicted motion information of the target moving object, and estimates and optimizes the motion trajectory of the object based on the output, thereby achieving position, velocity and acceleration motion modeling and prediction of the flying target;

[0095] The optical flow information generation module obtains optical flow information through the PWC-Net (Pyramidal, Warping, and Cost VolumeNet, based on feature pyramid extraction, optical flow mapping, and matching correlation cost measurement) dense optical flow method, and inputs it into the KCF tracker to achieve autonomous dynamic field of view maintenance and handle the problem of target field of view loss caused by occlusion, rapid motion or target change. The obtained optical flow information solves the problem of target moving object loss through optical flow consistency.

[0096] The advantages and beneficial effects of the present invention are:

[0097] This invention solves the equations of motion for a flying sphere by designing an adaptive Kalman filter algorithm. It addresses dynamic tracking using convolution features and correlation filtering, and addresses the problem of autonomous field of view loss using optical flow, enabling self-perceived motion estimation and tracking of flying objects. Experiments demonstrate that this invention can perform perceptual tracking using the FCOS algorithm and KCF tracker, predict the target flying sphere using an adaptive Kalman filter algorithm, and reduce the probability of target loss using the PWC-Net algorithm, achieving rapid and accurate tracking of the flying sphere. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] Figure 1 Flowchart of the method in an embodiment of the present invention.

[0099] Figure 2 This is a force analysis diagram of a flying spherical target in an embodiment of the present invention.

[0100] Figure 3 Schematic diagram of bounding box regression and center prediction results in an embodiment of the present invention.

[0101] Figure 4 Schematic diagram of the real-time tracking result of a flying sphere in an embodiment of the present invention.

[0102] Figure 5 Schematic diagram of the predicted trajectory of the flying sphere self-perception motion estimation and tracking method in an embodiment of the present invention.

[0103] Figure 6a This is one of the optical flow information graphs of a flying sphere in an embodiment of the present invention.

[0104] Figure 6b This is the second optical flow information diagram of a flying sphere in an embodiment of the present invention.

[0105] Figure 6c This is the third optical flow information diagram of a flying sphere in an embodiment of the present invention.

[0106] Figure 7 Schematic diagram of the structure of the device in an embodiment of the present invention. DETAILED DESCRIPTION

[0107] The following describes the specific embodiments of the present invention in detail with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only used to illustrate and explain the present invention and are not intended to limit the present invention.

[0108] like Figure 1 As shown in FIG, the object motion estimation and tracking method based on adaptive Kalman filtering includes the following steps:

[0109] Step 1: Perform kinematic analysis based on the wind resistance, rotation, and Magnus effect of the flying sphere to accurately establish the motion equation of the flying sphere.

[0110] The kinematic analysis and design of the wind resistance, rotation and Magnus effect involved in the flying sphere are carried out. The sphere will produce a certain rotation during the movement. The model is simplified and the sphere is approximately considered to be a six-degree-of-freedom rigid body. At this time, it is subject to gravity F g , air resistance F d and Magnus force F m , considering the three forces, the system has nonlinearity and time variability, and nonlinear differential equations can be established. The force analysis is as follows Figure 2 shown.

[0111]

[0112] Where m is the mass of the sphere and r is the position vector. The three forces are expanded as follows:

[0113] F g =mg

[0114]

[0115] F m =S(ω×v)

[0116] Where g is the gravitational acceleration vector, C dis the drag coefficient, ρ is the air density, A is the frontal area of ​​the sphere, and v is the velocity vector of the sphere; the Magnus force is F m , S is a constant related to the surface roughness of the sphere and the air density, ω is the angular velocity vector of the sphere, and v is the velocity vector of the sphere. Substituting the above forces into Newton's second law and decomposing the acceleration a into differential equations for position and velocity, we obtain the nonlinear differential equation for the flying sphere:

[0117]

[0118] That is, the corresponding motion equation is obtained:

[0119] a=g+a drag +a magnus

[0120] Among them, r(t) is the position, v(t) is the velocity, a is the acceleration, g is the acceleration due to gravity, a drag is the equivalent acceleration due to air resistance, a magnus is the Magnus effect equivalent acceleration, and the calculation formulas for the air resistance equivalent acceleration and the Magnus effect equivalent acceleration are:

[0121] a drag =-c d ∥v∥v

[0122] a magnus =c m (ω×v)

[0123] in, is the air resistance coefficient, is the Magnus effect coefficient. Considering the three forces mentioned above, the system has nonlinearity and time variability.

[0124] Step 2: Use the FCOS algorithm to extract convolution features and perform feature matching to achieve flying sphere target detection.

[0125] Using the FCOS algorithm, we extract convolutional features and perform feature matching, and directly perform bounding box regression and center prediction at each scale to achieve flying sphere target detection. The bounding box regression and center prediction results are shown in Figure 2. Figure 3 shown.

[0126] Specifically, the FCOS algorithm is used to directly predict the target bounding box at each pixel position in the image. First, the target flying sphere image and feature extraction are input. The input image is passed through the pyramid network (FPN) to extract the feature map F. In the mentioned scenario, only the target bounding box needs to be predicted:

[0127] (l,t,r,b)=f reg (F ball )

[0128] Among them, l represents the left distance, t represents the upper distance, r represents the right distance, b represents the lower distance, and f reg It is the convolution layer of the regression branch. In order to solve the problem of blurred boundary of the target flying sphere image, the center degree C is used. ball A measure of how far the predicted point is from the center of the flying sphere:

[0129]

[0130] The value of the center degree is between 0 and 1. The higher the center degree, the closer the predicted point is to the target center. Regression prediction is performed on each pixel position of the feature map. The prediction box with high confidence is retained through threshold screening. The overlapping boxes are removed using NMS to obtain the final detection result y ball .

[0131] Step 3: Based on the KCF tracker, fast matching is performed in the target area. The detection result of the flying sphere area by the FCOS algorithm is used as the initial detection boundary. The features of the area are extracted and the position with the largest response is found to be the target flying sphere to be tracked, thereby achieving real-time tracking of the flying sphere. The real-time tracking results are as follows: Figure 4 shown.

[0132] The goal of the KCF tracker is to find a filter f ball , so that the input image patch x and the target sample y ball The correlation is the largest. The objective function can be expressed as:

[0133]

[0134] in, <f ball ,x i > represents the filter f ball With the input image x i The convolution result of is the target response, and λ is the regularization parameter. KCF uses Fourier transform to convert the convolution operation into the frequency domain. In order to speed up the calculation, the objective function can be converted to:

[0135]

[0136] in, and They are f ball 、 and x i Fourier transform of the kernel matrix, Represents element-wise multiplication. The solution to the filter can be obtained:

[0137]

[0138] Choose a Gaussian kernel function:

[0139]

[0140] Through Fourier transform, the kernel matrix is ​​expressed in the frequency domain as: In each frame, KCF updates the filter and uses the target position of the current frame and the new image patch to update the model. The update formula is:

[0141]

[0142] Where η is the learning rate, is the filter of the previous frame, is the filter of the current frame. By solving the frequency domain representation of the convolution operation, the target position can be calculated by the following formula:

[0143]

[0144] in, Represents the inverse Fourier transform. The position of the maximum value in the response graph is the position of the flying sphere, and the corresponding flying sphere target bounding box (l r ,t r ,r r ,b r ).

[0145] Step 4: Obtain the state vector; use the depth camera to reconstruct the 3D position, velocity, and acceleration of the target.

[0146] Specifically, the center of mass of the flying sphere is converted from image coordinates to actual three-dimensional coordinates using the camera's intrinsic parameters to obtain the target's position, velocity, and acceleration information, and the angular velocity is obtained through the micro-gyroscope on the sphere.

[0147] Extract the depth data of the object from the depth image, calculate the center of mass of the object based on the average position of the object's pixels, use the camera's intrinsic parameters to convert the center of mass from image coordinates to actual three-dimensional coordinates, continuously sample the object's position data over a period of time, and calculate the speed based on the rate of change of the position:

[0148]

[0149] Calculate the acceleration from the rate of change of velocity:

[0150]

[0151] Among them, r is the position vector of the target, v is the velocity vector of the target, and a is the acceleration vector of the target.

[0152] Step 5: Input the target information into the adaptive Kalman filter, solve the nonlinear motion equation of the flying sphere, output the predicted target position, velocity and acceleration information, estimate and optimize the object's motion trajectory, thereby realizing the position, velocity and acceleration motion modeling and prediction of the flying target. The output predicted trajectory is as follows: Figure 5 shown.

[0153] Update the target trajectory into the discrete motion equation:

[0154]

[0155] The target position, velocity, and acceleration information in step 4 are used as the input of the adaptive Kalman filter, and the initial state vector x0 = [r0, v0] is set. T , where r0 is the initial position, v0 is the initial velocity, the initial control input u0 = a0, and a0 is the initial acceleration. The discrete time step is Δt, and the state transfer matrix F and control matrix B are:

[0156]

[0157] Use the state transfer matrix and control matrix to predict the state and covariance at the next moment and predict the state vector z k+1∣k and the covariance matrix P k+1∣k for

[0158] x k+1∣k =Fx k +Bu k

[0159] P k+1∣k =FP k F T +Q k

[0160] Among them, u k =a k is the control input, Q k is the process noise covariance matrix, initially the unit matrix Q0=I 6×6 , the covariance matrix is ​​initially set to the identity matrix P0 = I 6×6 .

[0161] Combine the observations and Kalman gain to update the state and covariance, and calculate the Kalman gain K k :

[0162] K k =P k+1∣k H T (HP k+1∣k H T +R k ) -1

[0163] Update the state vector x k+1 and the covariance matrix P k+1 :

[0164] y k =z k -Hx k+1∣k

[0165] z k+1 =x k+1∣k +K k y k

[0166] P k+1 =(IK k H)P k+1∣k

[0167] Among them, the observation value is z k , H is the observation matrix. According to the observation residual y k Dynamically adjust the process noise covariance matrix Q k and the observation noise covariance matrix R k , adapt to system changes:

[0168]

[0169] Then, according to the formula listed in step 1, u k+1 To update:

[0170]

[0171] According to the modified Kalman gain K k , observation residual y k To update the predicted subsequent state vector x and control input u:

[0172]

[0173] Assume that the discrete time until the end of the flight state is N, then n = 1, 2, 3 ... N, and the updated initial vector is the state vector

[0174] Step 6: The designed PWC-Net dense optical flow method uses PWC-Net to obtain optical flow information and input it into the KCF tracker to achieve autonomous field of view dynamic maintenance and handle the loss of target field of view caused by occlusion, rapid motion, or target changes. The resulting optical flow information is shown in Figure 6.

[0175] The PWC-Net algorithm solves the problem of autonomous field of view loss. In the target detector of step 1, two images I1 and I2 of the flying sphere target are detected, and CNN is used to extract pyramid features:

[0176]

[0177] in, and Represents the feature map at layer l, L represents the number of pyramid levels, from high resolution to low resolution. At each pyramid level l, the optical flow w estimated by the previous level is used l+1 Features of the current level To make a distortion:

[0178]

[0179] Align the features of the second image to the features of the first image, reducing the displacement difference between the images, where x is the pixel coordinate in the image. After the warp, calculate the cost volume:

[0180]

[0181] Where d is the displacement hypothesis. The matching cost under different displacement hypotheses is expressed by comparing the similarity between the distorted features and the features of the first image. A small convolutional network is used to process the cost volume and refine the optical flow estimate layer by layer. The optical flow estimate can be expressed as:

[0182] w l =ConvNet(C l ,w l+1 )

[0183] Among them, w l is the optical flow estimation at layer l, w l+1 is the optical flow estimation result of the previous layer. The optical flow estimation result is further refined through the context network:

[0184]

[0185] The optical flow information is input into the KCF tracker. When target loss occurs due to occlusion, rapid motion or target change, strategies such as optical flow consistency detection, re-detection mechanism, multi-frame backtracking and multi-feature fusion are used to handle target loss.

[0186] Experimental results show that this method achieves good results in the perception, tracking and prediction of flying spherical targets.

[0187] like Figure 7 As shown, the device of the object motion estimation and tracking method based on adaptive Kalman filtering includes a kinematic equation construction module, a target detection module, a real-time tracking module, a state acquisition module, a prediction module and an optical flow information generation module;

[0188] Kinematic equation construction module, which performs kinematic analysis on objects and constructs the object's motion equation;

[0189] The object detection module uses the FCOS (Fully Convolutional One-stage Object Detector) algorithm to extract and match features of moving objects, and performs bounding box regression and center prediction at each scale to complete the detection of moving objects.

[0190] The real-time tracking module uses the KCF (Kernelized Correlation Filter) tracker to match the target area, uses the target detection result of the moving object as the initial detection boundary, extracts the features of the area, and finds the position with the maximum response as the target moving object to track the moving object in real time.

[0191] The state acquisition module is used to obtain the state vector of the target moving object; it uses the depth camera internal parameters to convert the center of mass of the moving object from image coordinates to actual three-dimensional coordinates, obtains the position, velocity and acceleration information of the target moving object, and obtains the angular velocity through the micro gyroscope on the moving object;

[0192] The prediction module inputs the state vector of the target moving object into the adaptive Kalman filter, solves the motion equation of the moving object, outputs the predicted motion information of the target moving object, and estimates and optimizes the motion trajectory of the object based on the output, thereby achieving the position, velocity and acceleration motion modeling and prediction of the flying target;

[0193] The optical flow information generation module obtains optical flow information through the PWC-Net (Pyramidal, Warping, and Cost Volume Net) dense optical flow method, which is based on feature pyramid extraction, optical flow mapping, and matching correlation cost measurement. The optical flow information is input into the KCF tracker to achieve autonomous dynamic field of view maintenance and handle the problem of target field of view loss caused by occlusion, rapid motion or target change. The obtained optical flow information is used to resolve the loss of moving target objects through optical flow consistency.

[0194] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some or all of the technical features therein. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. Object motion estimation and tracking method based on adaptive Kalman filtering, characterized by The steps include: Step 1: Perform kinematic analysis on the object and construct the object's motion equation; Step 2: Use the target detection algorithm to extract and match the features of the moving object, perform bounding box regression and center prediction at each scale to complete the target detection of the moving object; Step 3: Based on the matching of the tracker in the target area, the target detection result of the moving object is used as the initial detection boundary, the features of the area are extracted, and the position with the largest response is found as the target moving object to be tracked, so as to track the moving object in real time; Step 4: Obtain the state vector of the target moving object; Step 5: Input the state vector of the target moving object into the adaptive Kalman filter, solve the motion equation of the moving object, output the predicted motion information of the target moving object, and estimate and optimize the motion trajectory of the object based on the predicted information; Step 6: Obtain optical flow information through an optical flow method and input it into the tracker to obtain the optical flow information.

2. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, characterized in that: The object motion equation constructed in step 1 is the nonlinear differential equation of the object: F g =mg F m =S(ω×v) Among them, m represents the mass of the object, r represents the position vector of the object, and t represents the time. represents the second-order derivative of the position vector with respect to time, F g Indicates the gravity on the object, F d Indicates the resistance the object receives, F m represents the Magnus force, g represents the gravitational acceleration vector, C d Represents the drag coefficient, ρ represents the density, A represents the obstructed area of ​​the object, v represents the velocity vector of the object, S represents a constant related to the surface roughness and density of the object, ω represents the angular velocity vector of the object, v represents the velocity vector of the object, substitute the above forces into Newton's second law, decompose the acceleration a into differential equations of position and velocity, and obtain the nonlinear differential equation of the moving object: That is, the corresponding motion equation is obtained: a=g+a drg +a magnus and drag =-c d ||v||v a magnus =c m (ω×v) Among them, a drag is the equivalent acceleration of drag, a magnus represents the Magnus effect equivalent acceleration, represents the drag coefficient, represents the Magnus effect coefficient.

3. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, characterized in that: In step 2, the target detection algorithm is used to directly predict the target bounding box at each pixel position of the image. First, the target moving object image and feature extraction are input, and the input image is passed through the pyramid network to extract the feature map F. In the mentioned scenario, only the target bounding box needs to be predicted: (l,t,r,b)=f reg (F) Among them, l represents the left distance, t represents the upper distance, r represents the right distance, b represents the lower distance, and freg represents the convolutional layer of the regression branch.

4. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 3, characterized in that: In step 2, the centrality C is used to measure the distance between the predicted point and the center of the moving object: Among them, min(,) represents the minimum operation, max(,) represents the maximum operation, the higher the centrality, the closer the predicted point is to the target center, and regression prediction is performed on each pixel position of the feature map. Through threshold screening, the predicted bounding boxes with high confidence are retained, and the final detection result is obtained by deduplication.

5. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, characterized in that: In step 3, the tracker goal is to find a filter f such that the input image patch x i The correlation with the detection result y of the target moving object is the largest, and the objective function is expressed as: in, <f,x i > represents the filter f and the input image x i The convolution result, y i represents the detection result of the i-th target moving object, λ represents the regularization parameter, and the tracker uses Fourier transform to convert the convolution operation into the frequency domain, and the objective function is converted to: in, and Respectively represent the filter f and the detection result y i and image patch x i The Fourier transform of the kernel matrix, Represents element-wise multiplication, and obtains the solution of the filter: Choose a Gaussian kernel function: Through Fourier transform, the kernel matrix is ​​expressed in the frequency domain as: Among them, x represents the feature vector of the image patch used for tracking in the current frame, z represents the template feature vector of the target model, σ represents the bandwidth parameter, represents Fourier transform; In each frame, the tracker updates the filter and uses the target position of the current frame and the new image patch to update the model. The update formula is: Among them, η represents the learning rate, represents the filter of the previous frame, The filter representing the current frame is solved by the frequency domain representation of the convolution operation. The position of the target moving object is calculated by the following formula: in, Represents the inverse Fourier transform. The position of the maximum value in the response graph is the position of the moving object, and the corresponding moving object target bounding box is output.

6. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, characterized in that: In step 4, a depth image is acquired and the depth data of the moving object is extracted from it. The center of mass of the object is calculated according to the average position of the pixel points of the object. The center of mass is converted from the image coordinates to the actual three-dimensional coordinates using the intrinsic parameters of the camera. The position data of the object is continuously sampled over a period of time, and the speed is calculated according to the rate of change of the position: Calculate the acceleration from the rate of change of velocity: Among them, r represents the position vector of the target moving object, v represents the velocity vector of the target moving object, a represents the acceleration vector of the target moving object, k represents the current state, and Δt represents the discrete time step.

7. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, characterized in that: In step 5, the trajectory of the target moving object is updated, and its discrete motion equation is as follows: The target motion bitmap position, velocity and acceleration information obtained in step 4 are used as the input of the adaptive Kalman filter, and the initial state vector x0=[r0, v0] is set T , where r0 represents the initial position, v0 represents the initial velocity, the initial control input u0=a0, a0 represents the initial acceleration, the discrete time step is Δt, and the state transfer matrix F and the control matrix B are: Use the state transfer matrix and control matrix to predict the state and covariance at the next moment and predict the state vector x k+1|k and the covariance matrix P k+1|k for: x k+1|k =Fx k +Bu k P k+1|k =FP k F T +Q k Among them, u k =a k represents the control input, Q k is the process noise covariance matrix, initially the unit matrix Q0=I 6×6 , the covariance matrix is ​​initially set to the identity matrix P0 = I 6×6 ; Combine the observations and Kalman gain to update the state and covariance, and calculate the Kalman gain K k : K k =P k+1|k H T (HP k+1|k H T +R k ) -1 Update the state vector x k+1 and the covariance matrix P k+1 : y k =z k -Hx k+1|k x k+1 =x k+1|k +K k y k P k+1 =(I-K k H)P k+1|k Among them, z k represents the observed value, H represents the observation matrix, and according to the observed residual y k Dynamically adjust the process noise covariance matrix Q k and the observation noise covariance matrix R k , adapt to system changes: Among them, α represents the equilibrium parameter; According to the object motion equation in step 1, u k+1 To update: Where g represents the gravitational acceleration vector, c d represents the drag coefficient, v k+1 represents the updated velocity vector of the moving object, S represents a constant related to the surface roughness and density of the object, m represents the mass of the object, ω k+1 Represents the updated angular velocity vector of the moving object; According to the modified Kalman gain K k , observation residual y k To update the predicted subsequent state vector x and control input u: Among them, the updated initial vector is the state vector N represents the discrete time until the motion state ends, n=1, 2, 3...N.

8. The object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, characterized in that: In step 6, CNN is used to extract pyramid features from the two images I1 and I2 of the detected target moving object: in, and Represents the feature map at layer l, L represents the number of pyramid levels, from high resolution to low resolution, at each pyramid level l, the optical flow w estimated by the previous level is used l+1 Features of the current level To make a distortion: Align the features of the second image to the features of the first image, where x represents the pixel coordinates in the image, and after distortion, calculate the cost volume: Among them, d represents the displacement hypothesis. The matching cost under different displacement hypotheses is represented by comparing the similarity between the distorted features and the features of the first image. The cost volume is processed using a convolutional network to refine the optical flow estimation layer by layer. The optical flow estimation is expressed as: w l =ConvNet(C l ,w l+1 ) Among them, w l represents the optical flow estimation at layer l, w l+1 Represents the optical flow estimation result of the previous layer, and further refines the optical flow estimation result through the context network: The optical flow information is input into the tracker, and when target loss occurs, the optical flow consistency strategy is used to handle the target loss situation.

9. A device for object motion estimation and tracking method based on adaptive Kalman filtering according to claim 1, comprising a kinematic equation construction module, a target detection module, a real-time tracking module, a state acquisition module, a prediction module and an optical flow information generation module, characterized in that: The kinematic equation construction module performs kinematic analysis on the object and constructs the motion equation of the object; The target detection module uses the target detection algorithm to extract and match the features of the moving object, and performs bounding box regression and center prediction at each scale to complete the target detection of the moving object; The real-time tracking module matches the target area based on the tracker, uses the target detection result of the moving object as the initial detection boundary, extracts the features of the area, and finds the position with the maximum response as the target moving object to be tracked, so as to track the moving object in real time; The state acquisition module is used to acquire the state vector of the target moving object; The prediction module inputs the state vector of the target moving object into an adaptive Kalman filter, solves the motion equation of the moving object, outputs the predicted motion information of the target moving object, and estimates and optimizes the motion trajectory of the object based on the predicted information; The optical flow information generating module obtains optical flow information through an optical flow method and inputs the optical flow information into the tracker to obtain the optical flow information.

Images