Target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion

Abstract

The present invention relates to a target heading angle fitting processing method of a 4D millimeter wave radar point cloud based on data fusion, comprising: obtaining 4D millimeter wave radar point cloud information; performing point cloud clustering on the obtained 4D millimeter wave radar point cloud information, and calculating point cloud cluster features; associating the current tracking point with the previous frame track to determine whether the track disappearance condition is met; calculating the first heading angle, the second heading angle, and the third heading angle respectively by principal component analysis, Lshape fitting, and the ratio of the lateral and longitudinal absolute speeds of the target vehicle for the track with associated point clouds; designing the data fusion weights of the first heading angle, the second heading angle, and the third heading angle respectively according to the cluster length and width of the current target and the prior information of the ground speed, so as to calculate the fused heading angle of this frame; bringing the fused heading angle as a measured value into the heading angle filter to calculate the target heading angle. The method of the present invention is adopted, and the accuracy of the target heading angle is significantly improved.

Description

Technical Field

[0001] The present invention belongs to the field of 4D millimeter wave radar, and in particular to the field of target heading angle fitting, and specifically refers to a target heading angle fitting processing method, device, processor and computer-readable storage medium of a 4D millimeter wave radar point cloud based on data fusion. Background Art

[0002] With the rapid development of autonomous driving technology, the ability of smart driving cars to perceive the surrounding environment is becoming increasingly important. At present, smart driving cars mainly perceive the environment through two methods: cameras and radars. Among them, millimeter wave radar is widely used in the field of smart driving due to its advantages such as high accuracy in ranging and speed measurement, little interference from weather, and moderate cost. The heading angle of the vehicle is a very important information, which is crucial for predicting the state of the target vehicle at the next moment and behavior analysis.

[0003] Prior art CN202410594751.4 proposes a method, device and medium for calculating target heading angle based on millimeter wave radar point cloud. The patent requires that the reference vehicle obtain the absolute lateral and longitudinal speed ratio based on the same target multi-frame filtering association to determine the millimeter wave radar heading angle. For low-speed targets, this method directly uses the absolute lateral and longitudinal speed ratio to determine the heading angle, which will affect the accuracy of the heading angle result due to the large noise of the filtering speed.

[0004] The prior art CN202210471195.2 proposes a 4D millimeter-wave radar target heading angle fitting method and storage medium. After analyzing the characteristics of the sensor, this patent adopts different heading angle fitting algorithms for targets at different positions, and uses different weights under different conditions to calculate the target heading angle in combination with the heading angle of the target after reference tracking and the heading angle formed by the target's external frame. However, compared with the laser radar, the point cloud density of the millimeter-wave radar is related to the radial distance and angle of the target. The error of the target heading angle fitted by single-frame Lshape fitting and PCA (principal component analysis) is large, which will affect the accuracy of the heading angle result. Summary of the invention

[0005] The purpose of the present invention is to overcome the shortcomings of the above-mentioned prior art and provide a target heading angle fitting processing method, device, processor and computer-readable storage medium thereof for 4D millimeter-wave radar point cloud based on data fusion.

[0006] In order to achieve the above-mentioned object, the target heading angle fitting processing method, device, processor and computer-readable storage medium of the 4D millimeter wave radar point cloud based on data fusion of the present invention are as follows:

[0007] The target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion has the following main features:

[0008] (1) Obtain 4D millimeter wave radar point cloud information;

[0009] (2) Cluster the acquired 4D millimeter-wave radar point cloud information and calculate the point cloud cluster features;

[0010] (3) Associate the current tracking point with the track of the previous frame and determine whether the track disappearance condition is met;

[0011] (4) For the track with associated point clouds, the first heading angle, the second heading angle, and the third heading angle are calculated respectively by principal component analysis, Lshape fitting, and the ratio of the lateral and longitudinal absolute velocities of the target vehicle;

[0012] (5) According to the prior information of the cluster length and width of the current target and the ground speed, the data fusion weights of the first heading angle, the second heading angle and the third heading angle are designed respectively, so as to calculate the fusion heading angle of the current frame;

[0013] (6) Substitute the fused heading angle as a measurement value into the heading angle filter to calculate the target heading angle.

