A target tracking filtering method based on nonlinear motion model

Abstract

The present invention discloses a target tracking filtering method based on a nonlinear motion model. The method realizes target feature tracking based on target perception of a binocular vision camera and Kalman filtering. Before tracking and filtering the target features in the binocular camera image, the filter is designed as follows: a system state vector and a system observation vector are selected; based on the assumption that the target moves at a uniform speed relative to the camera, a nonlinear motion model of the target feature points in the image plane is derived, and then the model is linearized to determine the system state transfer matrix; an observation model is derived based on the binocular pinhole observation principle, and the system observation matrix is ​​determined. The depth information of the target feature points is calculated based on the information simultaneously observed by the binocular camera, and then the system observation quantity is determined. The target tracking filter of the present invention is based on a strong nonlinear motion model of the target feature motion in the image plane, thereby improving the position estimation accuracy of target feature tracking.

Description

Technical Field

[0001] The present invention relates to a target tracking technology, and in particular to a target tracking filtering method based on a nonlinear motion model. Background Art

[0002] Environmental object tracking technology is a key component for safe driving in smart cars. It involves identifying and tracking moving objects in the surrounding environment, such as other vehicles, pedestrians, and bicycles. This technology typically relies on multiple sensors and advanced algorithms to ensure that the vehicle can perceive and respond to the dynamic surroundings in real time.

[0003] Road target tracking technology involves sensors, target detection and recognition, tracking algorithms, and data fusion. Sensors include cameras, lidar, millimeter-wave radar, ultrasonic radar, or satellite / inertial combination devices; target detection and recognition technologies include feature detection and deep learning; and tracking algorithms include Kalman filtering, particle filtering, and multi-target tracking.

[0004] In the field of target tracking, Kalman filtering technology is widely used to predict and estimate the position, velocity, and other state information of moving objects, given its reliability and efficiency. In target tracking scenarios, Kalman filtering can effectively handle the uncertainty of target motion and sensor measurement noise. However, for nonlinear systems, standard Kalman filtering is no longer applicable. In these cases, extended Kalman filtering or unscented Kalman filtering can be used.

[0005] Although Kalman filtering and its variants are theoretically powerful, in practice, their accuracy is challenged by challenges such as multi-target tracking association, non-Gaussian noise environments, and system nonlinearity. Especially for rapidly changing or highly nonlinear systems, the accuracy of the filtering model must be even higher. In dynamic motion scenes with sparse visual image features, the commonly used optical flow tracking algorithm suffers from poor accuracy and may even fail to track. This urgently requires a high-precision, efficient target feature tracking method that can be applied in complex scenes.

[0006] To address the nonlinearity issues in target tracking, patent CN114882070A discloses a three-dimensional target motion tracking method based on binocular vision. This technical solution first performs three-dimensional reconstruction based on the parallax of the visual image, obtains the three-dimensional information of the target in the camera coordinate system, and then uses an unscented Kalman filter to predict and update the system state. Because it involves three-dimensional reconstruction, this method is subject to certain limitations in textured environments. In addition, the three-dimensional reconstruction and unscented transformation process to generate sigma points in this method is relatively time-consuming.

[0007] In addressing nonlinear target tracking, Guo Hui et al.'s "Research on Target Tracking Algorithms Based on Nonlinear Filtering" and Cheng Ran et al.'s "Research on Target Tracking Algorithms Based on Nonlinear Filtering" focus on research into filtering theories such as volumetric Kalman filtering and particle filtering, and designing related filtering algorithms. However, these algorithms are relatively time-consuming. Summary of the Invention

[0008] Purpose of the invention: The purpose of the present invention is to address the deficiencies in the prior art and provide a target tracking filtering method based on a nonlinear motion model. By analyzing the projection relationship of three-dimensional target features onto the image plane, a nonlinear motion model is established, so that the filter can well track the nonlinear motion of target feature points in the image plane; and the target feature motion parameters are simultaneously measured in the image plane using the main camera image combined with the auxiliary camera image to calculate the filter observation quantity, thereby realizing high-precision and high-efficiency vision-based target feature tracking in complex scenes.

