Target tracking range finding method of vehicle-mounted front-view monocular camera

Abstract

The invention belongs to the technical field of target tracking distance measurement, and particularly relates to a target tracking distance measurement method for a vehicle-mounted forward-looking monocular camera, which comprises the following steps: predicting a target position in each frame of scene, and converting the target position to a world coordinate under a vehicle body coordinate system according to internal and external parameters of the camera obtained through calibration; calculating pose information of each frame of the camera through a visual ORB feature operator in combination with a PnP algorithm to obtain a rotation translation matrix and a rotation translation matrix; target position information under a vehicle body coordinate system is obtained through the target position and the rotation translation matrix, the relative speed of the target relative to the vehicle is calculated according to the inter-frame time, and direction angle information of the vehicle is obtained; obtaining the speed information of the vehicle according to the inertial measurement unit (IMU) fused with the wheel speed CAN data; deriving a CCA target state vector and an EKF state equation under a self-vehicle coordinate system; and continuously iteratively correcting the target state information by using an extended Kalman filter (EKF) to achieve real-time tracking of the target state information. The actual motion track of the vehicle is described in real time by establishing an accurate vehicle kinematics model.

Description

Technical Field

[0001] The invention belongs to the technical field of target tracking and ranging, and in particular relates to a target tracking and ranging method of a vehicle-mounted forward-looking monocular camera. Background Art

[0002] At present, advanced driver assistance systems (ADAS) are one of the most promising technologies for improving road safety and driving comfort. Establishing a target tracking system relative to the vehicle coordinate system (abbreviated as: relative coordinate system) as an important part of ADAS is of great significance for mobile target detection and target warning system. Most of the current target tracking and ranging schemes are based on pure visual algorithms or rely solely on radar (laser, millimeter wave) to track and measure distance. Patent [CN201911377802.3] A monocular ranging method in an intelligent driving environment, patent [CN202310309920.0] A monocular ranging method, device, equipment, medium and vehicle for vehicle intelligent driving, patent [CN201810957944.6] A monocular ranging system, method, equipment and storage medium integrated with semantic segmentation, patent [CN201811056040.2] A monocular ranging method for advanced intelligent assisted driving, all rely on pure visual algorithms for target tracking and ranging. Pure visual algorithms have certain limitations. For example, they cannot effectively deal with problems such as the loss of the target grounding point due to bad weather during tracking. Patent [CN202110763071.7] Method and system for correcting monocular ranging using single-line laser radar and patent [CN201910540030.4] radar ranging device and method rely solely on radar. In bad weather (such as heavy rain, dense fog, snow particles, etc.), the performance is significantly reduced.

[0003] Therefore, there is an urgent need to improve the existing target tracking and ranging schemes to solve the technical problems of pure visual algorithms losing the target grounding point due to bad weather during the tracking process and relying solely on radar, which significantly reduces performance in bad weather. Summary of the invention

[0004] The purpose of the present invention is to provide a target tracking and ranging method for a vehicle-mounted forward-looking monocular camera, so as to improve the existing target tracking and ranging scheme, so as to solve the technical problems that the target grounding point is lost due to bad weather during the tracking process of the pure vision algorithm and the performance is significantly reduced in bad weather when relying solely on radar.

[0005] In order to solve the above technical problems, the technical solution adopted by the present invention is as follows:

[0006] A method for target tracking and ranging using a vehicle-mounted forward-looking monocular camera comprises the following steps:

[0007] S1: Create a lightweight model fastest-Det, use the lightweight model fastest-Det to predict the target position in each frame of the scene, assign the same ID to the same target, and convert the target position to the world coordinates in the vehicle coordinate system based on the camera internal and external parameters obtained by calibration;

[0008] S2: Calculate the pose information of each frame of the camera through the visual ORB feature operator combined with the PnP algorithm to obtain the rotation and translation matrix T from the IMU to the vehicle coordinate system vi And the rotation and translation matrix T from the camera coordinates to the world coordinate system wc ;