[0014] Preferably, the step (1) is: calculating the prior state information of the current target based on the target tracking state information of the previous frame ,in, They are the position, velocity and acceleration of the target in the vehicle coordinate system, specifically:

[0015] ;

[0016] ;

[0017] ;

[0018] in, is the state at the previous moment, is the prior state at this moment, is the estimated covariance at the previous moment, is the prior estimated covariance at this moment, A is the transition matrix, express A is the transpose of , Q is the variance of the process noise, and T is the frame length.

[0019] Preferably, the step (2) is specifically as follows:

[0020] The DBSCAN clustering algorithm is used to cluster the 4D millimeter wave point cloud data to obtain the clustering results and calculate the cluster related information, including the cluster center point status information. , i.e. the x, y position and radial velocity information of the target in the vehicle coordinate system; BOX information , that is, the size information of the point cloud cluster, including the length, width, height and heading angle of the BOX.

[0021] Preferably, the step (3) specifically comprises the following steps:

[0022] (3.1) According to the Hungarian algorithm, calculate whether there is a related point cloud between the tracked target and the cluster. If so, continue to track the target and filter the target state, and enter step (4); otherwise, enter step (3.2);

[0023] (3.2) Further determine whether the current tracking point meets the track disappearance condition. If so, the target track is extrapolated and the BOX information is kept unchanged. Otherwise, the track is deleted directly.

[0024] (3.3) According to the association success result, the matching pair between the tracking target ID and the cluster ID number of the perceived target is obtained. .

[0025] Preferably, the step (3) further comprises updating the state equation according to the obtained optimal matching pair:

[0026] ;

[0027] ;

[0028] ;

[0029] in, , , express H The transpose of represents the Kalman gain calculated at this moment, is the posterior state at this moment, is the prior state at this moment, is the posterior estimated covariance at this moment, is the prior estimated covariance at this moment, is the identity matrix.

[0030] Preferably, the first heading angle Specifically:

[0031] The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are denoted as [ ], and use this to establish a two-dimensional position feature point set P:

[0032] ;

[0033] Calculate the covariance matrix of P :

[0034] ,in ;

[0035] Compute the eigenvalues ​​and eigenvectors of the covariance matrix C:

[0036] ;

[0037] Among them, the matrix U The columns of the covariance matrix The unit orthogonal eigenvectors of , the eigenvalues ​​are arranged from large to small, and the first k eigenvalues ​​and corresponding eigenvectors are retained, which are the directions of the first k principal component vectors;

[0038] By calculating the single vehicle point cloud coordinate data (x k ,y k ), take the maximum eigenvalue, and its corresponding eigenvector is the vehicle heading direction.

[0039] Preferably, the second heading angle Specifically:

[0040] The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are recorded as , select the two farthest outliers and , draw a straight line Ld between the two points, take these two points as the upper and lower intervals, project all the points of the current obstacle onto the straight line Ld, and record the length of each point from the straight line Ld as L i , L i The point with the maximum value is recorded as ,by , , A rectangle is drawn for the vertex, wherein the direction of the long side obtained is the second heading angle.

[0041] Preferably, the third heading angle is specifically, the ratio of the lateral and longitudinal absolute speeds of the target vehicle is used as the third heading angle of the target vehicle. :

[0042] ;

[0043] According to the vehicle information obtained by the current 4D millimeter wave radar and the a posteriori information at this moment, the new ground speed of the current target (v gx ,v gy ), and use this to update the third heading angle .

[0044] Preferably, the step (5) is specifically to calculate the current frame fusion heading angle in the following manner: :

[0045] ;

[0046] in, , , are all information weights.

[0047] Preferably, the step (6) is: taking the fused heading angle of the current frame as a measurement value into a heading angle filter to calculate the target heading angle, and the heading angle filter uses a Kalman filter to perform multi-frame filtering, specifically:

[0048] ;

[0049] ;

[0050] ;

[0051] ;

[0052] ;

[0053] in, is the heading angle state at the previous moment, is the heading angle state predicted at this moment, is the estimated covariance at the previous moment, is the variance of process noise, is the prior estimated covariance of the forecast at this moment, is the variance of the measurement noise, is the Kalman gain calculated at this moment, is the target heading angle, is the posterior state of the heading angle at this moment, is the posterior error covariance of the heading angle at this moment.

