An improved Hungarian algorithm target tracking and matching method based on camera and millimeter-wave radar fusion

Abstract

The present invention discloses an improved Hungarian algorithm target tracking and matching method based on the fusion of camera and millimeter wave radar, including a sensor used by the system, wherein the target set sent to the fusion center by the sensor used by the system is Φ radar,refr , Φ camera,refc , wherein the subscript radar indicates that it comes from the sensor millimeter wave radar, and the subscript refr indicates that the reference coordinate system comes from the millimeter wave radar; the target tracking and matching method includes the following steps: S1, time registration of multi-sensor data; S2, spatial registration of multi-sensor data; S3, sensor target-system track matching; the present invention improves the accuracy of the Hungarian matching algorithm; because the matrix dimension is decomposed and reduced, the speed of Hungarian matching is improved; the target tracking fusion matching effect of Hungarian matching and Kalman filtering is stable, and can be applied to the fusion matching work of multiple types of sensors.

Description

Technical Field

[0001] The invention belongs to the technical field of automobile driving, and in particular relates to an improved Hungarian algorithm target tracking and matching method based on the fusion of a camera and a millimeter-wave radar. Background Art

[0002] With the continuous progress of the automobile industry and the continuous improvement of my country's economic level, the number of cars in my country has been increasing. However, it also brings many problems, such as frequent traffic accidents, increasingly serious environmental pollution and traffic jams. According to a survey by a research institute, drivers' bad driving behavior and failure to strictly abide by traffic rules are the main reasons for the frequent occurrence of various traffic accidents. At present, while major manufacturers are constantly optimizing automobile structures and applying high-strength materials to improve automobile passive safety, advanced driver assistance systems (ADAS) that rely on continuous iteration and upgrading of technology are playing a powerful role. The ADAS system can alert the driver in an emergency or take necessary safety measures proactively, which can greatly reduce the probability and severity of traffic accidents.

[0003] At present, commonly used sensors include laser radar, millimeter wave radar, camera and ultrasonic radar. Millimeter wave radar and camera are the most common sensors in ADAS system, which have the advantages of low cost and relatively mature technology, and are easy to realize industrial and industrial application. Cameras are low-cost and can identify different objects. They have outstanding advantages in object height and width measurement accuracy, pedestrian and road sign recognition accuracy, etc. Placing a binocular camera in front of the vehicle can also realize the positioning function. However, the detection effect of the camera is easily affected by harsh environment, such as rainy and foggy weather and dark days. Millimeter wave radar realizes distance and speed measurement functions by emitting electromagnetic waves, which is not affected by light and weather factors, but cannot recognize lane lines and traffic signs. Therefore, by combining cameras and radars to realize the perception of road environment, the advantages of cameras and radars are complementary, and more stable and reliable ADAS functions are realized, which is of great significance to improve driving safety. Multi-sensor fusion can improve the accuracy and robustness of vehicle detection and tracking. According to the data abstraction level, the information fusion system can be divided into three levels: data-level fusion, feature-level fusion and decision-level fusion. Data-level fusion directly fuses sensor observation data, and extracts features and makes judgments on the fusion results. This type of data fusion requires that sensors are of the same type. Feature-level fusion can be divided into two categories: target state information fusion and target feature information fusion. Target state information fusion is mainly used in the field of multi-sensor target tracking. After completing data registration of sensor data, data association and state estimation are performed. Decision-level fusion is a high-level fusion. Each sensor first makes a decision based on its own data, and then completes the fusion processing of local decisions in the fusion center. This fusion method has the greatest data loss, so the accuracy is low.

[0004] The fusion of multiple sensors at the target level has become an important direction, and Hungarian matching is one of the commonly used target and track matching algorithms. The algorithm is simple and consumes few resources. However, in actual use, Hungarian matching often forcibly matches targets that should not be matched in order to ensure the global maximum matching, resulting in incorrect matching of target tracks that should have been correctly matched. Therefore, it is of great significance to maximize the matching accuracy in target-level fusion. For this reason, we propose an improved Hungarian algorithm target tracking and matching method based on the fusion of camera and millimeter-wave radar. Summary of the invention

[0005] The purpose of the present invention is to provide an improved Hungarian algorithm target tracking and matching method based on the fusion of camera and millimeter-wave radar to solve the problems raised in the above background technology.