[0009] S3: Obtain the target position information in the vehicle coordinate system through the target position obtained in step S1 and the rotation and translation matrix obtained in step S2, and calculate the relative speed of the target relative to the vehicle according to the inter-frame time;

[0010] S4: Rotation and translation matrix T from inertial measurement unit IMU calibration data vi And pose information calculation to obtain T vc , using the Euler angle formula, get the direction angle information of the vehicle;

[0011] S5: Obtain the vehicle speed information by fusing the wheel speed CAN data with the inertial measurement unit IMU;

[0012] S6: Derive the CCA target state vector and EKF state equation in the ego-vehicle coordinate system;

[0013] S7: Use the extended Kalman filter (EKF) to continuously iteratively correct the target state information to achieve real-time tracking of the target state information, which includes position, speed and distance.

[0014] Preferably, the specific process in step S6 is as follows:

[0015] S61: Based on direction angle and displacement change v g ego Establish a state equation, the specific formula of the state equation is as follows:

[0016]

[0017] Among them, P t rel is the target information in the relative coordinate system, ego in the formula is the ego vehicle, the superscript t is the target vehicle, and the subscript g is the global coordinate system global;

[0018]

[0019]

[0020]

[0021] in,

[0022]

[0023]

[0024]

[0025] get:

[0026]

[0027] because

[0028] but

[0029]

[0030] Also because

[0031]

[0032] but

[0033]

[0034] Also because

[0035]

[0036] but

[0037]

[0038] S62: Calculate the F matrix from the state equation;

[0039] in

[0040]

[0041] in, S63: Calculate the Jacobian G matrix from the F matrix:

[0042]

[0043] According to the state equation, c, a, Taking partial derivatives of _ego and v_ego respectively, we can get:

[0044]

[0045] in:

[0046]

[0047] S64: Calculate the Jacobian H matrix:

[0048] Observation vector Z k+1 =(x k+1 ,y k+1 ,w k+1 ) T , w k+1 is the width of the target;

[0049] Calculate the coordinates of the target camera coordinate system in the prediction stage. The formula is as follows

[0050]

[0051] Assume that the rotation and translation matrices from the camera coordinate system to the vehicle coordinate system are:

[0052]

[0053] Substitution

[0054]

[0055] Let x, y, and z represent the three components respectively, and normalize them to get:

[0056]

[0057] therefore,

[0058]

[0059] in,

[0060]

[0061]

[0062]

[0063] Preferably, step S7 includes the following specific process:

[0064] S71: Initialize the target state and the noise matrix;

[0065] S72: predicting target state information;

[0066] S73: updating the target status information;

[0067] S74: Repeat steps S71 to S73, and continuously correct and iterate the target state information until the real-time accurate estimation requirement of the target state is met or the preset number of iterations is reached.

[0068] Preferably, the specific process of step S71 is as follows:

[0069] S711: Add a target width to form a 7-dimensional state vector to initialize the target state:

[0070]

[0071] The empirical values ​​obtained from actual tests are initialized to obtain

[0072] (0.01, 0.001, 0, 0.05, 0, 0.01, 0.001);

[0073] S712: Initialize the noise matrix (Q and N):

[0074] The empirical values ​​obtained from actual tests are initialized as:

[0075] (0.000000001, 0.001, 0.000001, 0.0001, 0.000001)

[0076] The empirical values ​​obtained from actual tests are initialized as:

[0077] (0.000004, 0.000008, 0.000001, 0, 0).

[0078] Preferably, the specific process of step S72 is as follows:

[0079] S721: Update the position, direction, velocity, curvature, and acceleration status information of the target at time k in the relative coordinate system according to S1

[0080] S722: Calculate the Jacobian matrix F, G according to the target state information at the target time k, and update the target state at the k+1 time; t (k+1|k)=FP t (k|k)F′ + GQG′.

[0081] Preferably, the specific process of step S73 is as follows:

[0082] S731: The target grounding point information can be obtained from the target detection frame learned by deep learning in S1. Combined with the camera’s position information, the observation vector at time k+1 is obtained. In order to verify the tracking error of the target state, the target position and width information predicted in S72 are compared with the observation vector to form a residual.