[0054] The target heading angle fitting processing device of the 4D millimeter wave radar point cloud based on data fusion has the following main features: the device comprises:

[0055] a processor configured to execute computer executable instructions;

[0056] The memory stores one or more computer executable instructions. When the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion are implemented.

[0057] The processor for target heading angle fitting processing of 4D millimeter wave radar point cloud based on data fusion has the main feature that the processor is configured to execute computer executable instructions. When the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion are implemented.

[0058] The main feature of the computer-readable storage medium is that a computer program is stored thereon, and the computer program can be executed by a processor to implement the steps of the target heading angle fitting processing method of the 4D millimeter-wave radar point cloud based on data fusion described above.

[0059] The target heading angle fitting processing method, device, processor and computer-readable storage medium of the 4D millimeter-wave radar point cloud based on data fusion of the present invention are adopted, and by placing a corner reflector at any position in the radar common illumination area, an equation or error optimization function is established to estimate the radar installation angle. The method proposed by the present invention makes full use of the characteristics of millimeter-wave radar, and performs data fusion and multi-frame association on the single-frame Lshape fitting heading angle, PCA (principal component analysis) fitting target heading angle and motion speed estimation fitting heading angle data, which significantly improves the accuracy of the target heading angle. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 The present invention is a flow chart of the target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion.

[0061] Figure 2 The present invention is a flowchart of a target heading angle fitting processing method of a 4D millimeter wave radar point cloud based on data fusion in a specific embodiment.

[0062] Figure 3 A schematic diagram of calculating the second heading angle according to the present invention.

[0063] Figure 4 The figure is a schematic diagram of the measured results of the target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion according to the present invention. DETAILED DESCRIPTION

[0064] In order to more clearly describe the technical content of the present invention, further description is given below in conjunction with specific embodiments.

[0065] Before describing in detail embodiments according to the present invention, it should be noted that, hereinafter, the terms "comprises", "includes" or any other variations are intended to cover non-exclusive inclusion, whereby a process, method, article or apparatus comprising a series of elements includes not only these elements, but also other elements not explicitly listed or inherent to such process, method, article or apparatus.

[0066] See also Figure 1 As shown, the target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion, wherein the method comprises the following steps:

[0067] (1) Obtain 4D millimeter wave radar point cloud information;

[0068] (2) Cluster the acquired 4D millimeter-wave radar point cloud information and calculate the point cloud cluster features;

[0069] (3) Associate the current tracking point with the track of the previous frame and determine whether the track disappearance condition is met;

[0070] (4) For the track with associated point clouds, the first heading angle, the second heading angle, and the third heading angle are calculated respectively by principal component analysis, Lshape fitting, and the ratio of the lateral and longitudinal absolute velocities of the target vehicle;

[0071] (5) According to the prior information of the cluster length and width of the current target and the ground speed, the data fusion weights of the first heading angle, the second heading angle and the third heading angle are designed respectively, so as to calculate the fusion heading angle of the current frame;

[0072] (6) Substitute the fused heading angle as a measurement value into the heading angle filter to calculate the target heading angle.

[0073] As a preferred embodiment of the present invention, the step (1) is: according to the target tracking state information of the previous frame, calculate the prior state information of the current target ,in, They are the position, velocity and acceleration of the target in the vehicle coordinate system, specifically:

[0074] ;

[0075] ;

[0076] ;

[0077] in, is the state at the previous moment, is the prior state at this moment, is the estimated covariance at the previous moment, is the prior estimated covariance at this moment, A is the transition matrix, express A is the transpose of , Q is the variance of the process noise, and T is the frame length.

[0078] As a preferred embodiment of the present invention, the step (2) is specifically as follows:

[0079] The DBSCAN clustering algorithm is used to cluster the 4D millimeter wave point cloud data to obtain the clustering results and calculate the cluster related information, including the cluster center point status information. , i.e. the x, y position and radial velocity information of the target in the vehicle coordinate system; BOX information , that is, the size information of the point cloud cluster, including the length, width, height and heading angle of the BOX.

