Target detection and tracking method based on fusion of visual sensor and millimeter wave radar

Abstract

The invention discloses a target detection and tracking method based on fusion of a visual sensor and a millimeter wave radar, and the method comprises the following steps: 1, carrying out the real-time detection of a detected target in an image shot by the visual sensor, and obtaining a target prior frame; step 2, performing projection transformation on the target feature points in the target prior frame obtained in the step 1 to obtain coordinates of a detected target in a radar coordinate system; step 3, filtering the detection signal of the millimeter wave radar, and removing noise to obtain an effective signal; 4, continuously tracking the effective signal obtained in the step 3 by using an interactive multi-model IMM algorithm, and further obtaining the coordinate of the millimeter wave radar signal target in the radar coordinate system; and 5, calculating the matching cost dij of each radar detection target and each visual identification target, and judging and outputting a fusion detection result according to a matching cost calculation result. According to the invention, the stability of target identification in the vehicle driving process is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of target detection for autonomous driving vehicles, and in particular to a target detection and tracking method based on the fusion of a visual sensor and a millimeter-wave radar. Background Art

[0002] The core of self-driving cars is their ability to complete driving tasks autonomously without human intervention. To achieve this goal, the self-driving system needs to accurately perceive and understand the surrounding environment in order to make safe and effective driving decisions. Sensors are the "senses" of the self-driving system, of which visual sensors and millimeter-wave radars are the two most commonly used types. Although these two sensors have their own advantages and disadvantages, they both play a vital role in self-driving technology.

[0003] Visual sensors, especially cameras, are widely used in autonomous driving systems. They are able to capture high-resolution images and videos, providing rich environmental information such as lane markings, traffic signs, pedestrians and other vehicles. The main advantages of visual sensors include high resolution, rich color information, and the ability to detect and recognize complex objects and scenes. However, visual sensors also have some significant limitations. For example, the performance of cameras will be greatly reduced in low light, strong light interference, or bad weather conditions (such as rain, snow, and fog). In addition, visual sensors require high computing resources to process image data, which is a challenge for autonomous driving systems with high real-time requirements.

[0004] Millimeter-wave radar is a sensor that uses electromagnetic waves in the millimeter wave band for detection. It has good penetration ability and high-precision distance and speed measurement capabilities. Millimeter-wave radar can work stably in various weather conditions (such as rain, snow, fog, etc.) and is not affected by changes in light. Therefore, it has unique advantages in environmental perception, especially in detecting the distance and speed of vehicles, pedestrians and other obstacles. However, millimeter-wave radar also has some defects. First, its resolution is low, and it is difficult to accurately identify the shape and details of objects. Secondly, radar signals are easily affected by multipath effects and interference, resulting in reduced detection accuracy. In addition, the amount of data in millimeter-wave radar is large, and effective signal processing algorithms are required to extract useful information.

[0005] In order to make up for the shortcomings of a single sensor and improve the accuracy and reliability of environmental perception, autonomous driving systems usually use multi-sensor fusion technology. Sensor fusion technology is divided into two types: front fusion and back fusion. Front fusion refers to data fusion at the sensor level, while back fusion refers to data fusion at the feature extraction or decision level. At present, most autonomous driving systems use front fusion technology to initially fuse data from different sensors at the sensor level. This method can take advantage of the advantages of multiple sensors, but its disadvantage is that the data processing is complex and it is difficult to cope with various environmental changes. In contrast, back fusion technology can adapt to environmental changes more flexibly and improve the robustness and accuracy of the system by processing the data of each sensor separately and then fusing it at the feature or decision level.

[0006] Applying the post-fusion of visual sensors and millimeter-wave radars to autonomous driving systems can fully utilize the advantages of both sensors and make up for their respective shortcomings. Specifically, visual sensors can provide high-resolution image information to help identify and classify objects, while millimeter-wave radars can accurately measure the distance and speed of objects and are not affected by weather and light conditions. Through post-fusion technology, the data of the two sensors can be combined at the decision-making level to form more accurate and reliable environmental perception results. For example, in urban road scenes, visual sensors can identify and classify pedestrians, vehicles, and traffic signs on the road, while millimeter-wave radars can provide distance and speed information for these objects. Through the post-fusion algorithm, the classification results of the visual sensor are combined with the ranging information of the millimeter-wave radar, which can more accurately perceive the surrounding environment and improve the safety and reliability of the autonomous driving system.

[0007] In addition, post-fusion technology is highly flexible and scalable. It can adaptively adjust fusion algorithms and parameters according to different application scenarios and requirements to adapt to various complex driving environments. For example, on highways, the role of millimeter-wave radar may be more important, while on city streets, the role of visual sensors is more significant. By flexibly adjusting the post-fusion algorithm, optimized perception of different driving environments can be achieved.