[0009] Technical solution: The present invention discloses a target tracking filtering method based on a nonlinear motion model. First, a binocular vision camera (including a main camera and an auxiliary camera) is used to acquire a visual image. Then, a tracking filter is performed on the target features in the obtained visual image based on Kalman filtering. Before implementing the tracking filter, the state variables of the target features need to be estimated. The steps include determining a system state vector, determining a system observation vector, determining a system state transfer matrix, determining a system observation matrix, and calculating a system observation quantity. The specific method is as follows;

[0010] Based on the target perception method of the binocular vision camera, the system state vector X at the current time k is determined k ,expression:

[0011]

[0012] x k 、y k 、 are the horizontal position, vertical position, horizontal speed, and vertical speed of the k target feature point in the main camera image coordinate system at the current moment, respectively. are the lateral position and lateral velocity of the k-th target feature point in the auxiliary camera image coordinate system at the current moment, respectively. T Refers to the transpose operation of the matrix;

[0013] Based on the target perception method of the binocular vision camera, the system observation vector Y is determined k , the expression is:

[0014]

[0015] are the reciprocal distance and reduced speed of the k-th target feature point relative to the optical center in the direction of the camera optical axis at the current moment; z k 、 are the distance and speed of target feature point k relative to the optical center in the direction of the main camera’s optical axis at the current moment respectively;

[0016] Based on the assumption that the target moves at a uniform speed relative to the camera, the nonlinear motion model of the target characteristics is derived, and the system state transfer matrix F is determined after linearization. k The calculation equation is:

[0017]

[0018] dt is the interval between the current moment and the next moment,

[0019] At the same time, based on the binocular pinhole observation principle, the observation model is derived to determine the system observation matrix H k The calculation equation is:

[0020]

[0021] f is the focal length of the main (and auxiliary) camera, d is the parallax of the binocular camera, x is k 、 Based on the coordinates of the matching feature points (x k ,y k )and get:

[0022] Observation quantity Y of the observation information calculation system based on binocular camera k .

[0023] After obtaining the above five state variables, continue to execute the Kalman filtering process, including initialization, prediction of target feature points and update of target feature points.

[0024] Furthermore, based on the system state vector X at the current time k k , the formula for predicting the system state vector at the next moment is as follows: k+1 =F k X k +Q k ;

[0025] X k+1 is the system state vector at the next moment k+1, F k is the state transfer matrix, Q k is the process noise matrix.

[0026] Furthermore, based on the above prediction, the system state vector X at the next moment is obtained. k+1 , the system observation vector Y at the current time k k, update to determine the system state vector when a filtering cycle is completed, the basic formula on which the update depends is as follows: k =H k X k +R k ;

[0027] Y k is the system observation vector at the current moment k, H k is the system observation matrix at the current time k, R k is the observation noise matrix.

[0028] Furthermore, The calculation formula is:

[0029]

[0030] x k 、 The coordinates of the matching feature points (x k ,y k )and get;

[0031] It is obtained by applying (existing) optical flow tracking algorithms to the projections of target feature points in the image planes of the primary and auxiliary cameras.

[0032] Furthermore, the observation information calculation system observation quantity Y based on the binocular camera k The formula is:

[0033] Observation

[0034] where x k 、y k 、 The value of is directly adopted from its measured value at the current moment k.

[0035] Beneficial Effects: This paper uses binocular visual perception and observation to construct a nonlinear motion model of target feature points within the image plane and designs a Kalman filtering algorithm for target feature tracking. This algorithm is suitable for target tracking in complex dynamic scenes with highly nonlinear and sparse visual features, and can significantly improve the accuracy and efficiency of target feature tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is the overall flow chart of the present invention;

[0037] Figure 2 This is a geometric model diagram of the pinhole camera described in the present invention;

[0038] Figure 3 This is a geometric relationship diagram of camera imaging according to the present invention;

[0039] Figure 4 The effect diagram of the existing method used in the embodiment

[0040] Figure 5 This is a rendering of the target tracking embodiment of the present invention. DETAILED DESCRIPTION

[0041] The technical solution of the present invention is described in detail below, but the protection scope of the present invention is not limited to the embodiments.