[0006] To achieve the above object, the present invention provides the following technical solution: an improved Hungarian algorithm target tracking and matching method based on camera and millimeter wave radar fusion, comprising a sensor used by the system, wherein the target set sent to the fusion center by the sensor used by the system is Φ radar,refr , Φ camera,refc , where the subscript radar indicates that it comes from the sensor millimeter wave radar, and the subscript refr indicates that the reference coordinate system comes from the millimeter wave radar;

[0007] The target tracking and matching method includes the following steps:

[0008] S1, time registration of multi-sensor data;

[0009] S2, spatial registration of multi-sensor data;

[0010] S3, sensor target-system track matching;

[0011] The sensor target-system track matching includes the following steps:

[0012] A. Screening out matched targets: When a single sensor detects and tracks a target locally, it maintains a track set and is assigned a track ID to distinguish which target is being tracked. After the target set is sent to the fusion center, it is matched with the target track set ID maintained by the fusion center. If the same ID exists, it means that they are the same tracked target and are matched directly.

[0013] B. Calculate the association matrix M: Calculate the distance similarity between each target in the unmatched target set and each track in the unmatched system track, and fill it into the corresponding position of the association matrix. The distance similarity is calculated as follows:

[0014]

[0015] C. Maximum connected subgraph segmentation includes the following steps:

[0016] Step 1: Binarize the correlation matrix to form a new graph G:

[0017] Step 2: Subgraph segmentation: Using data structure method and breadth-first search, depth-first search with backtracking can be used to divide the number of maximum connected subgraphs and the nodes they contain, record them, and form a new association matrix for each subgraph. The value of the corresponding position of the association matrix is ​​taken from the initial association matrix M, which represents the distance similarity between the target and the system track. The association matrix set Ms = {M1, M2, ..., M n}, where n is the number of connected subgraphs cut;

[0018] D. Matching within a subgraph;

[0019] E. Arrangement and verification.

[0020] Preferably, in the new graph matrix of the new graph G in the maximum connected subgraph segmentation step, the value of each element in the new graph matrix is ​​as follows:

[0021]

[0022] Preferably, in the new graph matrix in the S3 maximum connected subgraph segmentation, M i,j Indicates the value of the corresponding position in the association matrix = distance similarity, th sensor1 It represents the threshold set when sensor 1 matches the system track. size(unassignedobjs) represents the number of unmatched targets, that is, the number of rows / columns of the association matrix. size(unassignedtracks) represents the number of unmatched system tracks. Its value is 1, indicating that the two nodes in the graph are connected, and 0, indicating that they are not connected.

[0023] Preferably, the time alignment in S1 is: taking the newer timestamp t in the camera and the millimeter wave target set as the predicted time, that is, Then, the prediction step of Kalman filter is used to predict the target with a timestamp earlier than the predicted time, and the timestamp is predicted using a constant speed model; the prediction step formula of Kalman filter is as follows:

[0024] The system state equation is:

[0025] x k =Ax k-1 +Bu k-1 +w k-1

[0026] The prediction step estimates the state at the current moment (moment k) based on the posterior estimate at the previous moment (moment k-1), and obtains the prior estimate at moment k:

[0027]

[0028]

[0029] This achieves time synchronization, part of the Kalman prediction.

[0030] Preferably, the spatial registration of S2 is: coordinate transformation of targets sent to the fusion center by different sensors, and transforming the sensor data into the reference system of the fusion center system track:

[0031]

[0032] in Represents the homogeneous coordinates formed by the coordinates of the target in the sensor coordinate system, It represents the homogeneous coordinates transformed to the system track coordinates, R represents the rotation matrix from the sensor coordinates to the system track coordinates, and t represents the translation matrix from the sensor coordinates to the system track coordinates, which is determined by the sensor hardware installation position.

[0033] Preferably, the pseudo code for filtering out the matched targets in the track matching in S3 is as follows:

[0034]

[0035]

[0036] Organize successfully matched target-track pairs, unmatched targets, and unmatched tracks.

[0037] Preferably, in the calculation method of calculating the correlation matrix M in the track matching in S3, s ,y s ,z s ,v s , are the coordinates, velocity and position angle of the sensor target in the system track coordinate system after the prediction step, x c ,y c ,z c ,v c , are the coordinates, speed and position angle saved by the system track and after the prediction step, p x ,p y ,p z ,p v , are the weight coefficients of the corresponding distance similarity components respectively;