[0080] As a preferred embodiment of the present invention, the step (3) specifically includes the following steps:

[0081] (3.1) According to the Hungarian algorithm, calculate whether there is a related point cloud between the tracked target and the cluster. If so, continue to track the target and filter the target state, and enter step (4); otherwise, enter step (3.2);

[0082] (3.2) Further determine whether the current tracking point meets the track disappearance condition. If so, the target track is extrapolated and the BOX information is kept unchanged. Otherwise, the track is deleted directly.

[0083] (3.3) According to the association success result, the matching pair between the tracking target ID and the cluster ID number of the perceived target is obtained. .

[0084] As a preferred embodiment of the present invention, the step (3) further includes updating the state equation according to the obtained optimal matching pair:

[0085] ;

[0086] ;

[0087] ;

[0088] in, , , express H The transpose of represents the Kalman gain calculated at this moment, is the posterior state at this moment, is the prior state at this moment, is the posterior estimated covariance at this moment, is the prior estimated covariance at this moment, is the identity matrix.

[0089] As a preferred embodiment of the present invention, the first heading angle Specifically:

[0090] The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are denoted as [ ], and use this to establish a two-dimensional position feature point set P:

[0091] ;

[0092] Calculate the covariance matrix of P :

[0093] ,in ;

[0094] Compute the eigenvalues ​​and eigenvectors of the covariance matrix C:

[0095] ;

[0096] Among them, the matrix U The columns of the covariance matrix The unit orthogonal eigenvectors of , the eigenvalues ​​are arranged from large to small, and the first k eigenvalues ​​and corresponding eigenvectors are retained, which are the directions of the first k principal component vectors;

[0097] By calculating the single vehicle point cloud coordinate data (x k ,y k ), take the maximum eigenvalue, and its corresponding eigenvector is the vehicle heading direction.

[0098] As a preferred embodiment of the present invention, the second heading angle Specifically:

[0099] The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are recorded as , select the two farthest outliers and , draw a straight line Ld between the two points, take these two points as the upper and lower intervals, project all the points of the current obstacle onto the straight line Ld, and record the length of each point from the straight line Ld as L i , L i The point with the maximum value is recorded as ,by , , A rectangle is drawn for the vertex, wherein the direction of the long side obtained is the second heading angle.

[0100] As a preferred embodiment of the present invention, the third heading angle is specifically, the ratio of the lateral and longitudinal absolute speeds of the target vehicle is used as the third heading angle of the target vehicle. :

[0101] ;

[0102] According to the vehicle information obtained by the current 4D millimeter wave radar and the a posteriori information at this moment, the new ground speed of the current target (v gx ,v gy ), and use this to update the third heading angle .

[0103] As a preferred embodiment of the present invention, the step (5) is specifically to calculate the current frame fusion heading angle in the following manner: :

[0104] ;

[0105] in, , , are all information weights.

[0106] As a preferred embodiment of the present invention, the step (6) is: taking the fused heading angle of the current frame as a measurement value into the heading angle filter to calculate the target heading angle, and the heading angle filter uses a Kalman filter to perform multi-frame filtering, specifically:

[0107] ;

[0108] ;

[0109] ;

[0110] ;

[0111] ;

[0112] in, is the heading angle state at the previous moment, is the heading angle state predicted at this moment, is the estimated covariance at the previous moment, is the variance of process noise, is the prior estimated covariance of the forecast at this moment, is the variance of the measurement noise, is the Kalman gain calculated at this moment, is the target heading angle, is the posterior state of the heading angle at this moment, is the posterior error covariance of the heading angle at this moment.

[0113] like Figure 1 As shown, in actual application, the processing flow of this technical solution is as follows:

[0114] 1. Obtain 4D radar point cloud information;

[0115] 2. Cluster the acquired point cloud information and extract status information such as position, speed, RCS, etc.;

[0116] 3. Associate with the previous frame track:

[0117] Delete the tracks that have no associated point clouds and meet the track disappearance conditions;

[0118] For the track extrapolation without associated point cloud and not meeting the track disappearance condition, the track BOX information (heading angle, length, width and height) remains unchanged;

[0119] Filter the track states that have associated point clouds.

