Target tracking method and system based on third-order Kalman filtering algorithm

Abstract

The invention discloses a target tracking method and system based on a third-order Kalman filtering algorithm, and the method comprises the following steps: obtaining the angular velocity information of a pan-tilt, and controlling a pan-tilt motor to act according to the angular velocity information, so as to track a target; and in the tracking process, when the target is lost, angular velocity information of the holder is predicted based on third-order Kalman filtering, and a holder motor is controlled according to a predicted value of the angular velocity information of the holder. According to the method, tracking trajectory prediction is carried out by adopting three pieces of angular velocity information, namely the angular velocity, the angular acceleration and the angular acceleration change rate, so that prediction of the motion trajectory by the three-order linear Kalman filtering algorithm is closer to the actual motion trajectory of the target, and the robustness of target tracking in a complex environment can be greatly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of machine vision, and particularly to an object tracking method and system based on a third-order Kalman filtering algorithm. Background Art

[0002] The function of tracking the target on a pan-tilt has important application values in many fields such as military strikes, intelligent security, UAV navigation, and robot control. The loss of the target during the tracking process, as a common problem of this function, seriously affects the tracking performance and robustness. Therefore, how to accurately and effectively predict the trajectory of the target after the loss of the tracked target is an urgent problem to be solved.

[0003] Therefore, in the prior art, the target trajectory is mainly predicted and tracked by recording the angular velocity control amount at the moment when the target is lost. However, the robustness of the predicted trajectory during tracking in a complex environment is poor with this method, and it is only applicable to the motion scenario where the target motion trajectory is a uniform straight line. The prediction effect for a variable acceleration motion scenario is poor, which further affects the re-capture effect of the target. Summary of the Invention

[0004] Aiming at the deficiencies of the prior art, the present invention provides an object tracking method and system based on a third-order Kalman filtering algorithm. It uses three angular velocity information, namely angular velocity, angular acceleration, and angular acceleration change rate, for tracking trajectory prediction, making the prediction of the third-order linear Kalman filtering algorithm for the motion trajectory closer to the actual motion trajectory of the target, and can greatly improve the robustness during object tracking in a complex environment.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] On the one hand, it provides an object tracking method based on a third-order Kalman filtering algorithm, which includes the following steps:

[0007] Obtain the angular velocity information of the pan-tilt, and control the action of the pan-tilt motor according to the angular velocity information to track the target;

[0008] And during the tracking process, when the target is lost, predict the angular velocity information of the pan-tilt based on the third-order Kalman filtering, and control the pan-tilt motor according to the predicted value of the pan-tilt angular velocity information.

[0009] Preferably, the angular velocity information includes one or several of the pan-tilt angular velocity, angular acceleration, and angular acceleration change rate.

[0010] Preferably, the angular velocity information includes: the pitch axis angular velocity spd on the pitch axis P P , the azimuth axis angular velocity spd on the azimuth axis Y Y , the pitch axis angular acceleration acc P, azimuth axis angular acceleration acc Y , pitch axis angular acceleration change rate dacc P , azimuth axis angular acceleration change rate dacc Y .

[0011] Preferably, the angular velocity information of the pan-tilt is predicted based on the third-order Kalman filter, and the pan-tilt motor is controlled according to the predicted value of the pan-tilt angular velocity information, which includes the following steps:

[0012] Obtain the prior estimate of the system state and the prior estimate of the error covariance at time k, and the prior estimate of the system state is obtained based on the following formula:

[0013]

[0014] where, take as the system state; is the prior estimate of the system state at time k; x k-1|k-1 is the posterior estimate of the system state at time k-1; u k is the input state value; and A is the system state transition matrix; B is the input state transition matrix; spd Pk , spd Yk , acc Pk , acc Yk , dacc Pk , dacc Yk are respectively the average pitch axis angular velocity, average azimuth axis angular velocity, average pitch axis angular acceleration, average azimuth axis angular acceleration, average pitch axis angular acceleration change rate and average azimuth axis angular acceleration change rate within the first n seconds before time k;

[0015] Update the system state and error covariance according to the following formula to obtain the predicted value of the pan-tilt angular velocity information:

[0016]

[0017] P k|k =(I-K k H k )P k|k-1

[0018] where, x k|k is the posterior estimate of the system state at time k, z k is the system measurement state; H is the measurement state transition matrix, I is the second-order identity matrix; P k|k-1 is the prior estimate of the error covariance at time k, P k|k is the posterior estimate of the error covariance at time k;

[0019] Control the pan-tilt motor according to the predicted value of the pan-tilt angular velocity information.

[0020] Preferably, wherein, T s is the sampling time of the third-order Kalman filter.

