Target tracking filtering method based on nonlinear motion model

Abstract

The invention discloses a target tracking filtering method based on a nonlinear motion model. Target feature tracking is realized based on target perception of a binocular vision camera and Kalman filtering. Before tracking filtering is carried out on target features in a binocular camera image, a filter is designed as follows: a system state vector is selected, and a system observation vector is selected; deducing a nonlinear motion model of a target feature point in an image plane based on the assumption that a target does uniform motion relative to a camera, then implementing model linearization, and determining a system state transition matrix; an observation model is deduced based on a binocular pinhole observation principle, a system observation matrix is determined, depth information of target feature points is calculated based on information observed by a binocular camera at the same time, and then system observation quantity is determined. The target tracking filter is based on a strong nonlinear motion model of target feature motion in an image plane, so that the position estimation precision of target feature tracking is improved.

Description

Technical Field

[0001] The 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 on the road where smart cars are driving is one of the key components to achieve safe vehicle driving. It involves identifying and tracking other vehicles, pedestrians, bicycles and other moving objects in the surrounding environment. This technology usually relies on multiple sensors and advanced algorithms to ensure that the vehicle can perceive and respond to the dynamic situation around it in real time.

[0003] Road environment target tracking technology involves sensors, target detection and recognition, tracking algorithms, data fusion and other technologies. Among them, sensors involve cameras, laser radars, millimeter wave radars, ultrasonic radars or satellite / inertial combination devices, etc.; target detection and recognition technology involves feature detection, deep learning and other technologies; tracking algorithms involve Kalman filtering, particle filtering, multi-target tracking and other algorithms.

[0004] In the field of target tracking, considering the reliability and efficiency of target tracking, Kalman filtering technology is widely used to predict and estimate the position, speed and other state information of moving objects. In the target tracking scenario, Kalman filtering can effectively deal with the uncertainty of target motion and the noise of sensor measurement. However, for nonlinear systems, standard Kalman filtering is no longer applicable. In this case, extended Kalman filtering or unscented Kalman filtering can be used.

[0005] Although Kalman filtering and its variant algorithms are very powerful in theory, in practical applications, the accuracy of filtering is challenged when faced with some problems such as multi-target tracking association, non-Gaussian noise environment, and system nonlinearity. Especially for rapidly changing or highly nonlinear systems, the accuracy of the filtering model is required to be higher. In dynamic motion scenes with sparse visual image features, the commonly used optical flow tracking algorithm has poor accuracy and even tracking failures. This urgently requires a high-precision and high-efficiency target feature tracking method that can be applied to complex scenes.

[0006] In response to the nonlinear problem 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 unscented Kalman filtering to predict and update the system state. Because it involves three-dimensional reconstruction, this method will be subject to certain limitations in textured environments. In addition, the process of three-dimensional reconstruction and unscented transformation to generate sigma points in this method is relatively time-consuming.

[0007] In dealing with the problem of nonlinear target tracking, Guo Hui et al.'s "Guo Hui, Research on Target Tracking Algorithm Based on Nonlinear Filtering" and Cheng Ran et al.'s "Research on Target Tracking Algorithm Based on Nonlinear Filtering" mainly focus on the research of filtering theories such as volumetric Kalman filtering and particle filtering, and design 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 solve the deficiencies in the prior art and to 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 filter observations are calculated by simultaneously measuring the target feature motion parameters in the image plane using the main camera image in combination with the auxiliary camera image, thereby achieving high-precision and high-efficiency vision-based target feature tracking in complex scenes.

[0009] Technical solution: A target tracking filtering method based on a nonlinear motion model of the present invention first uses a binocular vision camera (including a main camera and an auxiliary camera) to obtain 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 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, determine the system state vector X at the current time k 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 target feature point in the auxiliary camera image coordinate system at the current moment, respectively. T It refers to the transposition operation of the matrix;

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

[0014]

[0015] z k 、vz k 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 of the k target feature point relative to the optical center in the direction of the main camera optical axis and the moving speed relative to the optical center 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 imaging 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 in the binocular camera image (x k ,y k )and get:

[0022] Observation quantity Y of 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 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 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 time k, H k is the system observation matrix at the current time k, R k is the observation noise matrix.

[0028] Further, The calculation formula is:

[0029]

[0030] x k , The coordinates of the matching feature points in the binocular camera image (x k ,y k )and get;

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