[0008] Although the post-fusion technology of vision and millimeter-wave radar has significant advantages, it still faces some challenges in its implementation. The first is the problem of data synchronization and calibration. Since the working principles of visual sensors and millimeter-wave radars are different, the data acquisition frequency and delay are also different. How to achieve accurate synchronization and spatial calibration of the data between the two is a difficult point. Secondly, the design and implementation of the post-fusion algorithm need to consider many factors, such as the real-time nature of data processing, the complexity of the algorithm, and the demand for computing resources. In addition, a large number of experiments and tests are needed to verify the effectiveness and reliability of the algorithm.

[0009] In order to overcome the above problems, the present invention proposes a target detection and tracking technology that fuses visual sensor and millimeter-wave radar information. Each sensor performs target detection and target tracking independently, and the data fusion method adopts decision-level fusion, which effectively overcomes the problem that a single sensor has low accuracy in target perception and is greatly affected by the environment. Summary of the invention

[0010] The purpose of the present invention is to provide a target detection and tracking method based on the fusion of visual sensor and millimeter wave radar to address the problems in the prior art that the detection accuracy of a single sensor is not high enough and is greatly disturbed by changes in the external environment.

[0011] The technical solution adopted to achieve the purpose of the present invention is:

[0012] A target detection and tracking method based on the fusion of visual sensor and millimeter wave radar includes the following steps:

[0013] Step 1: Perform real-time detection on the target in the image captured by the visual sensor to obtain the target prior frame;

[0014] Step 2, performing a projection transformation on the target feature points in the target prior frame obtained in step 1 to obtain the coordinates of the detected target in the radar coordinate system;

[0015] Step 3: filter the detection signal of the millimeter wave radar to remove noise and obtain a valid signal;

[0016] Step 4, using the interactive multi-model IMM algorithm to continuously track the effective signal obtained in step 3, so as to obtain the horizontal and vertical distances and horizontal and vertical velocities of the detected target, and then obtain the coordinates of the millimeter-wave radar signal target in the radar coordinate system;

[0017] Step 5: Calculate the matching cost d of each radar detection target and visual recognition target based on the coordinates of the millimeter wave radar signal target in the radar coordinate system obtained in step 4 and the coordinates of the mapping point of the visually recognized target in the radar coordinate system obtained in step 2. ij k , according to the matching cost calculation result, the output fusion detection result is determined.

[0018] In the above technical solution, in step 1, the YOLOV5s algorithm based on convolutional neural network is used for detection to obtain the target prior frame.

[0019] In the above technical solution, in step 2, the target feature point is the midpoint of the bottom edge of the target priori box.

[0020] In the above technical solution, in step 2, the step of projection transformation is:

[0021] The target feature points are transformed from the original pixel coordinate system to the image coordinate system, and then the target feature points are projected and transformed to the camera coordinate system, and then transformed to the radar coordinate system through rotation and translation transformation.

[0022] In the above technical solution, the pixel coordinates of the target feature point obtained by vision are (u, v), the corresponding coordinates in the radar coordinate system are (X, Y, Z), and the coordinates in the camera coordinate system are (X c , Y c , Z c ), dx and dy are the physical sizes of pixels, f is the focal length, (u0, v0) is the coordinate of the optical center of the camera in the image coordinate system, R and T are the rotation matrix and translation vector between the radar coordinate system and the camera coordinate system, respectively. The transformation formula between the pixel coordinate system and the radar coordinate system is:

[0023]

[0024] Among them, Z c is the Z value of the target feature point in the camera coordinate system, K is the intrinsic parameter matrix of the visual sensor, and the expression of K is:

[0025]

[0026] The corresponding coordinates of the target feature points obtained by vision in the radar coordinate system are:

[0027]

[0028] Let [a1 a2 a3] T Represents R -1 K -1 , where a i Represents the matrix R -1 K -1 The i-th row of the matrix, [b1 b2 b3] T Represents R - 1 T, b i Represents the matrix R -1 The i-th row of T. Then formula (3) can be expressed as:

[0029]

[0030] To find the coordinates of the point in the pixel coordinate system in the radar coordinate system, we also need to find the unknown number Z c , assuming that the bottom edge of the target prior frame from the image is on the ground plane and the installation height of the radar is h, the coordinates of the midpoint of the bottom edge of the target prior frame in the image in the radar coordinate system can be expressed as:

[0031]

[0032] From formula (5), we can get:

[0033]

[0034] Substituting formula (6) into formula (4), the projection relationship between the pixel coordinate system and the radar coordinate system can be expressed as:

[0035]

[0036] According to formula (7), after measuring the radar installation height h, the coordinates of the visual detection target feature points in the radar coordinate system can be calculated.

[0037] In the above technical solution, in step 3, invalid targets and ghost targets are filtered out according to the position, speed, reflectivity and life cycle of the target detected by the radar. Invalid targets are divided into empty targets, false targets and non-threatening targets, and are filtered in different ways. Specifically, the following steps are included:

[0038] Step s1, filter out empty targets: when the radar cross-sectional area RCS is 0, filter it out;

[0039] Step s2, filtering out non-threatening targets: Based on experience, set the horizontal distance threshold to X, set the vertical distance threshold to Y, compare the horizontal coordinate x of the target with the threshold X, and compare the vertical coordinate y of the target with the threshold Y. If |x|>X or |y|>Y, filter it out;

[0040] Step s3, filter out false targets: create a thread to combine the results of consecutive radar frames and set a threshold ε. k -x k-1 When the absolute value of is greater than ε, it is filtered out, x k is the state vector of the target at time k, x k-1 is the state vector of the target at time k-1;

[0041] Step s4, filter out ghost targets: The effective target life cycle method is divided into three stages: "generation", "persistence" and "disappearance". The state parameters of "generation" are DetectCount+1and LostCount=0, and the parameter meaning is "detecting a target ahead, the number of target detections +1, and the number of lost counts is cleared to 0", and the state parameter of "persistence" is DetectCount≥C D andLostCount≤C L , the parameter means "when the number of target detections is greater than the threshold C D And the number of losses is less than the threshold C LWhen the target is a true target, the state parameter of "disappeared" is DetectCount <C D or LostCount>C L , the parameter means "when the number of target detections is less than the threshold C D Or the number of losses is greater than the threshold C L When DetectCount≥C, the target is a false target. The state parameter DetectCount is the number of times the target is detected, and the state parameter LostCount is the number of times the target is lost continuously. D When andLostCount≤C, output the valid target, otherwise filter it out.

[0042] In the above technical solution, in step 4, the interactive multi-model Kalman filter is used to track the effective signal detected by the millimeter-wave radar, predict the target state of the next cycle, read the actual measurement data of the next frame of millimeter-wave radar, and match the predicted state with the measured state, which specifically includes the following steps:

[0043] Step a: According to the initial target information detected by the millimeter wave radar, set the initial state vector and covariance matrix. Select the constant velocity model CV and the constant acceleration model CA, and initialize the state vector, covariance matrix and transition probability matrix for each model;

[0044] Step b: For each model M i (CV or CA) state prediction steps are as follows:

[0045]

[0046] in, represents the state estimate of filter i at time k-1, represents the estimated mixed state after interaction, the state includes the transverse and longitudinal distance and transverse and longitudinal speed variables, i = 1, 2, 3..., is the state transfer matrix, represents the corresponding state covariance matrix, represents the covariance matrix of the mixed state after interaction, is the process noise covariance matrix;

[0047] Step c: For each model M i , the Kalman filter update steps are as follows:

[0048]

[0049] in, is the k-time model M i The Kalman filter gain, Indicated in The Jacobian matrix at the state, z k is the observed value of the target at time k, is the residual covariance matrix, is the measurement noise covariance matrix;

[0050] Step d: The probability update steps for each model are as follows:

[0051]

[0052] in, is the Gaussian likelihood function of the measured values, It is model M i The probability at time k, c i It is model M i The initial probability of

[0053] Step e: Model interaction steps are as follows:

[0054]

[0055] j represents the jth filter, Represents model M i and Model M oj The difference between the model M oj is the state after conversion;

[0056] Step f, the final state estimation and prediction covariance matrix update steps are as follows:

[0057]

[0058] In the above technical solution, the decision layer fusion in step 5 includes the following steps:

[0059] Step 5a: At the same time, based on the coordinates of the millimeter wave radar signal target in the radar coordinate system obtained in step 4 and the coordinates of the mapping point of the target visually recognized in step 2 in the radar coordinate system, calculate the Euclidean distance d as the matching cost, denoted by d ij k is the matching cost between the i-th target visually recognized at time k and the j-th target detected by the millimeter-wave radar, and the minimum matching cost between each visually recognized target and each millimeter-wave radar target is retained.

[0060] Step 5b: Divide the matching cost calculation results, determine whether the detection results of the two sensors are the same target, and output the fusion detection results to complete the target detection of millimeter wave radar and vision fusion, which specifically includes the following situations:

[0061] If both the millimeter wave radar and vision recognize the target, and the matching cost d ijk <d max When , the two sensors detect the same target, determine the target category through visual recognition, determine the target's motion state information through the millimeter wave radar detection result, and output the target category and motion state information;

[0062] If the millimeter wave radar detects the target but the vision fails to recognize it, or d ij k Greater than d max , then extreme weather occurs or the visual detection accuracy decreases due to occlusion, and only the target motion status information detected by the millimeter-wave radar is output;

[0063] If the target is visually recognized but not detected by the millimeter-wave radar, the millimeter-wave radar loses the target. At this time, the category information of the visually recognized target and the projection coordinates of its feature points in the radar coordinate system are output.