[0021] Preferably, wherein, T s is the sampling time of the third-order Kalman filter.

[0022] Preferably, the target tracking method further includes: determining whether the target is recaptured or whether the prediction duration of the pan-tilt angular velocity exceeds a threshold. If the target has been recaptured or the prediction duration of the pan-tilt angular velocity has exceeded the threshold, end the target tracking; otherwise, if the target has not been recaptured or the prediction duration of the pan-tilt angular velocity has not exceeded the threshold, continue the target tracking.

[0023] Preferably, controlling the pan-tilt motor according to the predicted value of the angular velocity information to track the target includes the following steps:

[0024] Obtain the predicted value spd of the pitch-axis angular velocity according to the following formula Pt0 , the predicted value spd of the azimuth-axis angular velocity Yt0 , both of which can be obtained based on the miss distance. The specific calculation formula is as follows:

[0025] spd = k s *vision

[0026] wherein, vision is the miss distance at the current moment t; k s is a coefficient; spd is the predicted value of the angular velocity;

[0027] Obtain the pitch-axis angular velocity spd Pt , the azimuth-axis angular velocity spd Yt at the current moment t in real time according to the angular velocity sensor, and correspondingly obtain the pitch-axis angular velocity difference between the predicted value spd Pt0 of the pitch-axis angular velocity and the pitch-axis angular velocity spd Pt , and the azimuth-axis angular velocity difference between the predicted value spd Yt0 of the azimuth-axis angular velocity and the azimuth-axis angular velocity spd Yt ;

[0028] Control the pan-tilt motor according to the pitch-axis angular velocity difference and / or the azimuth-axis angular velocity difference to achieve target tracking.

[0029] Preferably, the miss distance is the distance between the target in the image and the center of the image.

[0030] On the other hand, a target tracking system is also provided, which includes:

[0031] Pan-tilt head;

[0032] An imaging device, connected to the pan-tilt head, for acquiring a target image;

[0033] An angular velocity sensor, which is used to respectively and real-time acquire the pitch axis angular velocity spd on the pitch axis P of the pan-tilt head at an mHz sampling rate P and the azimuth axis angular velocity spd on the azimuth axis Y Y ;

[0034] An angular velocity information acquisition unit, which is used to acquire angular velocity information according to the angular velocity of the pan-tilt head;

[0035] A Kalman filter, which is used to predict the angular velocity information of the pan-tilt head based on third-order Kalman filtering and obtain a predicted value of the angular velocity information when the target is lost during the process of tracking the target;

[0036] And a control unit, which is used to control the operation of the pan-tilt head motor according to the miss distance.

[0037] The present invention predicts the tracking trajectory through three angular velocity information, namely angular velocity, angular acceleration, and angular acceleration change rate. It can accurately acquire target motion information from multiple dimensions, so that the prediction of the third-order linear Kalman filter algorithm for the motion trajectory is closer to the actual motion trajectory of the target, effectively improving the ability to re-capture the target, being applicable to complex scenarios such as variable acceleration curve motion, and greatly enhancing the robustness during target tracking in complex environments. Brief Description of the Drawings

[0038] Figure 1 is a flowchart of the steps of the target tracking method based on the third-order Kalman filter algorithm in the present invention;

[0039] Figure 2 is a schematic diagram of the target tracking effect based on the third-order Kalman filter algorithm in the present invention;

[0040] Figure 3 is a schematic diagram of the structure of the target tracking system based on the third-order Kalman filter algorithm in the present invention. Detailed Embodiments

[0041] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. 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.

[0042] Embodiment 1:

[0043] AsFigure 1-2 As shown in the figure, this embodiment provides a target tracking method based on a third-order Kalman filtering algorithm, which includes the following steps:

[0044] S1. Obtain a target image, and calculate the distance between the target in the image and the center of the image in real time based on the target image as the off-target amount.

[0045] In this embodiment, the target detection unit located on the pan-tilt head is used to identify the target and collect the target image. Specifically, the target detection unit mainly includes an imaging device (such as a visible light camera, etc.) provided on the pan-tilt head or the pod and a visual detection module with a built-in visual detection algorithm. The visual detection algorithm is a prior art and will not be elaborated here.

[0046] S2. Obtain the angular velocity information of the pan-tilt head in real time, and control the pan-tilt head motor to act according to the angular velocity information to track the target. During the tracking process, when the target is lost, step S3 is executed.

[0047] In this embodiment, the angular velocity information includes one or several of the pan-tilt head angular velocity, angular acceleration, and angular acceleration change rate, and is obtained through the following steps:

[0048] S21. The angular velocity sensor respectively obtains the pitch axis angular velocity spd on the pitch axis P of the pan-tilt head P , the azimuth axis angular velocity spd on the azimuth axis Y Y at a sampling rate of tHz in real time, and respectively calculate the pitch axis angular acceleration acc P between two adjacent pitch axis angular velocity sampling moments, the azimuth axis angular acceleration acc Y between two adjacent azimuth axis angular velocity sampling moments, and respectively calculate the pitch axis angular acceleration change rate dacc P between two adjacent pitch axis angular velocity sampling moments, the azimuth axis angular acceleration change rate dacc Y between two adjacent azimuth axis angular velocity sampling moments;

[0049] Specifically, the pitch axis angular acceleration acc P , the azimuth axis angular acceleration acc Y , the pitch axis angular acceleration change rate dacc P , the azimuth axis angular acceleration change rate dacc Y are respectively obtained through the following formulas:

[0050]

[0051] where T is the time interval between two adjacent sampling times (i.e., the i-th time and the (i + 1)-th time), and T = 1 / t, with the unit of seconds (s); and the above parameters can be obtained by processing the sampling data through a moving average filter;

[0052] S22. Use modules such as a moving average filter with 50 Hz and 60 window numbers to calculate the average pitch-axis angular velocity spd within the previous n seconds at the current moment m Pm , the average azimuth-axis angular velocity spd Ym , the average pitch-axis angular acceleration acc Pm , the average azimuth-axis angular acceleration acc Ym , the average pitch-axis angular acceleration change rate dacc Pm and the average azimuth-axis angular acceleration change rate dacc Ym ;

[0053] S3. Predict the angular velocity information of the pan-tilt based on a third-order Kalman filter, and control the pan-tilt motor according to the predicted value of the pan-tilt angular velocity information, which specifically includes the following steps:

[0054] Obtain the prior estimate of the system state and the prior estimate of the error covariance at the k-th moment;

[0055] Specifically, the prior estimate of the system state is obtained based on the following formula:

[0056]

[0057] where, take as the system state; is the prior estimate of the system state at the k-th moment; x k-1|k-1 is the posterior estimate of the system state at the (k - 1)-th moment; u k is the input state value; and A is the system state transition matrix, which can be expressed as B is the input state transition matrix, which can be expressed as T s is the sampling time of the third-order Kalman filter; spd Pk 、spd Yk 、acc Pk 、acc Yk 、dacc Pk 、dacc Yk are respectively the average pitch-axis angular velocity, average azimuth-axis angular velocity, average pitch-axis angular acceleration, average azimuth-axis angular acceleration, average pitch-axis angular acceleration change rate, and average azimuth-axis angular acceleration change rate within the previous n seconds at the k-th moment;

[0058] The prior estimate of the error covariance is obtained based on the following formula:

[0059] P k|k-1 = AP k-1|k-1 A + Q

[0060] Among them, P k|k-1 is the prior estimated value of the error covariance at time k; P k-1|k-1 is the posterior estimated value of the error covariance at time k - 1; Q is the system prediction process noise, which can be set according to the system characteristics;

[0061] Update the Kalman gain according to the following formula:

[0062] S k = HP k|k-1 H T + R

[0063]

[0064] Among them, H is the measurement state transition matrix; R is the system prediction process noise, which can be set according to the system characteristics; S k is the intermediate variable matrix, K k is the Kalman gain matrix at time k; the superscript T represents the matrix transpose;

[0065] And update the system state and error covariance according to the following formula to obtain the predicted value of the pan-tilt angular velocity information:

[0066]

[0067] P k|k = (I - K k H k )P k|k-1

[0068] Among them, x k|k is the posterior estimated value of the system state at time k (i.e., the predicted value of the pan-tilt angular velocity information), z k is the system measurement state; H is the measurement state transition matrix, I is the second-order identity matrix; P k|k-1 is the prior estimated value of the error covariance at time k, P k|k is the posterior estimated value of the error covariance at time k;

[0069] Control the pan-tilt motor action according to the predicted value of the pan-tilt angular velocity information to achieve the tracking of the target;

[0070] S4. Determine whether the target has been recaptured or the prediction duration of the pan-tilt angular velocity has exceeded the threshold (such as 3s). If the target has been recaptured or the prediction duration of the pan-tilt angular velocity has exceeded the threshold, end the target tracking; otherwise, if the target has not been recaptured or the prediction duration of the pan-tilt angular velocity has not exceeded the threshold, return to step S2 and continue the target tracking.