[0083] S732: Calculate the Jacobian H matrix, and the state vector of the target at time k+1 obtained in the prediction phase, and calculate the Kalman gain K;

[0084] K(k+1)=P t (k+1|k)*H T (k+1) / [H(k+1)*P t (k+1|k)*H T (k+1)+V*R*V T ];

[0085] Among them, V is set to the unit matrix;

[0086] S733: Update target state information by Kalman gain:

[0087] P t (k+1|k+1)=[IK(k+1)*H(k+1)] / P t (k+1|k).

[0088] Preferably, the specific process of calculating the pose information of each frame of the camera by combining the visual ORB feature operator with the PnP algorithm in step S2 is as follows:

[0089] S21: Initialize the camera pose through the ORB algorithm, extract the feature points of the first frame image and match them;

[0090] S22: extracting feature points of each two adjacent frames of images using the ORB algorithm, and matching them to obtain matching feature point pairs;

[0091] S23: using the triangulation method in binocular stereo vision, using the matched feature point information to restore the depth of the point in the three-dimensional space and determine the three-dimensional spatial position of the point;

[0092] S24: After initializing the spatial position of the feature point, mark the designated feature point as a landmark point, process the landmark point using the PnP algorithm, perform posture calculation using the 3D point set of the given object and the 2D point set of the corresponding image, and obtain the initialized target posture;

[0093] S25: After obtaining the initialized target pose, the above matching, tracking, and triangulation processes are continuously repeated to calculate the target pose of each frame image.

[0094] Preferably, the state of the inertial measurement unit IMU in step S5 is expressed as:

[0095]

[0096] Preferably, the vehicle state in step S5 is expressed as:

[0097]

[0098] Preferably, the external parameters of the inertial measurement unit IMU and the vehicle state are expressed as:

[0099]

[0100] in,

[0101]

[0102] Find the relationship between the inertial measurement unit IMU and the vehicle state through external parameters:

[0103]

[0104] ω v＝ ω i

[0105]

[0106] get,

[0107]

[0108]

[0109]

[0110]

[0111] Finally we get:

[0112]

[0113] The beneficial effects of the present invention include:

[0114] The target tracking and ranging method of the vehicle-mounted forward-looking monocular camera provided by the present invention predicts the target position in each frame of the scene, and according to the camera internal and external parameters obtained by calibration, converts the target position to the world coordinates in the vehicle body coordinate system; calculates the position information of each frame of the camera by combining the visual ORB feature operator with the PnP algorithm, and obtains the rotation translation matrix and the rotation translation matrix; obtains the target position information in the vehicle body coordinate system by the target position and the rotation translation matrix, and calculates the relative speed of the target with respect to the vehicle according to the inter-frame time, and obtains the direction angle information of the vehicle; obtains the speed information of the vehicle according to the inertial measurement unit IMU fused with the wheel speed CAN data; derives the CCA target state vector and the EKF state equation in the vehicle body coordinate system; and uses the extended Kalman filter EKF to continuously iteratively correct the target state information, so as to achieve real-time tracking of the target state information.

[0115] The target is initially tracked through visual information to obtain the initial state of the moving target, and the wheel speed information and IMU information of the vehicle are integrated. The EKF (Extended Kalman Filter) is used to continuously predict and update (correct) the target state information, so as to achieve real-time tracking and ranging of the moving target in the vehicle coordinate system. More importantly, when the target is lost, that is, the grounding point cannot be sensed or is inaccurate, the EKF filter can still accurately estimate the position of the target. This tracking algorithm has the advantages of fast convergence, small tracking error, and timely response.

[0116] In the case of bad weather during target tracking, the tracking and ranging solution still has a certain degree of robustness. As one of the alternatives to the LiDAR ranging solution, it greatly reduces the hardware cost. An accurate vehicle kinematic model is established to describe the actual motion trajectory of the vehicle in real time, especially in complex scenes, and fully integrate multiple sensor data, such as cameras, IMU, CAN, etc., to solve the problem of real-time target tracking in complex scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0117] Figure 1 The present invention is a schematic diagram of the process structure of the target tracking and ranging method of the vehicle-mounted forward-looking monocular camera.