[0038] When the obtained distance similarity d is greater than the set threshold, it is directly taken to the maximum value of the data structure, indicating that it is marked as unmatched.

[0039] Preferably, the subgraph matching in S4 is to use the Hungarian matching method to match the inside of each connected subgraph, and finally obtain the following subgraph target-track matching pairs Φ match,i , there is no matching target in the subgraph Φ umobjs,i , No matching track in the subgraph Φ umtracks,i .

[0040] Preferably, the arrangement and verification of S5 includes: arranging the matching results of each subgraph in the subgraph of S4 to obtain target-track matching pairs, unmatched targets, and unmatched tracks, as follows:

[0041]

[0042]

[0043]

[0044] U represents the union, n is the number of subgraphs,

[0045] P match For each matching pair in Φ, a set threshold is used for verification. If it is greater than the threshold, it is changed to unmatched, and the target and track corresponding to the matching pair are added to Φ umobjs With Φ umtracks middle.

[0046] Preferably, the pairing result between the sensor target set and the system track set is the matching pair set Φ match , does not match the target Φ umobjs , unmatched track Φ umtracks .

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

[0048] 1. The present invention improves the accuracy of the Hungarian matching algorithm; because the matrix dimension is decomposed and reduced, the speed of Hungarian matching is improved; the target tracking fusion matching effect of Hungarian matching and Kalman filtering is stable and can be applied to the fusion matching work of multiple types of sensors.

[0049] 2. The present invention performs a pruning operation to split the connected subgraph in the case where Hungarian matching is prone to mismatching, thereby improving its matching accuracy. On the other hand, because pruning reduces the number of targets, that is, the dimension of the association matrix, the time complexity of the program can be reduced exponentially. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a schematic diagram of the steps of the present invention;

[0051] Figure 2 Schematic diagram of the shortcomings of forced matching of the Hungarian matching algorithm of the present invention. DETAILED DESCRIPTION

[0052] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0053] See also Figure 1 and Figure 2 The present invention provides a technical solution: an improved Hungarian algorithm target tracking and matching method based on the fusion of camera and millimeter wave radar, including a sensor used by the system, and the target set sent to the fusion center by the sensor used by the system is Φ radar，refr , Φ camera，refc , where the subscript radar indicates that it comes from the sensor millimeter wave radar, and the subscript refr indicates that the reference coordinate system comes from the millimeter wave radar;

[0054] The target tracking and matching method includes the following steps:

[0055] S1. Temporal registration of multi-sensor data: Take the newer timestamp t in the camera and millimeter wave target set as the predicted time, that is, Then, the prediction step of Kalman filter is used to predict the target with a timestamp earlier than the predicted time, and the timestamp is predicted using a constant speed model; the prediction step formula of Kalman filter is as follows:

[0056] The system state equation is:

[0057] x k =Ax k-1 +Bu k-1 +w k-1

[0058] The prediction step estimates the state at the current moment (moment k) based on the posterior estimate at the previous moment (moment k-1), and obtains the prior estimate at moment k:

[0059]

[0060]

[0061] This achieves time synchronization, part of Kalman's prediction;

[0062] S2. Spatial registration of multi-sensor data: Because the reference coordinate systems of targets sent to the fusion center by different sensors are not necessarily the same, coordinate transformation is required to transform the sensor data to the reference system of the fusion center system track:

[0063]

[0064] in Represents the homogeneous coordinates formed by the coordinates of the target in the sensor coordinate system, Represents the homogeneous coordinates transformed to the system track coordinates, R represents the rotation matrix from the sensor coordinates to the system track coordinates, and t represents the translation matrix from the sensor coordinates to the system track coordinates, which is determined by the sensor hardware installation position;

[0065] S3, sensor target-system track matching, including the following:

[0066] A. Filter out matched targets: When a single sensor detects and tracks a target locally, the sensor maintains a track set and is assigned a track ID to distinguish which target is being tracked. After the target set is sent to the fusion center, it is matched with the target track set ID maintained by the fusion center. If the same ID exists, it means that they are the same tracked target and are matched directly. The pseudo code is as follows:

[0067]

[0068]

[0069] Arrange successfully matched target-track pairs, unmatched targets, and unmatched tracks;