[0071] Therefore, if Figure 2 As shown, in the prior art, when only angular velocity is used to predict the tracking trajectory, only a straight line tracking prediction trajectory can be obtained, which has a large deviation from the actual motion trajectory of the target and cannot achieve accurate recapture of the target. In this embodiment, three angular velocity information, namely angular velocity, angular acceleration, and angular acceleration change rate, are used to predict the tracking trajectory, which can accurately obtain the target motion information from multiple dimensions, so that the third-order linear Kalman filter algorithm predicts the motion trajectory closer to the actual motion trajectory of the target, which can effectively improve the target recapture capability, is suitable for complex scenes such as variable acceleration curve motion, and greatly improves the robustness of target tracking in complex environments.

[0072] Embodiment 2:

[0073] The difference between this embodiment and embodiment 1 is that, in step S2, the pan / tilt motor is controlled to move according to the predicted value of the angular velocity information to track the target, and the steps include:

[0074] According to the following formula, the predicted value of the pitch axis angular velocity spd is obtained. Pt0 、Azimuth axis angular velocity prediction value spd Yt0 , can be obtained based on the off-target amount, and the specific calculation formula is as follows:

[0075] spd=k s *vision

[0076] Among them, vision is the miss distance at the current time t; k s is a coefficient, which can be set according to the performance parameters of the gimbal; spd is the angular velocity prediction value (i.e. the angular velocity prediction value of the pitch axis and the angular velocity prediction value of the azimuth axis);

[0077] According to the angular velocity sensor, the pitch axis angular velocity spd at the current time t is obtained in real time Pt 、Azimuth axis angular velocity spd Yt , and the corresponding pitch axis angular velocity prediction value spd is obtained Pt0 、Pitch axis angular velocity spd Pt The difference in angular velocity between the pitch axis and the predicted angular velocity of the azimuth axis spd Yt0 、Azimuth axis angular velocity spd Yt The azimuth axis angular velocity difference between

[0078] And the action of the gimbal motor is controlled according to the angular velocity difference of the pitch axis and / or the angular velocity difference of the azimuth axis to achieve target tracking.

[0079] Embodiment 3:

[0080] This embodiment provides a target tracking system for implementing the target tracking method described in Embodiment 1 or 2, as follows Figure 3 shown, which includes:

[0081] A pan-tilt 1, which can be mounted on unmanned devices such as drones;

[0082] An imaging device 2, which is connected to the pan-tilt 1 and is used to obtain target images;

[0083] A miss distance calculation unit 3, which is used to calculate the distance between the target in the image and the center of the image in real time based on the target image as the miss distance;

[0084] An angular velocity sensor 4, which is mounted on unmanned devices such as drones and is used to obtain the pitch axis angular velocity spd on the pitch axis P of the pan-tilt in real time at a sampling rate of mHz P and the azimuth axis angular velocity spd on the azimuth axis Y Y ;

[0085] An angular velocity information acquisition unit 5, which is used to obtain angular velocity information according to the angular velocity of the pan-tilt, and the process is the same as that in step S2;

[0086] A Kalman filter 6, which is used to predict the angular velocity information of the pan-tilt based on the third-order Kalman filter and obtain the predicted value of the angular velocity information when the target is lost during the process of tracking the target, and the process is the same as that in step S3;

[0087] And a control unit 7, which is used to control the operation of the pan-tilt motor according to the miss distance, and the process is the same as that in Embodiment 2, or control the operation of the pan-tilt motor according to the predicted value of the pan-tilt angular velocity information.

[0088] In summary, the present invention uses three angular velocity information, namely angular velocity, angular acceleration, and angular acceleration change rate, for tracking trajectory prediction, which can accurately obtain target motion information from multiple dimensions, so that the prediction of the third-order linear Kalman filter algorithm for the motion trajectory is closer to the actual motion trajectory of the target, can effectively improve the target recapture ability, is applicable to complex scenarios such as variable acceleration curve motion, and greatly improves the robustness when tracking targets in complex environments.

[0089] It should be noted that the technical features in the above Embodiments 1-3 can be combined arbitrarily, and the technical solutions formed by the combination all fall within the protection scope of the present application. In this text, terms such as "including", "comprising" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "including one..." does not exclude the existence of additional identical elements in the process, method, article or device including the said element.

[0090] Although the embodiments of the present invention have been shown and described, for those of ordinary skill in the art, it can be understood that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A target tracking method based on a third-order Kalman filter algorithm, characterized in that: The steps include: Acquiring angular velocity information of the gimbal, and controlling the gimbal motor movement according to the angular velocity information to track the target; In the tracking process, when the target is lost, the angular velocity information of the gimbal is predicted based on the third-order Kalman filter, and the gimbal motor is controlled according to the predicted value of the gimbal angular velocity information.