[0064] Compared with the prior art, the present invention has the following beneficial effects:

[0065] 1. In view of the problem that the existing target positioning through monocular camera requires the assumption of the prior size information of the target and the positioning stability is poor, a method is proposed to calculate the intermediate unknown quantity Z using the intersection of the target and the ground plane and the radar installation height. c The method improves the accuracy of monocular vision target positioning.

[0066] 2. In view of the problem that there are many false targets in millimeter-wave radar and targets are easily lost during vehicle movement, a false target filtering algorithm based on the target's reflectivity, position, speed, number of times it exists, etc. is proposed, and an interactive multi-model Kalman filter is used to track the processed millimeter-wave radar signal, thereby improving the accuracy of millimeter-wave radar detection.

[0067] 3. The fusion of millimeter-wave radar and camera is a decision-level fusion. The two sensors first detect and process the target separately, and then input them into the fusion logic module together to calculate the matching cost. The corresponding judgment is made according to the matching cost calculation result, breaking the dependence of traditional main and auxiliary fusion on the main sensor and improving the stability of target recognition during vehicle driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 It is a flow chart of the millimeter wave radar false target filtering algorithm.

[0069] Figure 2 This is the framework diagram of the millimeter wave radar target tracking algorithm.

[0070] Figure 3 It is the sensor information fusion decision principle diagram.

[0071] Figure 4 It is a specific implementation flow chart of the method of the present invention. DETAILED DESCRIPTION

[0072] The present invention is further described in detail below in conjunction with specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0073] Example 1

[0074] A target detection and tracking method based on the fusion of visual sensor and millimeter wave radar includes the following steps:

[0075] Step 1: Use the YOLOV5s algorithm based on convolutional neural network to perform real-time detection of the detection target in the image taken by the visual sensor (such as a monocular camera) to obtain the target prior frame.

[0076] Step 2, performing a projection transformation on the target feature point in the target prior frame obtained in step 1 to obtain the coordinates of the detected target in the radar coordinate system. Preferably, the target feature point is the midpoint of the bottom edge of the target prior frame.

[0077] The projection transformation specifically includes the following steps: transforming the target feature points from the original pixel coordinate system to the image coordinate system, then performing a projection transformation on the target feature points, transforming them to the camera coordinate system, and then transforming them to the radar coordinate system through a rotation and translation transformation.

[0078] The pixel coordinates of the target feature point obtained by vision are (u, v), the corresponding radar coordinate system coordinates are (X, Y, Z), and the coordinates in the camera coordinate system are (X c , Y c , Z c ), dx and dy are the physical sizes of pixels, f is the focal length, (u0, v0) is the coordinate of the optical center of the camera in the image coordinate system, R and T are the rotation matrix and translation vector between the radar coordinate system and the camera coordinate system, respectively. The transformation formula between the pixel coordinate system and the radar coordinate system is:

[0079]

[0080] Among them, Z c is the Z value of the target feature point in the camera coordinate system, K is the intrinsic parameter matrix of the visual sensor, and the expression of K is:

[0081]

[0082] The corresponding coordinates of the target feature points obtained by vision in the radar coordinate system are:

[0083]

[0084] Let [a1 a2 a3] T Represents R -1 K -1 , where a i Represents the matrix R -1 K -1 The i-th row of the matrix, [b1 b2 b3] T Represents R - 1 T, b i Represents the matrix R -1 The i-th row of T. Then formula (3) can be expressed as:

[0085]

[0086] To find the coordinates of the point in the pixel coordinate system in the radar coordinate system, we also need to find the unknown number Z c , assuming that the bottom edge of the target prior frame from the image is on the ground plane and the installation height of the radar is h, the coordinates of the midpoint of the bottom edge of the target prior frame in the image in the radar coordinate system are expressed as:

[0087]

[0088] From formula (5), we can get:

[0089]

[0090] Substituting formula (6) into formula (4), the projection relationship between the pixel coordinate system and the radar coordinate system can be expressed as:

[0091]

[0092] According to formula (7), after measuring the radar installation height h, the coordinates of the visual detection target feature points in the radar coordinate system can be calculated.

[0093] Step 3: Filter the detection signal of the millimeter wave radar to remove noise and obtain a valid signal. Specifically, invalid targets and ghost targets are filtered out according to the position, speed, reflectivity and life cycle of the radar detection target. Invalid targets are divided into empty targets, false targets and non-threatening targets, and filtered in different ways. More specifically, noise removal includes the following steps:

[0094] Step s1, filter out empty targets: when the radar cross-sectional area RCS is 0, filter it out;

[0095] Step s2, filtering out non-threatening targets: Based on experience, set the horizontal distance threshold to X, set the vertical distance threshold to Y, compare the horizontal coordinate x of the target with the threshold X, and compare the vertical coordinate y of the target with the threshold Y. If |x|>X or |y|>Y, filter it out;