[0032] Furthermore, the observation information of the binocular camera is used to calculate the observed quantity Y k The formula is:

[0033] Observable

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

[0035] Beneficial effects: The present invention constructs a nonlinear motion model of target feature points in the image plane through binocular visual perception and observation, and designs a target feature tracking Kalman filter algorithm. This algorithm is suitable for target tracking in complex dynamic scenes with high nonlinearity 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 This is a rendering of the effect of using the existing method in the embodiment; Figure 5 This is a rendering of the target tracking embodiment of the present invention. DETAILED DESCRIPTION

[0040] 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.

[0041] 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 obtain 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;

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

[0043]

[0044] 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 target feature point in the auxiliary camera image coordinate system at the current moment, respectively. T It refers to the transposition operation of the matrix;

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

[0046]

[0047] 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 of the k target feature point relative to the optical center in the direction of the main camera optical axis and the moving speed relative to the optical center at the current moment respectively;

[0048] 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:

[0049]

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

[0051] 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:

[0052]

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

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

[0055] 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 ;

[0056] 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.

[0057] Based on the above prediction, the 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 formula on which the update depends is as follows: k =H k X k +R k ;

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

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

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

[0061] 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), similar triangles of the pinhole model of this embodiment are as follows Figure 3 shown.

[0062] 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 P2' respectively. The image plane coordinate system of the main camera is O1xy. Among them, O1 is the origin of its image plane coordinate, and the origin is located at the center of the image. x and y are defined as 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, and 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 image plane coordinate system of the auxiliary camera 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, and 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 determined by the axis, and d is the parallax of the binocular camera.

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

[0064]

[0065] 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:

[0066]

[0067] remember have:

[0068]

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

[0070] The current time is t k , the next moment is t k+1 , the state update period is dt, and 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 .

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

[0072]

[0073] 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 optical axis at the current moment, and the moving speed 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 the x-axis at the current moment; 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 optical axis and the moving speed relative to the optical center at the next moment, h k+1 , They are respectively 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.

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

[0075]

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

[0077]

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

[0079]

[0080] Eliminate the quantity z in equation (11) k+1 , f, we have:

[0081]

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

[0083]

[0084] Further sorting, we can get:

[0085]

[0086] If dt is small, at z 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:

[0087] f(u)=(1-u) -1 =1+u+u 2 +… (15)

[0088] 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 optical axis and the moving speed 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 the y-axis at the current moment, respectively. They are respectively the target feature point at the current moment 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 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 obtain

[0112]

[0113] Arrange, get

[0114]

[0115] State transition matrix F k In According to equation (32), x k , The coordinates of the matching feature points in the binocular camera image (x k ,y k )and give; among them It is obtained by applying the (existing) optical flow tracking algorithm to the projection of the 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] The 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 in 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 the target features in the binocular camera image; the filter design process includes: first, designing and selecting a system state vector, which includes the position and speed of the target feature point in the image coordinate system of the main camera and the auxiliary camera; secondly, considering the simultaneous observation of the binocular camera, designing and selecting a system observation vector, which includes the depth information of the target feature point; thirdly, 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 camera, 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 the target feature tracking.

Claims

1. A target tracking filtering method based on a nonlinear motion model, first using a binocular vision camera to obtain a visual image, and then tracking and filtering 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, determine the system state vector X at the current time k 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 target feature point in the auxiliary camera image coordinate system at the current moment, respectively. T It refers to the transposition operation of the matrix; Based on the target perception method of the binocular vision camera, determine the system observation vector Y 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 of the k target feature point relative to the optical center in the direction of the main camera optical axis and the moving speed relative to the optical center 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; 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 is k , Based on the coordinates of the matching feature points in the binocular camera image (x k ,y k )and get; Observation quantity Y of observation information calculation system based on binocular camera 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 1, characterized in that: Based on the above prediction, the 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 formula on which the update depends is as follows: k =H k X k +R k ; Y k is the system observation vector at the current time k, H k is the system observation matrix at the current time k, R k is the observation noise matrix.

4. The target tracking filtering method based on nonlinear motion model according to claim 1, characterized in that: 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.

5. The target tracking filtering method based on nonlinear motion model according to claim 1, characterized in that: Observation quantity Y of observation information calculation system based on binocular camera k The formula is: Observable where x k ,y k , The value of is directly adopted from its measured value at the current moment k.

Images