[0120] 4. Calculate the first, second and third heading angles for the track with associated point clouds:

[0121] The first heading angle is the result of PCA (Principal Component Analysis):

[0122] The radar point cloud cluster observation data contains multiple point clouds, denoted as [X ｉ , Y ｉ ], these points can establish a two-dimensional position feature point set:

[0123] ;

[0124] Compute the covariance matrix of P:

[0125] ,in ;

[0126] Compute the eigenvalues ​​and eigenvectors of the covariance matrix:

[0127] Since the covariance matrix is ​​a real symmetric square matrix, it is a normal matrix and can be directly diagonalized. The diagonalized main line elements are eigenvalues. According to the structure theorem of normal matrices, the following matrix decomposition exists:

[0128] ;

[0129] The columns of U are the unit orthogonal eigenvectors of the covariance matrix ∑, and the eigenvalues ​​are arranged from large to small. Therefore, the first k eigenvalues ​​and corresponding eigenvectors are retained, which are the directions of the first k principal component vectors.

[0130] In summary, by calculating the single vehicle point cloud coordinate data (x k ,y k ), and then take the maximum eigenvalue, and its corresponding eigenvector is the vehicle heading direction.

[0131] The second heading angle is the result of Lshape calculation

[0132] like Figure 3 As shown, the radar point cloud cluster observation data contains multiple point clouds, denoted as , select the two farthest outliers and , draw a straight line Ld between these two points, take these two points as the upper and lower intervals, project all points of the obstacle to Ld, and get the length of each point from Ld, record it as L ｉ， Get L ｉ The largest point is ,by , , Draw a rectangle for the vertex, where the long side is the second heading angle.

[0133] The third heading angle is the ratio of the target vehicle's lateral and longitudinal absolute speeds to indirectly obtain the target vehicle's heading angle.

[0134] The third heading angle of the target vehicle is indirectly obtained by the ratio of the lateral and longitudinal absolute speeds of the target vehicle, that is:

[0135] .

[0136] 5. Design the data fusion weights of the first, second and third heading angles based on the prior information such as cluster length and width, ground speed, etc., and perform data fusion to calculate the fused heading angle of this frame:

[0137] ;

[0138] , , It is the information weight, which is adjusted according to the target characteristics and scene information in engineering.

[0139] 6. The fused heading angle is used as a measurement value and brought into the heading angle filter to calculate the target heading angle. The heading angle filter uses a Kalman filter for multi-frame filtering.

[0140] In a specific embodiment of the present invention: deceleration target heading angle fitting calculation

[0141] The radar is installed forward, and the vehicle is close to the deceleration target in front. According to the method proposed by the present invention, the heading angle fitting process is as follows:

[0142] 1. Calculate the target prior state information based on the target tracking state information of the previous frame , the position, velocity and acceleration of the target in the vehicle coordinate system, namely:

[0143] ;

[0144] ;

[0145] in, is the state at the previous moment, is the prior state at this moment, is the estimated covariance at the previous moment, is the prior estimated covariance at this moment, A is the transition matrix, represents the transpose of A, Q is the variance of process noise, and this method actually uses the CA model, namely:

[0146] , T is the frame length.

[0147] 2. Use the improved DBSCAN clustering algorithm to cluster the 4D point cloud data, obtain the clustering results, and calculate the cluster related information, including the cluster center point status information That is, the x, y direction position and radial velocity information of the target in the vehicle coordinate system, BOX information That is, the size information of the point cloud cluster, the length, width, height of the BOX formed by the point cloud cluster, and the heading angle obtained by PCA (principal component analysis) and Lshape fitting , .

[0148] 3. Calculate the association between the tracking target and the cluster according to the Hungarian algorithm to obtain the matching pair of the tracking target and the perceived target. .

[0149] 4. Update the state equation according to the optimal matching pair:

[0150] ;

[0151] ;

[0152] ;

[0153] Among them, measurement That is, the x, y direction position and radial velocity information of the target in the vehicle coordinate system, , , represents the transpose of H represents the Kalman gain calculated at this moment, is the posterior state at this moment, is the prior state at this moment, is the posterior estimated covariance at this moment, is the prior estimated covariance at this moment, is the identity matrix.