[0118] Figure 2 Schematic diagram of a simple vehicle circular motion model.

[0119] Figure 3 Schematic diagram of the instantaneous movement distance of the left and right wheels of the vehicle.

[0120] Figure 4 Schematic diagram of the vehicle motion model assuming that the vehicle semi-axle length is L and the moving radius is R.

[0121] Figure 5 Schematic diagram of further approximation of the vehicle motion model.

[0122] Figure 6 Schematic diagram of the final decomposition into x and y directions. DETAILED DESCRIPTION

[0123] The following is combined with Figure 1 to Figure 6 The present invention is further described in detail:

[0124] Example 1

[0125] See also Figure 1 and Figure 2 As shown, a method for target tracking and ranging of a vehicle-mounted forward-looking monocular camera includes the following steps:

[0126] S1: Create a lightweight model fastest-Det, use the lightweight model fastest-Det to predict the target position in each frame of the scene, assign the same ID to the same target, and convert the target position to the world coordinates in the vehicle coordinate system based on the camera internal and external parameters obtained by calibration;

[0127] S2: Calculate the pose information of each frame of the camera through the visual ORB feature operator combined with the PnP algorithm to obtain the rotation and translation matrix T from the IMU to the vehicle coordinate system vi And the rotation and translation matrix T from the camera coordinates to the world coordinate system wc ;

[0128] S3: Obtain the target position information in the vehicle coordinate system through the target position obtained in step S1 and the rotation and translation matrix obtained in step S2, and calculate the relative speed of the target relative to the vehicle according to the inter-frame time;

[0129] S4: Rotation and translation matrix T from inertial measurement unit IMU calibration data vi And pose information calculation to obtain T vc , using the Euler angle formula, get the direction angle information of the vehicle;

[0130] S5: Obtain the vehicle speed information by fusing the wheel speed CAN data with the inertial measurement unit IMU;

[0131] S6: Derive the CCA target state vector and EKF state equation in the ego-vehicle coordinate system;

[0132] S7: Use the extended Kalman filter (EKF) to continuously iteratively correct the target state information to achieve real-time tracking of the target state information, which includes position, speed and distance.

[0133] In this embodiment, the target is initially tracked using visual information to obtain the initial state of the moving target, and the wheel speed information and IMU information of the vehicle are integrated, and the EKF (Extended Kalman Filter) is used to continuously predict, update and correct the target state information, so as to achieve real-time tracking and ranging of the moving target in the vehicle coordinate system. More importantly, when the target is lost, or the grounding point cannot be sensed or is inaccurate, the EKF filter can still accurately estimate the position of the target. A large number of experimental results show that the tracking algorithm has the advantages of fast convergence, small tracking error, and timely response.

[0134] Example 2

[0135] See also Figure 2-Figure 6 , before this embodiment, a rigid body motion model is first established:

[0136] When the integration time is short enough, it is assumed that the vehicle's motion is a circular motion with a constant angular rate and speed. During the integration time, the vehicle's heading changes by an angle of α and the axis moves by a distance of Δs.

[0137] According to the length of the semi-axis, we can get:

[0138] Δθ=α

[0139] Δs l =Rα

[0140] Δs r =(R+2L)α

[0141] Δs=(R+L)α;

[0142] Then we get:

[0143] Rα=Δs l

[0144] Lα=Δs r / 2-Δs l / 2;

[0145] at last,

[0146] Δs=(Δs l +Δs r ) / 2

[0147] Δθ=α=(Δs l +Δs r ) / 2L;

[0148] When the integration time is short enough, assuming

[0149] Δd=Δs,

[0150] The average velocity direction is θ+Δθ / 2;

[0151] Then the horizontal and vertical increments are expressed as:

[0152] Δx=Δscos(θ+Δθ / 2)

[0153] Δy=Δssin(θ+Δθ / 2);