[0070] B. Calculate the association matrix M: Calculate the distance similarity between each target in the unmatched target set and each track in the unmatched system track, and fill it into the corresponding position of the association matrix. The distance similarity is calculated as follows:

[0071]

[0072] where x s ,y s ,z s ,v s , are the coordinates, velocity and position angle of the sensor target in the system track coordinate system after the prediction step, x c ,y c ,z c ,v c , are the coordinates, speed and position angle saved by the system track and after the prediction step, p x ,p y ,p z ,p v , are the weight coefficients of the corresponding distance similarity components respectively;

[0073] When the distance similarity d obtained is greater than the set threshold, it is directly taken to the maximum value of the data structure, indicating that it is marked as unmatched;

[0074] C. Maximum connected subgraph segmentation, because the Hungarian matching algorithm will try its best to form the maximum matching, it is easy to cause the disadvantage of forced matching, such as Figure 2 As shown in the figure, in order to ensure the global maximum match in Hungarian matching, it is easy to match points 1 with 2, and 3 with 4. However, the distance between 1 and 2 exceeds the threshold, and after being discarded in the final verification stage, 2 is in an unmatched state, and 3 and 4 are matched. In order to avoid this situation, the maximum connected subgraph segmentation method is used, and the process is as follows:

[0075] Step 1: Binarize the correlation matrix to form a new graph G: The value of each element in the new graph matrix of the new graph G is as follows:

[0076]

[0077] Among them, M i,j Indicates the value of the corresponding position in the association matrix = distance similarity, th sensor1 It indicates the threshold value set when sensor 1 matches the system track, size(unassignedobjs) indicates the number of unmatched targets, that is, the number of rows / columns of the association matrix, and size(unassignedtracks) indicates the number of unmatched system tracks. Its value is 1 if the two nodes in the graph are connected, and 0 if they are not connected;

[0078] Step 2, subgraph segmentation: This process uses data structure methods, using depth-first search with backtracking, or breadth-first search, which can easily divide the number of maximum connected subgraphs and the nodes they contain. Record them and form a new association matrix for each subgraph. The values ​​of the corresponding positions of the association matrix are taken from the initial association matrix M, which represents the distance similarity between the target and the system track. The association matrix set Ms = {M1, M2, ..., M n}, where n is the number of connected subgraphs cut;

[0079] D. Intra-subgraph matching: For each connected subgraph, use the Hungarian matching method to match, and finally get the subgraph: Intra-subgraph target-track matching pair Φ match,i, there is no matching target in the subgraph Φ umobjs,i , No matching track in the subgraph Φ umtracks,i ;

[0080] E. Arrangement and verification: Arrange the matching results of each sub-graph in the S4 sub-graph to obtain target-track matching pairs, unmatched targets, and unmatched tracks, as follows:

[0081]

[0082]

[0083]

[0084] U represents the union, and n is the number of subgraphs.

[0085] P match For each matching pair in Φ, a set threshold is used for verification. If it is greater than the threshold, it is changed to unmatched, and the target and track corresponding to the matching pair are added to Φ umobjs With Φ umtracks middle.

[0086] So far, the pairing results between the sensor target set and the system track set are obtained, and the matching pair set Φ match , does not match the target Φ umobjs , unmatched track Φ umtracks . It can be used for subsequent fusion work.

[0087] From the above description, it can be seen that the present invention has the following beneficial effects: the present invention designs a target tracking and matching method based on improved Hungarian matching and Kalman filtering for the target and track set that have been identified and tracked by the camera and millimeter wave respectively, thereby improving the accuracy of the Hungarian matching algorithm; because the matrix dimension is decomposed and reduced, the speed of Hungarian matching is improved; the fusion matching effect is stable and can be applied to the fusion matching work of multiple types of sensors.

[0088] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0089] The above description is only used to illustrate the technical solution of the present invention rather than to limit it. Other modifications or equivalent substitutions made to the technical solution of the present invention by ordinary technicians in this field should be included in the scope of the claims of the present invention as long as they do not depart from the spirit and scope of the technical solution of the present invention.

Claims