[0096] Step s3, filter out false targets: create a thread to combine the results of consecutive radar frames and set a threshold ε. k -x k-1 When the absolute value of is greater than ε, it is filtered out, x k is the state vector of the target at time k, x k-1 is the state vector of the target at time k-1;

[0097] Step s4, filter out ghost targets: The effective target life cycle method is divided into three stages: "generation", "persistence" and "disappearance". The state parameters of "generation" are DetectCount+1and LostCount=0, and the parameter meaning is "detecting a target ahead, the number of target detections +1, and the number of lost counts is cleared to 0", and the state parameter of "persistence" is DetectCount≥C D andLostCount≤C L , the parameter means "when the number of target detections is greater than the threshold C D And the number of losses is less than the threshold C L When the target is a true target, the state parameter of "disappeared" is DetectCount <C D or LostCount>C L , the parameter means "when the number of target detections is less than the threshold C D Or the number of losses is greater than the threshold C L When DetectCount≥C, the target is a false target. The state parameter DetectCount is the number of times the target is detected, and the state parameter LostCount is the number of times the target is lost continuously. D When andLostCount≤C, output the valid target, otherwise filter it out.

[0098] Cumulative count threshold C D and the loss count threshold C L It can be obtained through experiments. By setting the effective life cycle of the target detected by the radar, it is possible to prevent the false filtering of the valid target. For non-threatening targets, the appropriate target area is selected according to the actual application scenario, and the judgment is made based on whether the target is in the target area.

[0099] Step 4, continuously tracking the effective signal obtained in step 3, thereby obtaining the horizontal and vertical distances and horizontal and vertical speeds of the detected target, and then obtaining the coordinates of the millimeter wave radar signal target in the radar coordinate system;

[0100] The interactive multi-model Kalman filter is used to track the effective signal detected by the millimeter-wave radar, predict the target state of the next cycle, read the actual measurement data of the next frame of millimeter-wave radar, and match the predicted state with the measured state. The specific steps include:

[0101] Step a: According to the initial target information detected by the millimeter wave radar, set the initial state vector and covariance matrix. Select the constant velocity model CV and the constant acceleration model CA, and initialize the state vector, covariance matrix and transition probability matrix for each model;

[0102] Step b: For each model M i (CV or CA) state prediction steps are as follows:

[0103]

[0104] in, represents the state estimate of filter i at time k-1, represents the estimated mixed state after interaction, the state includes the transverse and longitudinal distance and transverse and longitudinal speed variables, i = 1, 2, 3..., is the state transfer matrix, represents the corresponding state covariance matrix, represents the covariance matrix of the mixed state after interaction, is the process noise covariance matrix;

[0105] Step c: For each model M i , the Kalman filter update steps are as follows:

[0106]

[0107] in, is the k-time model M i The Kalman filter gain, Indicated in The Jacobian matrix at the state, z k is the observed value of the target at time k, is the residual covariance matrix, is the measurement noise covariance matrix;

[0108] Step d: The probability update steps for each model are as follows:

[0109]

[0110] in, is the Gaussian likelihood function of the measured values, It is model M i The probability at time k, c i It is model M i The initial probability of

[0111] Step e: Model interaction steps are as follows:

[0112]

[0113] j represents the jth filter, Represents model M i and Model M oj The difference between the model M oj is the state after conversion;

[0114] Step f, the final state estimation and prediction covariance matrix update steps are as follows:

[0115]

[0116] If the difference between the observed state of this frame and the estimated state of the IMM algorithm in the previous frame is greater than the threshold ε, the target model is updated.

[0117] Step 5: Calculate the matching cost d of each radar detection target and visual recognition target based on the coordinates of the millimeter wave radar signal target in the radar coordinate system obtained in step 4 and the coordinates of the mapping point of the target visually recognized in step 2 in the radar coordinate system. ij , according to the matching cost calculation result, the output fusion detection result is judged, which specifically includes the following steps:

[0118] Step 5a: At the same time, based on the coordinates of the millimeter wave radar signal target in the radar coordinate system obtained in step 4 and the coordinates of the mapping point of the target visually recognized in step 2 in the radar coordinate system, calculate the Euclidean distance d as the matching cost, denoted by d ij k is the matching cost between the i-th target visually recognized at time k and the j-th target detected by the millimeter-wave radar, and the minimum matching cost between each visually recognized target and each millimeter-wave radar target is retained.

[0119] Step 5b: Divide the matching cost calculation results, determine whether the detection results of the two sensors are the same target, and output the fusion detection results to complete the target detection of millimeter wave radar and vision fusion, which specifically includes the following situations:

[0120] If both the millimeter wave radar and vision recognize the target, and the matching cost d ij k <d maxWhen , the two sensors detect the same target, determine the target category through visual recognition, determine the target's motion state information through the millimeter wave radar detection result, and output the target category and motion state information;