2. The target tracking method according to claim 1, characterized in that: The angular velocity information includes one or more of the gimbal angular velocity, angular acceleration, and angular acceleration change rate.

3. The target tracking method according to claim 1, characterized in that: The angular velocity information includes: the pitch axis angular velocity spd on the pitch axis P P 、Azimuth axis angular velocity spd on azimuth axis Y Y 、Pitch axis angular acceleration acc P 、Azimuth axis angular acceleration acc Y , pitch axis angular acceleration change rate dacc P 、Azimuth axis angular acceleration change rate dacc Y .

4. The target tracking method according to claim 1, characterized in that: The angular velocity information of the gimbal is predicted based on the third-order Kalman filter, and the gimbal motor is controlled according to the predicted value of the angular velocity information of the gimbal, which includes the following steps: Obtain a priori estimate of the system state and a priori estimate of the error covariance at time k, and the a priori estimate of the system state is obtained based on the following formula: Among them, take is the system status; is the prior estimate of the system state at time k; x k-1|k -1 is the a posteriori estimate of the system state at time k-1; u k is the input state value; and A is the system state transfer matrix; B is the input state transfer matrix; spd Pk 、spd Yk ,acc Pk ,acc Yk 、dacc Pk 、dacc Yk are the average pitch axis angular velocity, average azimuth axis angular velocity, average pitch axis angular acceleration, average azimuth axis angular acceleration, average pitch axis angular acceleration change rate and average azimuth axis angular acceleration change rate in n seconds before time k respectively; The system state and error covariance are updated according to the following formula to obtain the predicted value of the gimbal angular velocity information: P k|k =(I-K k H k )P k|k-1 Among them, x k|k is the a posteriori estimate of the system state at time k, z k is the system measurement state; H is the measurement state transfer matrix, I is the second-order unit matrix; P k|k-1 is the prior estimate of the error covariance at time k, P k|k is the posterior estimate of the error covariance at time k; The gimbal motor movement is controlled according to the predicted value of the gimbal angular velocity information.

5. The target tracking method according to claim 4, characterized in that: Among them, T s is the sampling time of the third-order Kalman filter.

6. The target tracking method according to claim 4, characterized in that: Among them, T s is the sampling time of the third-order Kalman filter.

7. The target tracking method according to claim 1, characterized in that: The target tracking method also includes: determining whether the target has been recaptured or whether the predicted duration of the gimbal angular velocity exceeds a threshold value; if the target has been recaptured or the predicted duration of the gimbal angular velocity has exceeded the threshold value, then terminating the target tracking; otherwise, if the target has not been recaptured or the predicted duration of the gimbal angular velocity has not exceeded the threshold value, then continuing the target tracking.

8. The target tracking method according to claim 1, characterized in that: Controlling the pan / tilt motor action according to the predicted value of the angular velocity information to track the target includes the following steps: According to the following formula, the predicted value of the pitch axis angular velocity spd is obtained. Pt0 、Azimuth axis angular velocity prediction value spd Yt0 , can be obtained based on the off-target amount, and the specific calculation formula is as follows: spd=k s *vision Among them, vision is the miss distance at the current time t; k s is the coefficient; spd is the predicted value of angular velocity; According to the angular velocity sensor, the pitch axis angular velocity spd at the current time t is obtained in real time Pt 、Azimuth axis angular velocity spd Yt , and the corresponding pitch axis angular velocity prediction value spd is obtained Pt0 、Pitch axis angular velocity spd Pt The difference in angular velocity between the pitch axis and the predicted angular velocity of the azimuth axis spd Yt0 、Azimuth axis angular velocity spd Yt The azimuth axis angular velocity difference between The action of the gimbal motor is controlled according to the angular velocity difference of the pitch axis and / or the angular velocity difference of the azimuth axis to achieve target tracking.

9. The target tracking method according to claim 8, characterized in that: The miss distance is the distance between the target in the image and the center of the image.

10. A target tracking system, characterized in that: include: PTZ; An imaging device connected to the pan / tilt platform for acquiring a target image; The angular velocity sensor is used to obtain the pitch axis angular velocity spd on the pitch axis P of the gimbal in real time at a mHz sampling rate. P 、Azimuth axis angular velocity spd on azimuth axis Y Y ; An angular velocity information acquisition unit, which is used to acquire angular velocity information according to the angular velocity of the gimbal; A Kalman filter is used to predict the angular velocity information of the gimbal based on a third-order Kalman filter when the target is lost during the target tracking process, and obtain the predicted value of the angular velocity information; And a control unit, which is used to control the action of the gimbal motor according to the miss amount.

Images