[0154] 5. Calculate the target's ground speed (v) based on the vehicle information obtained by the radar and the a posteriori state information calculated in the previous step. gx ,v gy ).

[0155] 6. According to clustering , , ground speed and other prior information to calculate the fused heading angle of this frame, that is:

[0156] ;

[0157] in, , , is the information weight, , , They are cluster PCA (principal component analysis), Lshape fitting heading angle, .

[0158] 7. Take the fused heading angle as the measured value and bring it into the heading angle filter to calculate the target heading angle. The heading angle filter uses the Kalman filter to perform multi-frame filtering. The target heading angle Recorded as the target state.

[0159] The target heading angle fitting processing device of the 4D millimeter wave radar point cloud based on data fusion, wherein the device comprises:

[0160] a processor configured to execute computer executable instructions;

[0161] The memory stores one or more computer executable instructions. When the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion are implemented.

[0162] The processor for target heading angle fitting processing of the 4D millimeter wave radar point cloud based on data fusion, wherein the processor is configured to execute computer executable instructions, and when the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of the 4D millimeter wave radar point cloud based on data fusion are implemented.

[0163] The computer-readable storage medium stores a computer program thereon, and the computer program can be executed by a processor to implement the steps of the target heading angle fitting processing method of the 4D millimeter-wave radar point cloud based on data fusion described above.

[0164] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention belong.

[0165] It should be understood that each part of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution device.

[0166] A person of ordinary skill in the art may understand that all or part of the steps of the method for implementing the above-mentioned embodiment may be completed by instructing the relevant hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one of the steps of the method embodiment or a combination thereof.

[0167] The storage medium mentioned above can be a read-only memory, a magnetic disk or an optical disk, etc.

[0168] In the description of this specification, the description with reference to the terms "an embodiment", "some embodiments", "example", "specific example", or "embodiment" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0169] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

[0170] The target heading angle fitting processing method, device, processor and computer-readable storage medium of the 4D millimeter-wave radar point cloud based on data fusion of the present invention are adopted, and by placing a corner reflector at any position in the radar common illumination area, an equation or error optimization function is established to estimate the radar installation angle. The method proposed by the present invention makes full use of the characteristics of millimeter-wave radar, and performs data fusion and multi-frame association on the single-frame Lshape fitting heading angle, PCA (principal component analysis) fitting target heading angle and motion speed estimation fitting heading angle data, which significantly improves the accuracy of the target heading angle.

[0171] In this specification, the present invention has been described with reference to specific embodiments thereof. However, it is apparent that various modifications and variations may be made without departing from the spirit and scope of the present invention. Therefore, the specification and drawings should be regarded as illustrative rather than restrictive.

Claims

1. A target heading angle fitting processing method for 4D millimeter wave radar point cloud based on data fusion, characterized in that: The method comprises the following steps: (1) Obtain 4D millimeter wave radar point cloud information; (2) Cluster the acquired 4D millimeter-wave radar point cloud information and calculate the point cloud cluster features; (3) Associate the current tracking point with the track of the previous frame and determine whether the track disappearance condition is met; (4) For the track with associated point clouds, the first heading angle, the second heading angle, and the third heading angle are calculated respectively by principal component analysis, Lshape fitting, and the ratio of the lateral and longitudinal absolute velocities of the target vehicle; (5) According to the prior information of the cluster length and width of the current target and the ground speed, the data fusion weights of the first heading angle, the second heading angle and the third heading angle are designed respectively, so as to calculate the fusion heading angle of the current frame; (6) Substituting the fused heading angle as a measurement value into a heading angle filter to calculate a target heading angle; The step (1) is: according to the target tracking state information of the previous frame, calculate the prior state information of the current target ,in, They are the position, velocity and acceleration of the target in the vehicle coordinate system, specifically: ； ； ； in, is the state at the previous moment, is the prior state at this moment, is the estimated covariance at the previous moment, is the prior estimated covariance at this moment, A is the transition matrix, express A The transpose of , Q is the variance of process noise, T is the frame length; The step (2) is specifically as follows: The DBSCAN clustering algorithm is used to cluster the 4D millimeter wave point cloud data to obtain the clustering results and calculate the cluster related information, including the cluster center point status information. , i.e. the x, y position and radial velocity information of the target in the vehicle coordinate system; BOX information , that is, the size information of the point cloud cluster, including the length, width, height and heading angle of the BOX; The step (3) specifically includes the following steps: (3.1) According to the Hungarian algorithm, calculate whether there is a related point cloud between the tracked target and the cluster. If so, continue to track the target and filter the target state, and enter step (4); otherwise, enter step (3.2); (3.2) Further determine whether the current tracking point meets the track disappearance condition. If so, the target track is extrapolated and the BOX information is kept unchanged. Otherwise, the track is deleted directly. (3.3) According to the association success result, the matching pair between the tracking target ID and the cluster ID number of the perceived target is obtained. ; The step (3) further includes updating the state equation according to the obtained optimal matching pair: ； ； ； in, , , express H The transpose of represents the Kalman gain calculated at this moment, is the posterior state at this moment, is the prior state at this moment, is the posterior estimated covariance at this moment, is the prior estimated covariance at this moment, is the identity matrix.

2. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 1 is characterized in that: The first heading angle Specifically: The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are denoted as [ ], and use this to establish a two-dimensional position feature point set P: ； Calculate the covariance matrix of P : ,in ; Calculate the eigenvalues ​​and eigenvectors of the covariance matrix C: ； Among them, the matrix U The columns of the covariance matrix The unit orthogonal eigenvectors of , the eigenvalues ​​are arranged from large to small, and the first k eigenvalues ​​and corresponding eigenvectors are retained, which are the directions of the first k principal component vectors; By calculating the single vehicle point cloud coordinate data (x k ,y k ), take the maximum eigenvalue, and its corresponding eigenvector is the vehicle heading direction.

3. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 2 is characterized in that: The second heading angle Specifically: The multiple point clouds contained in the 4D millimeter wave radar point cloud cluster observation data are recorded as , select the two farthest outliers and , draw a straight line Ld between the two points, take these two points as the upper and lower intervals, project all the points of the current obstacle onto the straight line Ld, and record the length of each point from the straight line Ld as L i , L i The point with the maximum value is recorded as ,by , , A rectangle is drawn for the vertex, wherein the direction of the long side obtained is the second heading angle.

4. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 3 is characterized in that: The third heading angle is specifically, the ratio of the lateral and longitudinal absolute speeds of the target vehicle is used as the third heading angle of the target vehicle. : ； According to the vehicle information obtained by the current 4D millimeter wave radar and the a posteriori information at this moment, the new ground speed of the current target (v gx ,v gy ), and use this to update the third heading angle .

5. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 4 is characterized in that: The step (5) is specifically to calculate the current frame fusion heading angle in the following manner: : ； in, , , are all information weights.

6. The target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to claim 5 is characterized in that: The step (6) is: taking the fused heading angle of the current frame as a measurement value and bringing it into the heading angle filter to calculate the target heading angle. The heading angle filter uses a Kalman filter to perform multi-frame filtering, specifically: ； ； ； ； ； in, is the heading angle state at the previous moment, is the heading angle state predicted at this moment, is the estimated covariance at the previous moment, is the variance of process noise, is the prior estimated covariance of the forecast at this moment, is the variance of the measurement noise, is the Kalman gain calculated at this moment, is the target heading angle, is the posterior state of the heading angle at this moment, is the posterior error covariance of the heading angle at this moment.

7. A target heading angle fitting processing device for 4D millimeter wave radar point cloud based on data fusion, characterized in that: The device comprises: a processor configured to execute computer executable instructions; A memory storing one or more computer executable instructions, wherein when the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion according to any one of claims 1 to 6 are implemented.

8. A processor for target heading angle fitting processing of 4D millimeter wave radar point cloud based on data fusion, characterized in that: The processor is configured to execute computer executable instructions. When the computer executable instructions are executed by the processor, the steps of the target heading angle fitting processing method of 4D millimeter wave radar point cloud based on data fusion described in any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and the computer program can be executed by a processor to implement the steps of the target heading angle fitting processing method of a 4D millimeter wave radar point cloud based on data fusion as described in any one of claims 1 to 6.

Images