[0154] again

[0155] Δs=(Δs l +Δs r ) / 2

[0156] Δθ=α=(Δs l +Δs r ) / 2L;

[0157] You can get the target status information:

[0158]

[0159] The rigid arm relationship is:

[0160] The state of the IMU is represented by

[0161]

[0162] The vehicle's status indicates:

[0163]

[0164] Their external parameters are:

[0165]

[0166] in:

[0167]

[0168] Then the relationship between the two odometers can be found through external parameters

[0169]

[0170] ω v＝ ω i

[0171]

[0172] further:

[0173]

[0174]

[0175]

[0176]

[0177] Finally, we can get:

[0178]

[0179] EKF (Extended Kalman Filter) is a simple nonlinear approximate filtering algorithm for situations where the motion or observation equations are not linear. To simplify the calculation, EKF linearizes the motion / observation equations through first-order Taylor decomposition. KF and EKF have the same algorithm structure, both describe the posterior probability density in Gaussian form, and are obtained by calculating the Bayesian recursive formula. The biggest difference is that when calculating the variance, the state transfer matrix (the state information k-1|k-1 at the previous moment) and the observation matrix (one-step prediction k|k-1) of EKF are both Jacobian matrices of the state information.

[0180] The state covariance transfer and observation covariance transfer are as follows: (F, H are Jacobian matrices, G, V are noise gain matrices):

[0181] P t (k+1|k)=FP t (k|k)F′ + GQG′

[0182] Z(k+1|k)=HZ(k / k)H′+VRV′.

[0183] Based on the preliminary knowledge of vehicle rigid body motion, the CCA state vector and state equation (the coordinate system is the global coordinate system) can be derived.

[0184] State vector:

[0185]

[0186] Equation of state:

[0187]

[0188]

[0189]

[0190] in,

[0191] Based on Example 1, the specific process in step S6 is as follows:

[0192] S61: Based on direction angle and displacement change v g ego Establish a state equation, the specific formula of the state equation is as follows:

[0193]

[0194] Among them, P t rel is the target information in the relative coordinate system. In the formula, ego is the ego vehicle, superscript t is the target vehicle, and subscript g is the global coordinate system. The other parameters are explained as follows:

[0195]

[0196]

[0197]

[0198]

[0199] in,

[0200]

[0201]

[0202]

[0203] get:

[0204]

[0205] because

[0206] but

[0207]

[0208] Also because

[0209]

[0210] but

[0211]

[0212] Also because

[0213]

[0214] but

[0215]

[0216] S62: Calculate the F matrix from the state equation;

[0217] in

[0218]

[0219] in, S63: Calculate the Jacobian G matrix from the F matrix:

[0220]

[0221] According to the state equation, c, a, Taking partial derivatives of _ego and v_ego respectively, we can get:

[0222]

[0223] in:

[0224]

[0225] S64: Calculate the Jacobian H matrix:

[0226] Observation vector Z k+1 =(x k+1 ,y k+1 ,w k+1 ) T , w k+1 is the width of the target;

[0227] Calculate the coordinates of the target camera coordinate system in the prediction stage. The formula is as follows

[0228]

[0229] Assume that the rotation and translation matrices from the camera coordinate system to the vehicle coordinate system are:

[0230]

[0231] Substitution

[0232]

[0233] Let x, y, and z represent the three components respectively, and normalize them to get:

[0234]

[0235] therefore,

[0236]

[0237] in,

[0238]

[0239]

[0240]

[0241] Example 3

[0242] Based on Example 1 or Example 2, step S7 includes the following specific process:

[0243] S71: Initialize the target state and the noise matrix;

[0244] S72: predicting target state information;

[0245] S73: updating the target status information;

[0246] S74: Repeat steps S71 to S73, and continuously correct and iterate the target state information until the real-time accurate estimation requirement of the target state is met or the preset number of iterations is reached.

[0247] In this embodiment, the specific process of step S71 is as follows:

[0248] S711: Add a target width to form a 7-dimensional state vector to initialize the target state:

[0249]

[0250] The empirical values ​​obtained from actual tests are initialized to obtain

[0251] (0.01, 0.001, 0, 0.05, 0, 0.01, 0.001);

[0252] S712: Initialize the noise matrix (Q and N):

[0253] The empirical values ​​obtained from actual tests are initialized as:

[0254] (0.000000001, 0.001, 0.000001, 0.0001, 0.000001)

[0255] The empirical values ​​obtained from actual tests are initialized as:

[0256] (0.000004, 0.000008, 0.000001, 0, 0).

[0257] The specific process of step S72 is as follows:

[0258] S721: Update the position, direction, velocity, curvature, and acceleration status information of the target at time k in the relative coordinate system according to S1

[0259] S722: Calculate the Jacobian matrix F, G according to the target state information at the target time k, and update the target state at the k+1 time; t (k+1|k)=FP t (k|k)F′ + GQG′.

[0260] The specific process of step S73 is as follows:

[0261] S731: The target grounding point information can be obtained from the target detection frame learned by deep learning in S1. Combined with the camera’s position information, the observation vector at time k+1 is obtained. In order to verify the tracking error of the target state, the target position and width information predicted in S72 are compared with the observation vector to form a residual.

[0262] S732: Calculate the Jacobian H matrix, and the state vector of the target k+1 time obtained in the prediction phase, and calculate the Kalman gain K;

[0263] K(k+1)=P t (k+1|k)*H T (k+1) / [H(k+1)*P t (k+1|k)*H T (k+1)+V*R*V T ];

[0264] Among them, V is set to the unit matrix;

[0265] S733: Update target state information by Kalman gain:

[0266] P t (k+1|k+1)=[IK(k+1)*H(k+1)] / P t (k+1|k).

[0267] Example 4

[0268] On the basis of Example 1 or Example 2 or Example 3, the specific process of calculating the pose information of each frame of the camera by combining the visual ORB feature operator with the PnP algorithm in step S2 is as follows:

[0269] S21: Initialize the camera pose through the ORB algorithm, extract the feature points of the first frame image and match them;

[0270] S22: extracting feature points of each two adjacent frames of images using the ORB algorithm, and matching them to obtain matching feature point pairs;

[0271] S23: using the triangulation method in binocular stereo vision, using the matched feature point information to restore the depth of the point in the three-dimensional space and determine the three-dimensional spatial position of the point;

[0272] S24: After initializing the spatial position of the feature point, mark the designated feature point as a landmark point, process the landmark point using the PnP algorithm, perform posture calculation using the 3D point set of the given object and the 2D point set of the corresponding image, and obtain the initialized target posture;

[0273] S25: After obtaining the initialized target pose, the above matching, tracking, and triangulation processes are continuously repeated to calculate the target pose of each frame image.

[0274] The state of the inertial measurement unit IMU in step S5 is expressed as:

[0275]

[0276] The vehicle state in step S5 is expressed as:

[0277]

[0278] The external parameters of the inertial measurement unit IMU and the vehicle state are expressed as:

[0279]

[0280] in,

[0281]

[0282] Find the relationship between the inertial measurement unit IMU and the vehicle state through external parameters:

[0283]

[0284] ω v＝ ω i

[0285]

[0286] get,

[0287]

[0288]

[0289]

[0290]

[0291] Finally we get:

[0292]

[0293] In summary, the target tracking and ranging method of the vehicle-mounted forward-looking monocular camera provided by the present invention predicts the target position in each frame of the scene, and converts the target position to the world coordinates in the vehicle body coordinate system according to the camera internal and external parameters obtained by calibration; calculates the position information of each frame of the camera by combining the visual ORB feature operator with the PnP algorithm to obtain the rotation translation matrix and the rotation translation matrix; obtains the target position information in the vehicle body coordinate system through the target position and the rotation translation matrix, and calculates the relative speed of the target relative to the vehicle according to the inter-frame time to obtain the direction angle information of the vehicle; obtains the speed information of the vehicle according to the inertial measurement unit IMU fused with the wheel speed CAN data; derives the CCA target state vector and the EKF state equation in the vehicle coordinate system; and uses the extended Kalman filter EKF to continuously iteratively correct the target state information to achieve real-time tracking of the target state information.