[0121] If the millimeter wave radar detects the target but the vision fails to recognize it, or d ij k Greater than d max , then extreme weather occurs or the visual detection accuracy decreases due to occlusion, and only the target motion status information detected by the millimeter-wave radar is output;

[0122] If the target is visually recognized but not detected by the millimeter-wave radar, the millimeter-wave radar loses the target. At this time, the category information of the visually recognized target and the projection coordinates of its feature points in the radar coordinate system are output.

[0123] Example 2

[0124] This embodiment is further described based on Embodiment 1.

[0125] A target detection and tracking method based on the fusion of visual sensor and millimeter wave radar includes the following steps:

[0126] Step 1: Obtain the original image data of the scene to be detected through a monocular camera. The processing algorithm used for the image data is the YOLOV5s algorithm based on a convolutional neural network. First, the acquired original image data is scaled to a size of 640×640, and then sent to the YOLOV5s algorithm for processing to obtain the target prior frame.

[0127] Step 2, performing a projection transformation on the target feature point in the target prior frame obtained in step 1 to obtain the coordinates of the detected target in the radar coordinate system. Preferably, the target feature point is the midpoint of the bottom edge of the target prior frame.

[0128] In this example, the parameters obtained through calibration are shown in Table 1.

[0129] Table 1

[0130]

[0131] The unit of the rotation matrix R is angle (°), and the units of other parameters are mm.

[0132] By extracting the feature points of the target in the image and converting them into the millimeter-wave radar coordinate system, the coordinates of the target in the radar coordinate system are visually identified:

[0133]

[0134] Among them, a i is the matrix R -1K -1 The i-th row of the matrix, b i Represents R -1 The i-th row of T.

[0135] Step 3: Acquire millimeter wave radar data of the scene to be detected by the millimeter wave radar, wherein the millimeter wave radar data includes the position, reflectivity and velocity measurement values ​​of all targets detected by the millimeter wave radar in the scene to be detected.

[0136] The processing of millimeter-wave radar data is based on the reflectivity (RCS), target position, target speed and life cycle of the millimeter-wave radar detection target signal to filter out false targets.

[0137] First, filter out empty targets with a reflectivity of 0. Then, based on the target position, eliminate targets that pose no threat to the vehicle, that is, |x|>X or |y|>Y. In this example, X and Y are taken as 100m and 4m respectively. Next, eliminate false targets whose motion state changes between the target in this frame of data and the target in the previous frame exceed a certain threshold ε. In this example, the sampling interval between two consecutive frames of data of the millimeter-wave radar is 75ms. Assuming that the vehicle speed does not exceed 60km / h, when the position distance between the targets in two consecutive frames of data exceeds 5m, or the speed difference exceeds 2m / s, it is considered to be a false target and is eliminated. Finally, the occasional "ghost target" is eliminated by using the number of frames where the target exists or disappears. The number of frames where the target is detected is less than C D Or the number of undetected frames exceeds C L When C is detected, it is a "ghost target" caused by interference from vehicle bumps, ground protrusions, etc., and it is removed. D and C L Take 3 and 5 respectively.

[0138] Step 4, continuously tracking the effective signal obtained in step 3, thereby obtaining the horizontal and vertical distances and horizontal and vertical speeds of the detected target, and then obtaining the coordinates of the millimeter wave radar signal target in the radar coordinate system;

[0139] Step 5: Calculate the matching cost d of each radar detection target and visual recognition target based on the coordinates of the processed millimeter wave radar signal target in the radar coordinate system and the coordinates of the mapping points of the visually recognized target in the radar coordinate system. ij , according to the matching cost calculation result, the output fusion detection result is determined.

[0140] Since the sampling frequencies of the millimeter-wave radar and camera sensors are inconsistent, the data will be inconsistent in time. To solve this problem, when collecting millimeter-wave radar data and camera image data, timestamp information is given to the data respectively. When processing the two sensor signals, data information fusion is performed only when the timestamps of the two data differ by less than 0.1s.

[0141] For the millimeter-wave radar signal and visual recognition signal at the same time, first, each valid target detected by the millimeter-wave radar constitutes set A, and each target recognized by the visual recognition constitutes set B. Then, the matching cost d of each millimeter-wave radar target signal and visual recognition target signal is calculated. ij . Use d ij represents the matching cost between the i-th target identified by vision and the j-th target identified by millimeter-wave radar. When the matching cost is greater than the threshold d max , then they are definitely not the same target and will be removed. max Set to 5m.