[0042] like Figure 1 As shown, the target tracking filtering method based on the nonlinear motion model of the present invention first uses a binocular vision camera to acquire a visual image, and then tracks and filters the target features in the obtained visual image based on Kalman filtering. Before implementing the tracking filtering, it is necessary to estimate the state variables of the target features. The steps include determining the system state vector, determining the system observation vector, determining the system state transfer matrix, determining the system observation matrix, and calculating the system observation quantity. The specific method is as follows;

[0043] Based on the target perception method of the binocular vision camera, the system state vector X at the current time k is determined k , the expression is as follows:

[0044]

[0045] x k 、y k 、 are the horizontal position, vertical position, horizontal speed, and vertical speed of the k target feature point in the main camera image coordinate system at the current moment, respectively. are the lateral position and lateral velocity of the k-th target feature point in the auxiliary camera image coordinate system at the current moment, respectively. T Refers to the transpose operation of the matrix;

[0046] Based on the target perception method of the binocular vision camera, the system observation vector Y is determined k , the expression is:

[0047]

[0048] are the reciprocal distance and reduced speed of the k-th target feature point relative to the optical center in the direction of the camera optical axis at the current moment; z k 、 are the distance and speed of target feature point k relative to the optical center in the direction of the main camera’s optical axis at the current moment respectively;

[0049] Based on the assumption that the target moves at a uniform speed relative to the camera, the nonlinear motion model of the target characteristics is derived, and the system state transfer matrix F is determined after linearization. k The calculation equation is:

[0050]

[0051] dt is the interval between the current moment and the next moment,

[0052] At the same time, based on the binocular pinhole observation principle, the observation model is derived to determine the system observation matrix H k The calculation equation is as follows:

[0053]

[0054] f is the imaging focal length of the main camera and the auxiliary camera, d is the parallax of the binocular camera, x k 、 The coordinates of the feature points (x k ,y k )and given;

[0055] Observation quantity Y of the observation information calculation system based on binocular camera k .

[0056] In this embodiment, based on the system state vector X at the current time k k , the formula for predicting the system state vector at the next moment is as follows: k+1 =F k X k +Q k ;

[0057] X k+1 is the system state vector at the next moment k+1, F k is the state transfer matrix, Q k is the process noise matrix.

[0058] Based on the above prediction, the system state vector X at the next moment is obtained k+1 , the system observation vector Y at the current time k k , update the system state vector when a filtering cycle is completed, and the update depends on the following formula: k =H k X k +R k ;

[0059] Y k is the system observation vector at the current moment k, H k is the system observation matrix at the current time k, R k is the observation noise matrix.

[0060] Before obtaining the state transfer matrix of the Kalman filter system, the following two constraints are given:

[0061] Constraint 1: The tracked target moves at a constant speed relative to the camera within one state update cycle.

[0062] Constraint 2: The target feature points are projected onto the image plane through camera imaging, and the imaging geometry principle complies with the pinhole model (such as Figure 2 As shown), the similar triangles of the pinhole model in this embodiment are as follows Figure 3 shown.

[0063] The target feature point is P, and the projection points of the target feature point on the image plane of the main camera and the auxiliary camera are: P1' and P'2 respectively. The main camera image plane coordinate system is: O1xy. Among them, O1 is the origin of its image plane coordinate, the origin is located at the center of the image, x and y are as defined above, z is the distance of the target feature point relative to the optical center in the direction of the optical axis of the main (and auxiliary) camera, f is the imaging focal length of the main (and auxiliary) camera, h is the distance from the target feature point to the plane determined by the optical axis of the main camera and the x-axis, l is the distance from the target feature point to the plane determined by the optical axis of the main camera and the y-axis; the auxiliary camera image plane coordinate system is: O2x r y r , where O2 is the origin of the image plane coordinates, the origin is located at the center of the image, x r As defined above, l r The distance between the target feature point and the main camera optical axis and y r The distance between the two axes is the distance to the plane, and d is the parallax of the binocular camera.

[0064] According to the principle of similar triangles, we have:

[0065]

[0066] According to the above known conditions, z, y, l and h are variables with respect to time t, and f is a constant. Taking the derivatives of both sides of equation (2) with respect to time t, we have:

[0067]

[0068] remember have:

[0069]

[0070] Among them, v y As defined above, v z is the velocity of the target feature point relative to the optical center in the direction of the main camera optical axis, v h It is the motion speed of the target feature point relative to the plane determined by the main camera optical axis and the y-axis.

[0071] The current time is t k , the next moment is t k+1 , the state update period is dt, dt = t k+1 -t k , a variable () at t k and t k+1 The values ​​of the moments are expressed as () k and() k+1 .

[0072] According to this definition, equation (2) and equation (4), we have:

[0073]

[0074] Among them, x k 、y k 、 As defined above, z k 、 are the distance of the target feature point relative to the optical center in the direction of the main camera's optical axis at the current moment, and the speed of the target feature point relative to the optical center, h k 、 are the distance and relative speed of the target feature point to the plane determined by the main camera optical axis and x-axis at the current moment; x k+1 、y k+1 、 As defined above, z k+1 、 are the distance of the target feature point relative to the optical center in the direction of the main camera's optical axis and the speed of movement relative to the optical center at the next moment, h k+1 、 They are the distance and relative speed from the target feature point to the plane determined by the main camera optical axis and the x-axis at the next moment, respectively.

[0075] In addition, according to the above known conditions, we have:

[0076]

[0077] Substituting equations (5)-(8) into equation (9), we have:

[0078]

[0079] Eliminate the quantity h in equation (10) k+1 、h k and v z k+1 ,have:

[0080]

[0081] Eliminate the quantity z in equation (11) k+1、f,there are:

[0082]

[0083] After rearranging equation (12), we get:

[0084]

[0085] Further sorting, we can get:

[0086]

[0087] If dt is small, then k and v z k Under certain conditions, dz is a small amount. Because the function f(u)=(1-u) -1 In the zero domain of the independent variable u, it can be expanded as:

[0088] f(u)=(1-u) -1 =1+u+u 2 +… (15) Then, equation (14) can be approximately expressed as:

[0089]

[0090] Similarly, by defining symbols, we can obtain the motion relationship of the projection point in the x direction in the image plane:

[0091]

[0092] The approximate expression of the above formula when dz is small is:

[0093]

[0094] According to equations (9), (16) and (18), combined with the definition of the state vector, the specific expression of the system state equation is given:

[0095]

[0096] Similarly, by defining symbols, we can obtain the motion relationship of the projection point in the x direction in the auxiliary camera image plane:

[0097]

[0098] The approximate expression of the above formula when dz is small is:

[0099]

[0100] From equations (19) and (21), the system state transfer matrix F is given k , specifically:

[0101]

[0102] System state transfer matrix F k The corresponding elements in are calculated as follows:

[0103]

[0104] Among them, x k 、 z k 、 As defined above, are the distance of the target feature point relative to the optical center in the direction of the auxiliary camera's optical axis and the speed of movement relative to the optical center at the current moment, respectively. k 、 are the distance and relative speed from the target feature point to the plane determined by the main camera optical axis and y-axis at the current moment, respectively. They are respectively the current target feature point to the auxiliary camera optical axis and y r The distance and relative speed of the plane determined by the axis.

[0105] From equations (23), (25) and (27), we can get:

[0106]

[0107] Substituting equation (28) back into equation (23), we get

[0108]

[0109] Furthermore, from equations (23), (24), (26) and (27), we can obtain:

[0110]

[0111] Substituting equations (30) and (23) back into equation (24), we get

[0112]

[0113] Arrange, get

[0114]

[0115] State transition matrix F k in Calculate according to equation (32), where x k 、 The coordinates of the matching feature points (x k ,y k )and given; among them It is obtained by applying (existing) optical flow tracking algorithms to the projections of target feature points in the image planes of the primary and auxiliary cameras.

[0116] This embodiment further determines the observation vector according to equations (29) and (32): The observations in are as follows:

[0117]

[0118]

[0119] Thus, the observation matrix H is given k , specifically:

[0120]

[0121] where x k 、 The coordinates of the feature points (x k ,y k )and given.