1. An improved Hungarian algorithm target tracking and matching method based on the fusion of camera and millimeter wave radar, characterized in that: Including the sensors used by the system, the target set sent to the fusion center by the sensors used by the system is Φ radar,refr , Φ camera,refc , where the subscript radar indicates that it comes from the sensor millimeter wave radar, and the subscript refr indicates that the reference coordinate system comes from the millimeter wave radar; The target tracking and matching method includes the following steps: S1, time registration of multi-sensor data; S2, spatial registration of multi-sensor data; S3, sensor target-system track matching; The sensor target-system track matching comprises the following steps: 1) Screening out matched targets: When a single sensor detects and tracks a target locally, it maintains a track set and is assigned a track ID to distinguish which target is being tracked. After the target set is sent to the fusion center, it is matched with the target track set ID maintained by the fusion center. If the same ID exists, it means that they are the same tracked target and are directly matched; 2) Calculate the association matrix M: Calculate the distance similarity between each target in the unmatched target set and each track in the unmatched system track, and fill it into the corresponding position of the association matrix. The distance similarity is calculated as follows: in are the coordinates, velocity and position angle of the sensor target in the system track coordinate system after the prediction step, are the coordinates, speed and position angle saved in the system track after the prediction step, are the weight coefficients of the corresponding distance similarity components respectively; When the distance similarity d obtained is greater than the set threshold, it is directly taken to the maximum value of the data structure, indicating that it is marked as unmatched; 3) Maximum connected subgraph segmentation, including the following steps: Step 1: Binarize the correlation matrix to form a new graph G: Step 2: Subgraph segmentation: Using data structure method and breadth-first search, use depth-first search with backtracking to divide the number of maximum connected subgraphs and the nodes they contain, record them, and form a new association matrix for each subgraph. The value of the corresponding position of the association matrix is ​​taken from the initial association matrix M, which represents the distance similarity between the target and the system track. The association matrix set Ms = {M1, M2, ..., M n }, where n is the number of connected subgraphs cut; 4) Matching within subgraph; 5) Arrangement and verification.

2. The improved Hungarian algorithm target tracking and matching method based on camera and millimeter wave radar fusion according to claim 1 is characterized in that: In the new graph matrix of the new graph G in the maximum connected subgraph segmentation step, the value of each element in the new graph matrix is ​​as follows: i=0,1,2,...,size(unassignedobjs)-1,j=0,1,2,...,size(unassignedtracks)-1 Among them, M i,j Indicates the value of the corresponding position in the association matrix = distance similarity, th sensor1 It indicates the threshold set when sensor 1 matches the system track. size(unassignedobjs) indicates the number of unmatched targets. size(unassignedtracks) indicates the number of unmatched system tracks. An element value of 1 indicates that the two nodes in the graph are connected, and 0 indicates that they are not connected.

3. The improved Hungarian algorithm target tracking and matching method based on camera and millimeter wave radar fusion according to claim 1 is characterized in that: The spatial registration of S2 is to transform the coordinates of the targets sent by different sensors to the fusion center, and transform the sensor data into the reference system of the fusion center system track: in Represents the homogeneous coordinates formed by the coordinates of the target in the sensor coordinate system, It represents the homogeneous coordinates transformed to the system track coordinates, R represents the rotation matrix from the sensor coordinates to the system track coordinates, and t represents the translation matrix from the sensor coordinates to the system track coordinates, which is determined by the sensor hardware installation position.

4. The improved Hungarian algorithm target tracking and matching method based on camera and millimeter wave radar fusion according to claim 1 is characterized in that: The subgraph matching is to use the Hungarian matching method to match the inside of each connected subgraph, and finally obtain the subgraph: the subgraph target-track matching pair Φ match,i , there is no matching target in the subgraph Φ umobjs,i , No matching track in the subgraph Φ umtracks,i .

5. The improved Hungarian algorithm target tracking and matching method based on camera and millimeter wave radar fusion according to claim 1 is characterized in that: The arrangement and verification includes: arranging the results of each sub-graph matching within the sub-graph to obtain target-track matching pairs, unmatched targets, and unmatched tracks, as follows: U represents the union, n is the number of subgraphs, P match For each matching pair in Φ, a set threshold is used for verification. If it is greater than the threshold, it is changed to unmatched, and the target and track corresponding to the matching pair are added to Φ umobjs With Φ umtracks middle.

6. The improved Hungarian algorithm target tracking and matching method based on camera and millimeter wave radar fusion according to claim 5 is characterized in that: The pairing result between the sensor target set and the system track set, the matching pair set Φ match , does not match the target Φ umobjs , unmatched track Φ umtracks .

Images