[0294] The target is initially tracked through visual information to obtain the initial state of the moving target, and the wheel speed information and IMU information of the vehicle are integrated. The EKF (Extended Kalman Filter) is used to continuously predict and update (correct) the target state information to achieve real-time tracking and ranging of the moving target in the vehicle coordinate system. More importantly, when the target is lost, that is, the grounding point cannot be sensed or is inaccurate, the EKF filter can still accurately estimate the position of the target. The tracking algorithm has the advantages of fast convergence, small tracking error, and timely response. In the process of target tracking, if bad weather is encountered midway, the tracking and ranging scheme still has a certain robustness. As one of the alternatives to the lidar ranging scheme, it greatly reduces the hardware cost. Establishing an accurate vehicle kinematic model can describe the actual motion trajectory of the vehicle in real time, especially the motion trajectory in complex scenes, and fully integrate multiple sensor data, such as cameras, IMUs, CAN, etc. to solve the problem of real-time target tracking in complex scenes.

Claims

1. A method for target tracking and ranging using a vehicle-mounted forward-looking monocular camera, characterized in that: The following steps are involved: S1: Create a lightweight model fastest-Det, use the lightweight model fastest-Det to predict the target position in each frame of the scene, assign the same ID to the same target, and convert the target position to the world coordinates in the vehicle coordinate system based on the camera internal and external parameters obtained by calibration; S2: Calculate the pose information of each frame of the camera through the visual ORB feature operator combined with the PnP algorithm to obtain the rotation and translation matrix T from the IMU to the vehicle coordinate system vi And the rotation and translation matrix T from the camera coordinates to the world coordinate system wc ; S3: Obtain the target position information in the vehicle coordinate system through the target position obtained in step S1 and the rotation and translation matrix obtained in step S2, and calculate the relative speed of the target relative to the vehicle according to the inter-frame time; S4: Rotation and translation matrix T from inertial measurement unit IMU calibration data vi And pose information calculation to obtain T vc , using the Euler angle formula, get the direction angle information of the vehicle; S5: Obtain the vehicle speed information by fusing the wheel speed CAN data with the inertial measurement unit IMU; S6: Derive the CCA target state vector and EKF state equation in the ego-vehicle coordinate system; S7: Use the extended Kalman filter (EKF) to continuously iteratively correct the target state information to achieve real-time tracking of the target state information, which includes position, speed and distance.

2. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 1, characterized in that: The specific process in step S6 is as follows: S61: Based on direction angle and displacement change v g ego Establish a state equation, the specific formula of the state equation is as follows: Among them, P t rel is the target information in the relative coordinate system. In the formula, ego is the ego vehicle, superscript t is the target vehicle, subscript rel is the relative coordinate system, subscript g is the global coordinate system, and P rel t (k+1|k) is the target vehicle position information in the relative coordinate system of the ego vehicle estimated from time k to time k+1, including the component information in the x and y directions. is the direction angle information of the target vehicle in the relative coordinate system at time k; for is the target vehicle moving distance in ΔT time in the global coordinate system at time k; for The curvature of the target vehicle in global coordinates at time k, is the acceleration of the target vehicle in the global coordinate system at time k; R(θ) is the rotation matrix of the world coordinate relative to the vehicle coordinate in the global coordinate system; R T (θ ) is the transpose of R, the rotation matrix of the vehicle coordinates relative to the world coordinates in the global coordinate system, δ c ,δ a , δ v_ego They are the noise standard deviations of curvature, acceleration, ego vehicle angular offset, and ego vehicle speed; in, get: because but Also because but Also because but S62: Calculate the F matrix from the state equation; in in, S63: Calculate the Jacobian G matrix from the F matrix: According to the state equation, c, a, By taking partial derivatives respectively, we can get: in: S64: Calculate the Jacobian H matrix: Observation vector Z k+1 =(x k+1 ,y k+1 ,w k+1 ) T , where x k+1 ,y k+1 is the target location information, w k+1 is the width of the target; Calculate the coordinates of the target camera coordinate system in the prediction stage. The formula is as follows Assume that the rotation and translation matrices from the camera coordinate system to the vehicle coordinate system are: Substitution Let x, y, and z represent the three components respectively, and normalize them to get: therefore, in, 3. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 2, characterized in that: Step S7 includes the following specific processes: S71: Initialize the target state and the noise matrix; S72: predicting target state information; S73: updating the target status information; S74: Repeat steps S71 to S73, and continuously correct and iterate the target state information until the real-time accurate estimation requirement of the target state is met or the preset number of iterations is reached.

4. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 3 is characterized in that: The specific process of step S71 is as follows: S711: Add a target width to form a 7-dimensional state vector to initialize the target state: The empirical values ​​obtained from actual tests are initialized to obtain (0.01，0.001，0，0.05，0，0.01，0.001)； S712: Initialize the noise matrix (Q and N): The empirical values ​​obtained from actual tests are initialized as: (0.000000001，0.001，0.000001，0.0001，0.000001) The empirical values ​​obtained from actual tests are initialized as: (0.000004，0.000008，0.000001，0，0)。 5. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 3 is characterized in that: The specific process of step S72 is as follows: S721: Update the position, direction, velocity, curvature, and acceleration status information of the target at time k in the relative coordinate system according to S1 S722: Calculate the Jacobian matrix F, G according to the target state information at the target time k, and update the target state at the k+1 time; t (k+1|k)=FP t (k|k)F′ + GQG′.

6. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 3, characterized in that: The specific process of step S73 is as follows: S731: The target grounding point information can be obtained from the target detection frame learned by deep learning in S1. Combined with the camera’s position information, the observation vector at time k+1 is obtained. In order to verify the tracking error of the target state, the target position and width information predicted in S72 are compared with the observation vector to form a residual. S732: Calculate the Jacobian H matrix, and the state vector of the target at time k+1 obtained in the prediction phase, and calculate the Kalman gain K; K(k+1)=P t (k+1|k)*H T (k+1) / [H(k+1)*P t (k+1|k)*H T (k+1)+V*R*V T ]; Among them, V is set to the unit matrix; S733: Update target state information by Kalman gain: P t (k+1|k+1)=[I-K(k+1)*H(k+1)] / P t (k+1|k)。 7. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 1, characterized in that: The specific process of calculating the pose information of each frame of the camera by combining the visual ORB feature operator with the PnP algorithm in step S2 is as follows: S21: Initialize the camera pose through the ORB algorithm, extract the feature points of the first frame image and match them; S22: extracting feature points of each two adjacent frames of images using the ORB algorithm, and matching them to obtain matching feature point pairs; S23: using the triangulation method in binocular stereo vision, using the matched feature point information to restore the depth of the point in the three-dimensional space and determine the three-dimensional spatial position of the point; S24: After initializing the spatial position of the feature point, mark the designated feature point as a landmark point, process the landmark point using the PnP algorithm, perform posture calculation using the 3D point set of the given object and the 2D point set of the corresponding image, and obtain the initialized target posture; S25: After obtaining the initialized target pose, the above matching, tracking, and triangulation processes are continuously repeated to calculate the target pose of each frame image.

8. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 1, characterized in that: The state of the inertial measurement unit IMU in step S5 is expressed as:

9. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 8, characterized in that: The vehicle state in step S5 is expressed as:

10. The target tracking and ranging method of a vehicle-mounted forward-looking monocular camera according to claim 9, characterized in that: The external parameters of the inertial measurement unit IMU and the vehicle state are expressed as: in, Find the relationship between the inertial measurement unit IMU and the vehicle state through external parameters: p v +R(θ i )Δt=p i oh v＝ oh i i i＝ i i +Δθ get, p v =p i -R(θ i )Δt p v =p i -R(θ v )R(Δθ)Δt Finally we get:

Images