[0142] Since the detection performance of millimeter-wave radar and camera is different, the number of targets detected by millimeter-wave radar may not be equal to the number of targets recognized by vision. For example, in a frame of data matching, the radar detects 5 targets and the camera recognizes 3 targets. The matching cost is calculated for each target detected by the millimeter-wave radar and each target recognized by the camera. When the matching cost is less than the threshold, it is considered to be matched. When a target recognized by the camera matches a target detected by the millimeter-wave radar, it means that the target recognized by the camera and the target detected by the millimeter-wave radar are the same target. When a target recognized by the camera matches targets detected by multiple millimeter-wave radars, the target with the lowest matching cost with the target recognized by the camera is selected for matching. Similarly, when a target detected by a millimeter-wave radar matches targets recognized by multiple cameras, the target recognized by the camera with the lowest matching cost with the target detected by the millimeter-wave radar is selected for matching. A target recognized by a camera can match at most one target detected by a millimeter-wave radar and output the same target detection result, and a target detected by a millimeter-wave radar can also match at most one target detected by a camera and output the same target detection result. There may also be a situation where the camera video target does not match any millimeter-wave radar target, in which case the camera video target will be considered to be a recognition error and will be filtered out. There may also be a situation where the millimeter-wave radar target does not match any camera video target, in which case the millimeter-wave radar target will be considered to be a misidentification and will be filtered out.

[0143] In the example of the present invention, the millimeter wave radar model used is 77Hz long-range millimeter wave radar ARS408-21, the average sampling interval is 75ms, and the model of the monocular camera is Senyun SG2-AR0233C, and the average sampling interval is 33ms.

[0144] Millimeter-wave radar and monocular camera are two sensors of different modalities that can provide complementary information. Millimeter-wave radar can provide stable target motion state information in extreme weather conditions, while monocular camera can provide high-resolution visual information. By combining the advantages of both, the present invention can improve the accuracy and environmental adaptability of target detection in various environmental scenarios.

[0145] The above is only a preferred embodiment of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A target detection and tracking method based on the fusion of visual sensor and millimeter wave radar, It is characterized in that The following steps are involved: Step 1: Perform real-time detection on the target in the image captured by the visual sensor to obtain the target prior frame; Step 2, performing a projection transformation on the target feature points in the target prior frame obtained in step 1 to obtain the coordinates of the detected target in the radar coordinate system; Step 3: filter the detection signal of the millimeter wave radar to remove noise and obtain a valid signal; Step 4, using the interactive multi-model IMM algorithm to continuously track the effective signal obtained in step 3, so as to obtain the horizontal and vertical distances and horizontal and vertical velocities of the detected target, and then obtain the coordinates of the millimeter-wave radar signal target in the radar coordinate system; Step 5: Calculate the matching cost d of each radar detection target and visual recognition target based on the coordinates of the millimeter wave radar signal target in the radar coordinate system obtained in step 4 and the coordinates of the mapping point of the visually recognized target in the radar coordinate system obtained in step 2. ij k , according to the matching cost calculation result, the output fusion detection result is determined.

2. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that In the step 1, the YOLOV5s algorithm based on the convolutional neural network is used for detection to obtain the target prior frame.

3. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that In step 2, the target feature point is the midpoint of the bottom edge of the target priori box.

4. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that In step 2, the steps of projection transformation are: The target feature points are transformed from the original pixel coordinate system to the image coordinate system, and then the target feature points are projected and transformed to the camera coordinate system, and then transformed to the radar coordinate system through rotation and translation transformation.

5. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that The pixel coordinates of the target feature point obtained by vision are (u, v), the corresponding radar coordinate system coordinates are (XY, Z), and the coordinates in the camera coordinate system are (X c , Y c , Z c ), dx and dy are the physical sizes of the pixel, f is the focal length, (u 0 , v 0 ) is the coordinate of the optical center of the camera in the image coordinate system, R and T are the rotation matrix and translation vector between the radar coordinate system and the camera coordinate system, respectively. The transformation formula between the pixel coordinate system and the radar coordinate system is: Among them, Z c is the Z value of the target feature point in the camera coordinate system, K is the intrinsic parameter matrix of the visual sensor, and the expression of K is: The corresponding coordinates of the target feature points obtained by vision in the radar coordinate system are: Assume [a 1 a 2 a 3 ] T Represents R -1 K -1 , where a i Represents the matrix R -1 K -1 The i-th row of the matrix, [b 1 b 2 b 3 ] T Represents R -1 T, b i Represents the matrix R -1 The i-th row of T. Then formula (3) can be expressed as: To find the coordinates of the point in the pixel coordinate system in the radar coordinate system, we also need to find the unknown number Z c , assuming that the bottom edge of the target prior frame from the image is on the ground plane and the installation height of the radar is h, the coordinates of the midpoint of the bottom edge of the target prior frame in the image in the radar coordinate system can be expressed as: From formula (5), we can get: Substituting formula (6) into formula (4), the projection relationship between the pixel coordinate system and the radar coordinate system can be expressed as: According to formula (7), after measuring the radar installation height h, the coordinates of the visual detection target feature points in the radar coordinate system can be calculated.

6. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that In step 3, invalid targets and ghost targets are filtered out according to the position, speed, reflectivity and life cycle of the target detected by the radar. Invalid targets are divided into empty targets, false targets and non-threatening targets, and are filtered in different ways. Specifically, the following steps are included: Step s1, filter out empty targets: when the radar cross-sectional area RCS is 0, filter it out; Step s2, filtering out non-threatening targets: Based on experience, set the horizontal distance threshold to X, set the vertical distance threshold to Y, compare the horizontal coordinate x of the target with the threshold X, and compare the vertical coordinate y of the target with the threshold Y. If |x|>X or |y|>Y, filter it out; Step s3, filter out false targets: create a thread to combine the results of consecutive radar frames and set a threshold ε. k -x k-1 When the absolute value of is greater than ε, it is filtered out, x k is the state vector of the target at time k, x k-1 is the state vector of the target at time k-1; Step s4, filtering out ghost targets: The effective target life cycle method is divided into three stages: "generation", "persistence" and "disappearance". The state parameters of "generation" are DetectCount+1 and LostCount=0, and the parameter meaning is "detecting a target ahead, the number of target detections +1, and the number of lost counts is cleared to 0", and the state parameter of "persistence" is DetectCount≥C D andLostCount≤C L , the parameter means "when the number of target detections is greater than the threshold C D And the number of losses is less than the threshold C L When the target is a true target, the state parameter of "disappeared" is DetectCount <C D or LostCount>C L , the parameter means "when the number of target detections is less than the threshold C D Or the number of losses is greater than the threshold C L When DetectCount≥C, the target is a false target. The state parameter DetectCount is the number of times the target is detected, and the state parameter LostCount is the number of times the target is lost continuously. D When andLostCount≤C, output the valid target, otherwise filter it out.

7. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that In step 4, an interactive multi-model Kalman filter is used to track the target of the effective signal detected by the millimeter-wave radar, predict the target state of the next cycle, read the actual measurement data of the next frame of millimeter-wave radar, and match the predicted state with the measured state.

8. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 7, It is characterized in that The step 4 specifically comprises the following steps: Step a: According to the initial target information detected by the millimeter wave radar, set the initial state vector and covariance matrix. Select the constant velocity model CV and the constant acceleration model CA, and initialize the state vector, covariance matrix and transition probability matrix for each model; Step b: For each model M i (CV or CA) state prediction steps are as follows: in, represents the state estimate of filter i at time k-1, represents the estimated mixed state after interaction, the state includes transverse and longitudinal distance and transverse and longitudinal speed variables, i=1, 2, 3..., is the state transfer matrix, represents the corresponding state covariance matrix, represents the covariance matrix of the mixed state after interaction, is the process noise covariance matrix; Step c: For each model M i , the Kalman filter update steps are as follows: in, is the k-time model M i The Kalman filter gain, Indicated in The Jacobian matrix at the state, z k is the observed value of the target at time k, is the residual covariance matrix, is the measurement noise covariance matrix; Step d: The probability update steps for each model are as follows: in, is the Gaussian likelihood function of the measured values, It is model M i The probability at time k, c i It is model M i The initial probability of Step e: Model interaction steps are as follows: j represents the jth filter, Represents model M i and Model M oj The difference between the model M oj is the state after conversion; Step f, the final state estimation and prediction covariance matrix update steps are as follows:

9. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 1, It is characterized in that The decision layer fusion of step 5 includes the following steps: Step 5a: At the same time, based on the coordinates of the millimeter wave radar signal target in the radar coordinate system obtained in step 4 and the coordinates of the mapping point of the target visually recognized in step 2 in the radar coordinate system, calculate the Euclidean distance d as the matching cost, denoted by d ij k is the matching cost between the i-th target visually recognized at time k and the i-th target detected by the millimeter-wave radar, and the minimum matching cost between each visually recognized target and each millimeter-wave radar target is retained; Step 5b: divide the matching cost calculation results, determine whether the detection results of the two sensors are the same target, and output the fusion detection results to complete the target detection of millimeter wave radar and vision fusion.

10. The target detection and tracking method based on the fusion of visual sensor and millimeter wave radar as claimed in claim 9, It is characterized in that In step 5b: If both the millimeter wave radar and vision recognize the target, and the matching cost d ij k <d max When , the two sensors detect the same target, determine the target category through visual recognition, determine the target's motion state information through the millimeter wave radar detection result, and output the target category and motion state information; If the millimeter wave radar detects the target but the vision fails to recognize it, or d ij k Greater than d max , then extreme weather occurs or the visual detection accuracy decreases due to occlusion, and only the target motion status information detected by the millimeter-wave radar is output; If the target is visually recognized but not detected by the millimeter-wave radar, the millimeter-wave radar loses the target. At this time, the category information of the visually recognized target and the projection coordinates of its feature points in the radar coordinate system are output.

Images