[0122] The noise matrix Q of the above process k , observation noise matrix R k And other relevant parameters in the Kalman filtering process are calculated using existing technologies.

[0123] The effect of this embodiment is as follows Figure 4 As shown; the target tracking filtering method based on the nonlinear motion model proposed by the present invention is used to track the target, and the effect of the embodiment is as follows Figure 5 Compare the two Figure 4 and Figure 5 It can be seen that the tracking effect of the present invention is better.

[0124] In summary, the present invention adopts a target perception method based on a binocular vision camera and a target feature tracking method based on Kalman filtering to track and filter target features in binocular camera images; the filter design process includes: first, designing and selecting a system state vector, which includes the position and velocity of the target feature point in the image coordinate system of the main camera and the auxiliary camera; second, considering the simultaneous observation of the binocular cameras, designing and selecting a system observation vector, which includes the depth information of the target feature point; third, based on the assumption that the target moves at a uniform speed relative to the camera, deriving a nonlinear motion model of the target feature point in the image plane, and then implementing model linearization to determine the system state transfer matrix; finally, based on the binocular pinhole observation principle, deriving an observation model and determining the system observation matrix, and calculating the depth information of the target feature point based on the information simultaneously observed by the binocular cameras, and then determining the system observation quantity. The target tracking filter in the present invention is based on a strong nonlinear motion model of the target feature motion in the image plane, thereby improving the position estimation accuracy of target feature tracking.

Claims

1. A target tracking filtering method based on a nonlinear motion model, which first uses a binocular visual camera to obtain a visual image, and then tracks and filters the target features in the obtained visual image based on Kalman filtering, characterized in that: Before implementing tracking filtering, it is necessary to estimate the state variables that determine the target characteristics. The steps include determining the system state vector, determining the system observation vector, determining the system state transfer matrix, determining the system observation matrix, and calculating the system observation quantity. The specific method is as follows; Based on the target perception method of the binocular vision camera, the system state vector X at the current time k is determined k , the expression is as follows: x k 、y k 、 are the horizontal position, vertical position, horizontal speed, and vertical speed of the k target feature point in the main camera image coordinate system at the current moment, respectively. are the lateral position and lateral velocity of the k-th target feature point in the auxiliary camera image coordinate system at the current moment, respectively. T Refers to the transpose operation of the matrix; Based on the target perception method of the binocular vision camera, the system observation vector Y is determined k , the expression is: are the reciprocal distance and reduced speed of the k-th target feature point relative to the optical center in the direction of the camera optical axis at the current moment; z k 、 are the distance and speed of target feature point k relative to the optical center in the direction of the main camera’s optical axis at the current moment respectively; Based on the assumption that the target moves at a uniform speed relative to the camera, the nonlinear motion model of the target characteristics is derived, and the system state transfer matrix F is determined after linearization. k The calculation equation is: dt is the interval between the current moment and the next moment; The calculation formula is: is the lateral velocity of the k target feature point in the main camera image coordinate system at the current moment; At the same time, based on the binocular pinhole observation principle, the observation model is derived to determine the system observation matrix H k The calculation equation is as follows: f is the imaging focal length of the main camera and the auxiliary camera, d is the parallax of the binocular camera, x k 、 Based on the coordinates of the matching feature points (x k ,y k )and get; Observation quantity Y of the observation information calculation system based on binocular camera k , the calculation formula is: Observation where x k 、y k 、 The value of is directly adopted from its measured value at the current moment k.

2. The target tracking filtering method based on nonlinear motion model according to claim 1, characterized in that: Based on the system state vector X at the current time k k , the formula for predicting the system state vector at the next moment is as follows: k+1 =F k X k +Q k ; X k+1 is the system state vector at the next moment k+1, F k is the state transfer matrix, Q k is the process noise matrix.

3. The target tracking filtering method based on nonlinear motion model according to claim 2, characterized in that: Based on the predicted system state vector X at the next moment k+1 , the system observation vector Y at the current time k k , update the system state vector when a filtering cycle is completed, and the update depends on the following formula: k =H k X k +R k ; Y k is the system observation vector at the current moment k, H k is the system observation matrix at the current time k, R k is the observation noise matrix.

Images