Synchronous online real-time calibration method for horizontal pitching installation error of 4D millimeter wave radar

By acquiring radar detection point attributes and vehicle body signals in real time, and using recursive least squares and derivation least squares methods to estimate the horizontal and pitch installation errors of the 4D millimeter-wave radar, the problems of environmental sensitivity and error coupling in the prior art are solved, and real-time and accurate installation error calibration is achieved.

CN119001633APending Publication Date: 2024-11-22HUAYU AUTOMOTIVE SYST
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411130685.1
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-16
Publication Date
2024-11-22

AI Technical Summary

Technical Problem

Existing 4D millimeter-wave radar calibration methods cannot effectively handle horizontal and pitch installation errors, and are sensitive to the environment, making it difficult to achieve real-time and accurate calibration.

Method used

By acquiring the attributes of radar detection points and vehicle body signals in real time, and combining recursive least squares estimation and recursive least squares method with attenuation factor, the horizontal and pitch installation angle errors are estimated, the error coupling effect is eliminated, and online real-time calibration is achieved.

Benefits of technology

It achieves real-time calibration that is insensitive to the environment, improves the accuracy of installation error angles, and can obtain the latest calibration value in real time during movement, making it suitable for all-weather environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119001633A_ABST
    Figure CN119001633A_ABST
Patent Text Reader

Abstract

The invention provides a synchronous on-line real-time calibration method for horizontal pitching installation errors of a 4D millimeter wave radar. The method comprises the following steps: acquiring attributes of a to-be-measured radar and a vehicle body signal in real time; according to the attribute of the detection point and the vehicle body signal, when the vehicle meets linear driving, performing coarse screening to obtain candidate stationary points; determining a corrected vehicle speed signal according to the candidate stationary point; according to the candidate stationary point and the corrected vehicle speed signal, estimating a horizontal installation angle error and a pitching installation angle error at the same time; counting a plurality of frames to obtain a mean value, a mode and a variance of pitching and horizontal mounting angle errors; according to the mean value, the mode and the variance, whether the output pitching and horizontal installation angle errors are updated or not is judged and output in real time. The synchronous online real-time calibration method for the horizontal pitching installation error of the 4D millimeter wave radar is not sensitive to the environment, and the horizontal pitching installation error can be accurately, synchronously and online estimated in real time.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of radar data processing, and in particular relates to a robust on-board 4D millimeter-wave radar horizontal and elevation installation error synchronous online real-time calibration method, which can be used in the field of radar data processing technology such as corner radar, forward radar, etc. Background Art

[0002] As the demand for intelligent driving continues to increase, environmental perception technology has become increasingly important. As a new product that has emerged in recent years, automotive 4D millimeter-wave radar has attracted widespread attention because of its all-weather operation, ability to continuously detect in adverse weather such as rain and snow, and ability to provide rich target information. However, the calibration of automotive 4D millimeter-wave radar is one of the key technologies to achieve its normal working ability.

[0003] The existing calibration methods can be roughly divided into the following categories: calibration methods based on the arrangement of corner reflectors in the external environment. This method requires the pre-arrangement of targets and is not suitable for real-time road calibration; calibration based on a single angle - horizontal or pitch. This method does not consider the impact of the angle error of horizontal-pitch coupling; based on the assumption of small horizontal and pitch angle errors, and the need to arrange corner reflectors, such assumptions are too strict; direct estimation of the calibration angle based on the vehicle speed signal. This method does not take into account the calibration error caused by inaccurate vehicle speed; based on direct angle statistical methods, this method is easily affected by the environment and may result in large deviations in the results. Summary of the invention

[0004] The object of the present invention is to provide a 4D millimeter wave radar horizontal pitch installation error synchronous online real-time calibration method, which is insensitive to the environment and can accurately and synchronously estimate the horizontal pitch installation error online in real time.

[0005] In order to achieve the above object, the present invention provides a 4D millimeter wave radar horizontal pitch installation error synchronous online real-time calibration method, comprising:

[0006] Step S1: providing a vehicle equipped with a vehicle-mounted 4D millimeter-wave radar, selecting a vehicle-mounted 4D millimeter-wave radar as a radar to be tested and establishing a corresponding vehicle coordinate system; obtaining in real time the pitch angle, horizontal angle, and radial velocity attributes of multiple detection points of the radar to be tested, as well as vehicle body signals;

[0007] Step S2: based on the attributes of the detection points and the vehicle body signal, when the vehicle meets the straight-line driving condition, a candidate stationary point is roughly screened from the detection points;

[0008] Step S3: determining a corrected vehicle speed signal according to the candidate stationary point;

[0009] Step S4: simultaneously estimating a horizontal installation angle error and a pitch installation angle error according to the candidate stationary point and the corrected vehicle speed signal;

[0010] Step S5: obtaining the pitch and horizontal installation angle errors of multiple frames by time accumulation, and obtaining the mean, mode and variance of the pitch and horizontal installation angle errors by statistics;

[0011] Step S6: judging whether to update the output pitch and horizontal installation angle errors according to the mean, mode and variance of the pitch and horizontal installation angle errors, and outputting the corresponding pitch and horizontal installation angle errors;

[0012] Step S7: Repeat steps S1 to S6 to output the pitch and horizontal installation angle errors in real time.

[0013] Preferably, the vehicle body signal includes the vehicle speed V in the vehicle body signal. ego and yaw rate ω ego ;

[0014] In step S2, the vehicle meets the straight-line driving condition, which means that: the vehicle speed V in the vehicle body information ego ≥T0, yaw rate in vehicle body information |ω ego |<T1 holds, T0 and T1 are the threshold values ​​of vehicle speed and yaw rate when the vehicle meets the straight-line driving condition;

[0015] The rough screening obtains the candidate stationary point, specifically including: the rough screening obtains the candidate stationary point P according to the following formula j , j is the ordinal number of the candidate stationary point, j = {1, ..., M}, M is the total number of candidate stationary points:

[0016]

[0017] Where Cos(·) represents the cosine operation, T2 is the candidate stationary point error threshold, V ego is the vehicle speed in the vehicle body information, j is the ordinal number of the candidate stationary point, V j , α j , β j They are respectively the radial velocity, pitch angle, and horizontal angle of the candidate stationary point.

[0018] Preferably, the step S3 specifically includes:

[0019] Step S31: Establish an approximate relationship between radial velocity and horizontal angle, and use recursive least squares to estimate the low-precision component V of the relative velocity of the stationary point based on the horizontal angle and radial velocity of the candidate stationary point. a , V b ;

[0020] The approximate relationship between radial velocity and horizontal angle is:

[0021]

[0022] Among them, V a , V b are low-precision components of the relative velocity of the stationary point, β j is the horizontal angle of the candidate stationary point, V j is the radial velocity of the candidate stationary point. Specifically, V a is the longitudinal component of the relative velocity of the candidate stationary point at the error angle, V b is the lateral component of the relative velocity of the candidate stationary point at the error angle;

[0023] Step S32: Based on the low-precision component V of the relative velocity of the stationary point a , V b Determine the corrected vehicle speed signal V est ;

[0024] Corrected vehicle speed signal V est for:

[0025]

[0026] Preferably, in step S31, a low-precision component V of the relative velocity of the stationary point is estimated by recursive least squares according to the horizontal angle and radial velocity of the candidate stationary point. a , V b , specifically including:

[0027] Step S311: Establish the least squares observation model y t =H t x t +v t ,

[0028] Among them, y t represents the observed value, which represents the radial velocity of the candidate stationary point, H t represents the observation matrix, H = [Cos(β t )Sin(β t )],β t is the horizontal angle of the tth loop index, x represents the state quantity,

[0029] Step S312: Utilize the M candidate stationary points to iterate through the iterative formula of the least squares observation model to obtain the optimal state quantity The estimated low-precision component V of the relative velocity of the stationary point a , V b are the optimal state quantities The first line and the second line

[0030] Preferably, the step S4 specifically includes:

[0031] Step S41: Establish an accurate relationship between radial velocity, pitch angle and horizontal angle, and use recursive least squares with attenuation factor to estimate the high-precision component V of the relative velocity of the stationary point. aa , V bb , V cc ;

[0032] The exact relationship between radial velocity, pitch angle and horizontal angle is:

[0033]

[0034] Among them, β j is the horizontal angle of the candidate stationary point, α j is the pitch angle of the candidate stationary point, V j is the radial velocity of the candidate stationary point;

[0035] Step S42: Based on the optimal state quantity Determine the normalized state quantity V f , and determine the normalized ideal vehicle speed vector V o ;

[0036] Step S43: Based on the normalized state quantity V f and the normalized ideal vehicle speed vector V o , determine the pitch installation angle error α offset and horizontal installation angle error β offset .

[0037] Preferably, in step S41, a recursive least square method with an attenuation factor is used to estimate the high-precision component V of the relative velocity of the stationary point. aa , V bb , V cc , specifically including:

[0038] Establish the least squares observation model y s =H s x s +v s ,

[0039] Among them, y s represents the observed value, which represents the radial velocity of the stationary point, H s represents the observation matrix, H s =[Cos(β s )Cos(α s) Cos(β s )Sin(α s ) Sin(α s )],x s Represents the state quantity,

[0040] The optimal state quantity is obtained by iterating the iterative formula of the least squares observation model with a decay factor using M candidate stationary points. The estimated high-precision component V of the relative velocity of the stationary point aa , V bb , V cc are the optimal state quantities The first, second and third rows of

[0041] The iterative formula of the least squares observation model with attenuation factor is:

[0042]

[0043]

[0044]

[0045] Among them, R s is the observation random noise v s The corresponding variance, P s-1 is the state quantity x s The estimated error covariance, K s is the gain, H s represents the observation matrix, y s represents the observed value, is the state quantity of the s-1th loop index, is the state quantity of the s-th loop index, the subscript s represents the loop index, s = {1, 2, ..., M}, M is the total number of candidate stationary points, I is the identity matrix applicable to matrix operations, p e For the forgetting factor.

[0046] Preferably, in step S43, the pitch installation angle error α is determined offset and horizontal installation angle error β offset , specifically including:

[0047] Step S431: Define correction factor Interaction vector V cross =V f *V o , then the rotation result matrix R es for:

[0048]

[0049] Among them, Eye is the unit matrix, c is the correction factor, V iv is the interaction vector V cross The transformation result of

[0050]

[0051] Among them, V cross is the interaction vector, where V cross [0], V cross [1] V cross [2] represent the interaction results of different dimensions in the interaction vector, V cross [0] is the pitch dimension interaction result, V cross [1] is the horizontal dimension interaction result, V cross [2] is the tumbling dimension interaction result;

[0052] Step S432: According to the rotation result matrix R es , get the pitch installation angle error α offset and horizontal installation angle error β offset ;

[0053] Pitch installation angle error α offset for:

[0054] α offset = atan(R es [1][0],R es [0][0]),

[0055] Among them, R es [0][0] represents the single-dimensional result of the first dimension of the rotation result matrix, R es [1][0] represents the interactive dimension result of the first and second dimensions of the rotation result matrix;

[0056] Horizontal installation angle error β offset for:

[0057] β offset =atan(-R es [2][0],sqrt(R es [2][1]*R es [2][1]+R es [2][2]*R es [2][2])),

[0058] Among them, atan(·) represents the inverse tangent operation, sqrt(·) represents the square root operation, Eye is the unit matrix, R es [2][2] represents the single-dimensional result of the third dimension of the rotation result matrix, Res [2][0] represents the interactive dimension result of the third and first dimensions of the rotation result matrix, R es [2][1] represents the interactive dimension result of the third and second dimensions of the rotation result matrix.

[0059] Preferably, step S5 specifically includes:

[0060] Step S51: obtaining the pitch and horizontal installation angle errors of multiple frames through time accumulation, and establishing a corresponding installation angle error histogram;

[0061] Step S52: Obtain the mean, mode and variance of the pitch and horizontal installation angle errors according to the installation angle error histogram statistics.

[0062] Preferably, obtaining the pitch and horizontal installation angle errors of multiple frames by time accumulation specifically includes: obtaining the pitch installation angle error α of a single frame by performing steps S1 to S4 once at a single time point offset and horizontal installation angle error β offset , looping through steps S1 to S4 at multiple time points to accumulate multiple frames of pitch and horizontal installation angle errors to form a time series of installation angle errors;

[0063] Among the multi-frame pitch and horizontal installation angle errors, the time series of the pitch installation angle error is: The time series of horizontal installation angle error is: Where L is the length of the time series;

[0064] Establish the corresponding installation angle error histogram, specifically including: pre-opening a section of memory, in which the bin size and number of the histogram are set; offset and horizontal installation angle error β offset They are all projected into the pre-established histogram to obtain the corresponding installation angle error histogram.

[0065] Preferably, in step S52, when the length L of the time series is greater than or equal to the time series threshold T3, the mean values ​​Meanα, Meanβ, and the mode Mode of the pitch installation angle error and the horizontal installation angle error are calculated. α , Mode β and variance Var α , Var β ;

[0066] The mean of the pitch installation angle error is α and variance Var α for:

[0067]

[0068]

[0069] Among them, N bin is the number of histogram bins, m is the ordinal number of the histogram bin, is the number of projection angles corresponding to the m-th histogram bin of the installation angle error histogram of the pitch installation angle error, S binα_m is the median of the m-th histogram bin of the installation angle error histogram of the pitch installation angle error;

[0070] The mean Mean β and variance Var β of the horizontal installation angle error are:

[0071]

[0072]

[0073] Among them, M bin is the number of histogram bins, M binβ_l is the number of projection angles corresponding to the th histogram bin of the installation angle error histogram of the horizontal installation angle error, S binβ_l is the th histogram bin n of the installation angle error histogram of the horizontal installation angle error.

[0074] Preferably, in the step S6, if |Mean α -Mode α | < T4, and Var α ≤ T5, at this time, update the output pitch installation angle error, where T4 and T5 are the error threshold between the mean and the mode of the pitch installation angle error, and the variance threshold respectively, Mean α , Mode α are the mean and the mode of the pitch installation angle error respectively, Var α is the variance of the pitch installation angle error; otherwise, keep the output pitch installation angle error unchanged;

[0075] If |Mean β -Mode β | < T6, Var β ≤ T7, at this time, update the output horizontal installation angle error, where T6 and T7 are the error threshold between the mean and the mode of the horizontal installation angle error, and the variance threshold respectively, Mean β , Mode β are the mean and the mode of the horizontal installation angle error respectively, Var βis the variance of the horizontal installation angle error; otherwise, keep the output horizontal installation angle error unchanged.

[0076] The present invention also provides a computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the above-mentioned 4D millimeter-wave radar horizontal pitch installation error synchronous online real-time calibration method.

[0077] The 4D millimeter-wave radar horizontal and pitch installation error synchronous online real-time calibration method of the present invention makes full use of multiple detection point information of the 4D millimeter-wave radar to estimate the vehicle speed, can estimate the corrected vehicle speed in real time, and simultaneously estimate the horizontal and pitch installation angle errors in combination with the corrected vehicle speed and the projection relationship of the radar on the radial speed, and does not need to make assumptions about the angle errors, eliminates the coupling influence of the angle errors between the horizontal and pitch, and improves the accuracy of the installation error angle calibration; even if the radar has additional installation deviations during the movement process, the latest calibration value can be obtained in real time, and real-time online robustness is achieved; in addition, there is no need to arrange auxiliary corner reflectors in advance, nor is there a need for straight guardrails around, the vehicle is not affected by changes in the environment and the calibration accuracy is not affected, it is insensitive to environmental conditions, is all-weather and easy to implement in engineering, and environmental robustness is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] Figure 1 The present invention is a flowchart of a method for synchronous online real-time calibration of 4D millimeter wave radar horizontal and elevation installation errors according to an embodiment of the present invention.

[0079] Figure 2 1 is a schematic diagram of the positional relationship between a vehicle coordinate system and an installation position of a radar according to an embodiment of the present invention, wherein a blue rectangle represents a tire and a triangle represents a radar.

[0080] Figure 3 is the installation angle error histogram of the horizontal installation angle error.

[0081] Figure 4 is the installation angle error histogram of the pitch installation angle error. DETAILED DESCRIPTION

[0082] The present invention is further described below in conjunction with specific examples. It should be understood that the following examples are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0083] like Figure 1 The figure shows a method for synchronous online real-time calibration of 4D millimeter wave radar horizontal pitch installation error according to an embodiment of the present invention, comprising the following steps:

[0084] Step S1: Provide a vehicle equipped with a vehicle-mounted 4D millimeter-wave radar, select a vehicle-mounted 4D millimeter-wave radar as the radar to be tested and establish a corresponding vehicle coordinate system; obtain the pitch angle α of N detection points of the radar to be tested in real time i , horizontal angle β i and radial velocity V i These attributes, as well as the vehicle body signal, i is the ordinal number of the detection point, i={1, ..., N}, N is the number of detection points and is a positive integer greater than 1.

[0085] The N detection points of the 4D millimeter wave radar have completed distance measurement, speed measurement and angle measurement, and each detection point includes the pitch angle α of the detection point i , horizontal angle β i and radial velocity V i Horizontal angle β i This is what is commonly referred to as the azimuth angle. The vehicle body signal includes the vehicle speed V in the vehicle body signal. ego and yaw rate ω ego , which is public and comes from the vehicle body, preferably directly from the CAN (Controller Area Network) in the chassis domain, and is directly input as a calibrated vehicle body signal.

[0086] In this embodiment, the 4D millimeter wave radar is selected as the front radar as the radar to be tested. The description of the established vehicle coordinate system and its installation position can be found in Figure 2 , wherein the ideal installation angle, i.e., the horizontal angle and the pitch angle, are both 0 degrees, the blue rectangle represents the tire, and the triangle represents the radar. In this embodiment, the errors of the horizontal angle and the pitch angle are both -3 degrees according to the preset horizontal and pitch calibration instruments.

[0087] Step S2: Based on the attributes of the detection points and the vehicle body signals, and when the vehicle meets the straight-line driving condition, candidate stationary points are roughly screened from the detection points.

[0088] It should be noted that the stationary point represents the reflection point of an absolutely stationary target, and the absolutely stationary target is defined as an absolutely stationary target in the world coordinate system.

[0089] The vehicle meets the straight-line driving condition when: the vehicle speed V in the vehicle body information ego ≥T0, yaw rate in vehicle body information |ω ego |<T1 holds, T0 and T1 are the threshold values ​​of vehicle speed and yaw angular velocity when the vehicle meets the straight-line driving condition.

[0090] The method of obtaining candidate stationary points by rough screening is mainly to compensate the radial velocity of the target point according to the vehicle speed, and then determine whether it is a candidate stationary point. The absolute radial velocity after compensation is For the stationary point, the compensated absolute radial velocity value fluctuates around 0. In the embodiment, the candidate stationary point is obtained by coarse screening according to the candidate stationary point error threshold T2.

[0091] Therefore, the rough screening to obtain the candidate stationary point specifically includes: rough screening to obtain the candidate stationary point P according to the following formula j , j is the ordinal number of the candidate stationary point, j = {1, ..., M}, M is the total number of candidate stationary points:

[0092]

[0093] Where Cos(·) represents the cosine operation, T2 is the candidate stationary point error threshold, V ego is the vehicle speed in the vehicle body information, j is the ordinal number of the candidate stationary point, V j , α j , β j They are respectively the radial velocity, pitch angle, and horizontal angle of the candidate stationary point.

[0094] In this embodiment, the vehicle speed threshold T0 is set to 6 m / s when the vehicle meets the straight-line driving condition, the yaw rate threshold T1 is set to 0.1 rad / s when the vehicle meets the straight-line driving condition, and the preselected stationary point error threshold T2 is set to 0.2 m / s.

[0095] Step S3: Determine the corrected vehicle speed signal V according to the candidate stationary point est .

[0096] The step S3 specifically includes:

[0097] Step S31: Establish an approximate relationship between radial velocity and horizontal angle, and use recursive least squares to estimate the low-precision component V of the relative velocity of the stationary point based on the horizontal angle and radial velocity of the candidate stationary point. a , V b .

[0098] The approximate relationship between radial velocity and horizontal angle is:

[0099]

[0100] Among them, V a , V b are low-precision components of the relative velocity of the stationary point, β j is the horizontal angle of the candidate stationary point, V j is the radial velocity of the candidate stationary point. Specifically, V a is the longitudinal component of the relative velocity of the candidate stationary point at the error angle, V bis the lateral component of the relative velocity of the candidate stationary point at the error angle.

[0101] It should be noted that, in other embodiments, whether for the corner radar or the front radar, V j Both represent radial velocity, and V j All satisfy the above formula.

[0102] Step S32: Based on the low-precision component V of the relative velocity of the stationary point a , V b Determine the corrected vehicle speed signal V est .

[0103] Corrected vehicle speed signal V est for:

[0104]

[0105] Among them, V a , V b Both are low-precision components of the relative velocity of the stationary point.

[0106] In step S31, based on the horizontal angle and radial velocity of the candidate stationary point, a low-precision component V of the relative velocity of the stationary point is estimated by recursive least squares. a , V b , specifically including:

[0107] Step S311: Establish the least squares observation model y t =H t x t +v t ,

[0108] Among them, y t represents the observed value, and in this embodiment represents the radial velocity of the candidate stationary point, H t represents the observation matrix. In this embodiment, H = [Cos(β t ) Sin(β t )],β t is the horizontal angle of the t-th loop index, x represents the state quantity, and in this embodiment represents the coefficient to be estimated, that is,

[0109] The iterative formula of the least squares observation model is:

[0110]

[0111]

[0112] P t =(IK t )H t Pt-1 (3)

[0113] Among them, R t is the observation random noise v t The corresponding variance, P t-1 is the estimated error covariance of the state variable x, K t is the gain, H t represents the observation matrix, y t represents the observed value, is the state quantity of the t-1th loop index, is the state quantity of the t-th loop index, I is the unit matrix, where the subscript t represents the loop index, t={1, 2, ..., M}, and M is the total number of candidate stationary points.

[0114] Step S312: Using the M candidate stationary points, the iterative formula of the least squares observation model (i.e., formula (1) to formula (3)) is iterated to obtain the optimal state quantity The estimated low-precision component V of the relative velocity of the stationary point a , V b are the optimal state quantities The first line and the second line

[0115] In step S2, the corrected vehicle speed signal V est for:

[0116] in, and are the optimal state quantities The first and second rows.

[0117] Step S4: Based on the candidate stationary point and the corrected vehicle speed signal V est , and simultaneously estimate the horizontal installation angle error and the pitch installation angle error.

[0118] The step S4 specifically includes:

[0119] Step S41: Establish an accurate relationship between radial velocity, pitch angle and horizontal angle, and use recursive least squares with attenuation factor to estimate the high-precision component V of the relative velocity of the stationary point. aa , V bb , V cc ;

[0120] Among them, the exact relationship between radial velocity, pitch angle and horizontal angle is:

[0121]

[0122] Among them, β j is the horizontal angle of the candidate stationary point, α j is the pitch angle of the candidate stationary point, V j is the radial velocity of the candidate stationary point.

[0123] In step S41, a recursive least square method with an attenuation factor is used to estimate the high-precision component V of the relative velocity of the stationary point. aa , V bb , V cc , specifically including:

[0124] Establish the least squares observation model y s =H s x s +v s ,

[0125] Among them, y s represents the observed value, which in this embodiment represents the radial velocity of the stationary point, H s Represents the observation matrix. In this embodiment, H s =[Cos(β s )Cos(α s ) Cos(β s )Sin(α s ) Sin(α s )],x s represents the state quantity, and in this embodiment, represents the coefficient to be estimated, that is,

[0126] The iterative formula of the least squares observation model with attenuation factor is:

[0127]

[0128]

[0129]

[0130] Among them, R s is the observation random noise v s The corresponding variance, P s-1 is the state quantity x s The estimated error covariance, K s is the gain, H s represents the observation matrix, y s represents the observed value, is the state quantity of the s-1th loop index, is the state quantity of the s-th loop index, the subscript s represents the loop index, s = {1, 2, ..., M}, M is the total number of candidate stationary points, I is the identity matrix applicable to matrix operations, pe For the forgetting factor.

[0131] Using M candidate stationary points, the iterative formula of the least squares observation model with a decay factor (Formula (4) to Formula (6)) is iterated to obtain the optimal state quantity The estimated high-precision component V of the relative velocity of the stationary point aa , V bb , V cc are the optimal state quantities The first, second, and third rows of .

[0132] In other embodiments, the high-precision component V of the relative velocity of the stationary point can also be solved directly based on the matrix inversion operation instead of the recursive least squares method. aa , V bb , V cc , and the low-precision component V of the relative velocity at the stationary point a , V b However, this will result in a large matrix and require more stack space during execution. In addition, matrix-based calculation methods require that the matrix be invertible. These requirements are relatively strict and difficult to implement in embedded systems.

[0133] Step S42: Based on the optimal state quantity Determine the normalized state quantity V f , and determine the normalized ideal vehicle speed vector V o ;

[0134] Among them, the normalized state quantity V f By the optimal state Normalized to get the normalized state quantity V f for:

[0135]

[0136] Normalized ideal vehicle speed vector V o for:

[0137]

[0138] Step S43: Based on the normalized state quantity V f and the normalized ideal vehicle speed vector V o , determine the pitch installation angle error α offset and horizontal installation angle error β offset .

[0139] According to the normalized state quantity V f and the normalized ideal vehicle speed vector V o The rotational projection relationship is used to determine the pitch installation angle error αoffset and horizontal installation angle error β offset , specifically including:

[0140] Step S431: Define correction factor Interaction vector V cross =V f *V o , then the rotation result matrix R es for:

[0141]

[0142] Among them, Eye is the unit matrix, c is the correction factor, V iv is the interaction vector V cross The transformation result of

[0143]

[0144] Among them, V cross is the interaction vector, where V cross [0], V cross [1] V cross [2] represent the interaction results of different dimensions in the interaction vector, V cross [0] is the pitch dimension interaction result, V cross [1] is the horizontal dimension interaction result, V cross [2] is the result of the tumbling dimension interaction.

[0145] Step S432: According to the rotation result matrix R es , get the pitch installation angle error α offset and horizontal installation angle error β offset ;

[0146] Pitch installation angle error α offset for:

[0147] α offset = atan(R es [1][0],R es [0][0]),

[0148] Among them, R es [0][0] represents the single-dimensional result of the first dimension of the rotation result matrix, R es [1][0] represents the interaction dimension result of the first and second dimensions of the rotation result matrix.

[0149] Horizontal installation angle error β offset for:

[0150] β offset =atan(-R es[2][0],sqrt(R es [2][1]*R es [2][1]+R es [2][2]*R es [2][2])),

[0151] Among them, atan(·) represents the inverse tangent operation, sqrt(·) represents the square root operation, Eye is the unit matrix, R es [2][2] represents the single-dimensional result of the third dimension of the rotation result matrix, R es [2][0] represents the interactive dimension result of the third and first dimensions of the rotation result matrix, R es [2][1] represents the interactive dimension result of the third and second dimensions of the rotation result matrix.

[0152] Step S5: Obtain the pitch and horizontal installation angle errors of multiple frames by time accumulation, and obtain the mean, mode and variance of the pitch and horizontal installation angle errors by statistics.

[0153] Step S5 specifically includes:

[0154] Step S51: obtaining the pitch and horizontal installation angle errors of multiple frames through time accumulation, and establishing a corresponding installation angle error histogram.

[0155] The pitch and horizontal installation angle errors of multiple frames are obtained by time accumulation, specifically including: performing steps S1 to S4 once at a single time point to obtain the pitch installation angle error α of a single frame offset and horizontal installation angle error β offset ,, executing step S1 to step S4 cyclically at multiple time points to accumulate multiple frames of pitch and horizontal installation angle errors to form a time series of installation angle errors;

[0156] Among the multi-frame pitch and horizontal installation angle errors, the time series of the pitch installation angle error is: The time series of horizontal installation angle error is: Where L is the length of the time series.

[0157] It should be noted that, since step S1 acquires the properties of the detection point and the vehicle body signal in real time, the data obtained in steps S1 to S4 can be expressed in the form of a time series, and each frame of the time series has a pitch and horizontal installation angle error; as time goes by, multiple frames of horizontal and pitch installation angle errors will be accumulated, and L represents the length of the time series (i.e., the accumulated number of frames). There are only two time series in the pitch and horizontal installation angle errors of multiple frames, namely, the time series of the pitch installation angle error and the time series of the horizontal installation angle error, and the length of both time series is L.

[0158] Establish the corresponding installation angle error histogram, specifically including: pre-opening a section of memory, in which the bin size and number of the histogram are set; offset and horizontal installation angle error β offset They are all projected into the pre-established histogram to obtain the corresponding installation angle error histogram.

[0159] The installation angle error histograms of the horizontal and elevation installation angle errors are obtained as follows: Figure 3 and Figure 4 shown.

[0160] Step S52: Obtain the mean, mode and variance of the pitch and horizontal installation angle errors according to the installation angle error histogram statistics.

[0161] When the length of the time series L ≥ the time series threshold T3, calculate the mean of the pitch installation angle error and the horizontal installation angle error. α , Mean β 、Mode α , Mode β and variance Var α , Var β .

[0162] In this embodiment, the time series threshold T3 is set to 800.

[0163] The mean of the pitch installation angle error is α and variance Var α for:

[0164]

[0165]

[0166] Among them, N bin is the number of histogram bins, m is the ordinal number of the histogram bin, is the number of projection angles corresponding to the mth histogram bin of the installation angle error histogram of the pitch installation angle error, S binα_m The median of the mth histogram bin of the installation angle error histogram of the pitch installation angle error. The median is to arrange the given set of numbers from small to large or from large to small, take the middle number if there are odd numbers, and take the average of the two middle numbers if there are even numbers.

[0167] The mean value of the horizontal installation angle error is β and variance Var β for:

[0168]

[0169]

[0170] Among them, M bin is the number of histogram bins, and M binβ_l is the number of projection angles corresponding to the, -th histogram bin of the installation angle error histogram of the horizontal installation angle error, and S binβ_l is the median of the, -th histogram bi n of the installation angle error histogram of the horizontal installation angle error.

[0171] Step S6: Determine whether to update the output pitch and horizontal installation angle errors based on the mean, mode, and variance of the pitch and horizontal installation angle errors, and output the corresponding pitch and horizontal installation angle errors.

[0172] Among them, if |Mean α - Mode α | < T4, and the corresponding variance Var α ≤ T5, at this time, update the output pitch installation angle error, where T4 and T5 are the error threshold between the mean and mode of the pitch installation angle error, and the variance threshold, respectively; otherwise, keep the output pitch installation angle error unchanged.

[0173] Among them, the output pitch installation angle error can be updated to the mean, mode, or the average of the mean and mode of the pitch installation angle error. In this embodiment, the output pitch installation angle error is updated to the mean of the pitch installation angle error to replace the previous pitch installation angle error.

[0174] Similarly, if |Mean β - Mode β | < T6, Var β ≤ T7, at this time, update the output horizontal installation angle error, where T6 and T7 are the error threshold between the mean and mode of the horizontal installation angle error, and the variance threshold, respectively; otherwise, keep the output horizontal installation angle error unchanged.

[0175] Similarly, the output horizontal installation angle error can be updated to the mean, mode, or the average of the mean and mode of the horizontal installation angle error. In this embodiment, the output horizontal installation angle error is updated to the mean of the horizontal installation angle error.

[0176] In this embodiment, T4 is set to 0.2 deg, T5 is set to 0.1, T6 is set to 0.2 deg, and T7 is set to 0.1.

[0177] Step S7: Repeat steps S1 to S6 to output the pitch and horizontal installation angle errors in real time.

[0178] The time interval for outputting the pitch and horizontal installation angle errors is the time interval corresponding to the length L of the time series.

[0179] The 4D millimeter-wave radar horizontal and pitch installation error synchronous online real-time calibration method of the present invention makes full use of multiple detection point information of the 4D millimeter-wave radar to estimate the vehicle speed, can estimate the corrected vehicle speed in real time, and simultaneously estimate the horizontal and pitch installation angle errors in combination with the corrected vehicle speed and the projection relationship of the radar on the radial speed, and does not need to make assumptions about the angle errors, eliminates the coupling influence of the angle errors between the horizontal and pitch, and improves the accuracy of the installation error angle calibration; even if the radar has additional installation deviations during the movement process, the latest calibration value can be obtained in real time, and real-time online robustness is achieved; in addition, there is no need to arrange auxiliary corner reflectors in advance, nor is there a need for straight guardrails around, the vehicle is not affected by changes in the environment and the calibration accuracy is not affected, it is insensitive to environmental conditions, is all-weather and easy to implement in engineering, and environmental robustness is achieved.

[0180] In addition, the present invention also provides a computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the above-mentioned 4D millimeter-wave radar horizontal pitch installation error synchronous online real-time calibration method.

[0181] The above is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. The above embodiments of the present invention can also be modified in various ways. All simple, equivalent changes and modifications made according to the claims and the description of the present invention fall within the scope of protection of the claims of the present invention. The contents not described in detail in the present invention are all conventional technical contents.

Claims

1. A synchronous online real-time calibration method for 4D millimeter wave radar horizontal pitch installation error, It is characterized in that include: Step S1: providing a vehicle equipped with a vehicle-mounted 4D millimeter-wave radar, selecting a vehicle-mounted 4D millimeter-wave radar as a radar to be tested and establishing a corresponding vehicle coordinate system; obtaining in real time the pitch angle, horizontal angle, and radial velocity attributes of multiple detection points of the radar to be tested, as well as vehicle body signals; Step S2: based on the attributes of the detection points and the vehicle body signal, when the vehicle meets the straight-line driving condition, a candidate stationary point is roughly screened from the detection points; Step S3: determining a corrected vehicle speed signal according to the candidate stationary point; Step S4: simultaneously estimating a horizontal installation angle error and a pitch installation angle error according to the candidate stationary point and the corrected vehicle speed signal; Step S5: obtaining the pitch and horizontal installation angle errors of multiple frames by time accumulation, and obtaining the mean, mode and variance of the pitch and horizontal installation angle errors by statistics; Step S6: judging whether to update the output pitch and horizontal installation angle errors according to the mean, mode and variance of the pitch and horizontal installation angle errors, and outputting the corresponding pitch and horizontal installation angle errors; Step S7: Repeat steps S1 to S6 to output the pitch and horizontal installation angle errors in real time.

2. According to the method for synchronous online real-time calibration of the horizontal and elevation installation errors of 4D millimeter wave radars according to claim 1, It is characterized in that The vehicle body signal includes the vehicle speed V in the vehicle body signal ego and yaw rate ω ego ; In step S2, the vehicle meets the straight-line driving condition, which means that: the vehicle speed V in the vehicle body information ego ≥T 0 , the yaw rate in the vehicle body information |ω ego | <T 1 Established, T 0 ,T 1 are respectively the threshold values ​​of vehicle speed and yaw rate when the vehicle meets the straight-line driving condition; The rough screening obtains the candidate stationary point, specifically including: the rough screening obtains the candidate stationary point P according to the following formula j , j is the ordinal number of the candidate stationary point, j = {1, ..., M}, M is the total number of candidate stationary points: Where Cos(·) represents the cosine operation, T 2 is the candidate stationary point error threshold, V ego is the vehicle speed in the vehicle body information, j is the ordinal number of the candidate stationary point, V j , α j , β j They are respectively the radial velocity, pitch angle, and horizontal angle of the candidate stationary point.

3. According to the method for synchronous online real-time calibration of the horizontal and elevation installation errors of 4D millimeter wave radars according to claim 1, It is characterized in that The step S3 specifically includes: Step S31: Establish an approximate relationship between radial velocity and horizontal angle, and use recursive least squares to estimate the low-precision component V of the relative velocity of the stationary point based on the horizontal angle and radial velocity of the candidate stationary point. a ,V b ; The approximate relationship between radial velocity and horizontal angle is: Among them, V a ,V b are low-precision components of the relative velocity of the stationary point, β j is the horizontal angle of the candidate stationary point, V j is the radial velocity of the candidate stationary point; Step S32: Based on the low-precision component V of the relative velocity of the stationary point a ,V b Determine the corrected vehicle speed signal V est ; Corrected vehicle speed signal V est for:

4. According to the method for synchronous online real-time calibration of 4D millimeter wave radar horizontal pitch installation error according to claim 1, It is characterized in that In step S31, based on the horizontal angle and radial velocity of the candidate stationary point, a low-precision component V of the relative velocity of the stationary point is estimated by recursive least squares. a ,V b , specifically including: Step S311: Establish the least squares observation model y t =H t x t +v t , Among them, y t represents the observed value, which represents the radial velocity of the candidate stationary point, H t represents the observation matrix, H = [Cos(β t )Sin(β t )],β t is the horizontal angle of the tth loop index, x represents the state quantity, Step S312: Utilize the M candidate stationary points to iterate through the iterative formula of the least squares observation model to obtain the optimal state quantity The estimated low-precision component V of the relative velocity of the stationary point a ,V b are the optimal state quantities The first line and the second line 5. According to claim 1, the 4D millimeter wave radar horizontal pitch installation error synchronous online real-time calibration method, It is characterized in that The step S4 specifically includes: Step S41: Establish an accurate relationship between radial velocity, pitch angle and horizontal angle, and use recursive least squares with attenuation factor to estimate the high-precision component V of the relative velocity of the stationary point. aa ,V bb ,V cc ; The exact relationship between radial velocity, pitch angle and horizontal angle is: Among them, β j is the horizontal angle of the candidate stationary point, α j is the pitch angle of the candidate stationary point, V j is the radial velocity of the candidate stationary point; Step S42: Based on the optimal state quantity Determine the normalized state quantity V f , and determine the normalized ideal vehicle speed vector V o ; Step S43: Based on the normalized state quantity V f and the normalized ideal vehicle speed vector V o , determine the pitch installation angle error α offset and horizontal installation angle error β offset .

6. According to the method for synchronous online real-time calibration of 4D millimeter wave radar horizontal pitch installation error according to claim 5, It is characterized in that In step S41, a recursive least square method with an attenuation factor is used to estimate the high-precision component V of the relative velocity of the stationary point. aa ,V bb ,V cc , including: Establish the least squares observation model y s =H s x s +v s , Among them, y s represents the observed value, which represents the radial velocity of the stationary point, H s represents the observation matrix, H s =[Cos(β s )Cos(α s )Cos(β s )Sin(α s ) Sin(α s )],x s Represents the state quantity, The optimal state quantity is obtained by iterating the iterative formula of the least squares observation model with a decay factor using M candidate stationary points. The estimated high-precision component V of the relative velocity of the stationary point aa ,V bb ,V cc are the optimal state quantities The first, second and third rows of The iterative formula of the least squares observation model with attenuation factor is: Among them, R s is the observation random noise v s The corresponding variance, P s-1 is the state quantity x s The estimated error covariance, K s is the gain, H s represents the observation matrix, y s represents the observed value, is the state quantity of the s-1th loop index, is the state quantity of the s-th loop index, the subscript s represents the loop index, s = {1, 2, ..., M}, M is the total number of candidate stationary points, I is the identity matrix applicable to matrix operations, p e For the forgetting factor.

7. According to claim 5, the 4D millimeter wave radar horizontal pitch installation error synchronous online real-time calibration method, It is characterized in that In step S43, the pitch installation angle error α is determined. offset and horizontal installation angle error β offset , specifically including: Step S431: Define correction factor Interaction vector v cross =V f *V o , then the rotation result matrix R es for: Among them, Eye is the unit matrix, c is the correction factor, V iv is the interaction vector V cross The transformation result of Among them, V cross is the interaction vector, where V cross [0], V cross [1] V cross [2] represent the interaction results of different dimensions in the interaction vector, V cross [0] is the pitch dimension interaction result, V cross [1] is the horizontal dimension interaction result, V cross [2] is the tumbling dimension interaction result; Step S432: According to the rotation result matrix R es , get the pitch installation angle error α offset and horizontal installation angle error β offset ; Pitch installation angle error α offset for: a offset =atan(R es [1][0],R es [0][0]), Among them, R es [0][0] represents the single-dimensional result of the first dimension of the rotation result matrix, R es [1][0] represents the interactive dimension result of the first and second dimensions of the rotation result matrix; Horizontal installation angle error β offset for: β offset =atan(-R es [2][0],sqrt(R es [2][1]*R es [2][1]+R es [2][2]*R es [2][2])), Among them, atan(·) represents the inverse tangent operation, sqrt(·) represents the square root operation, Eye is the unit matrix, R es [2][2] represents the single-dimensional result of the third dimension of the rotation result matrix, R es [2][0] represents the interactive dimension result of the third and first dimensions of the rotation result matrix, R es [2][1] represents the interactive dimension result of the third and second dimensions of the rotation result matrix.

8. According to claim 1, the 4D millimeter wave radar horizontal pitch installation error synchronous online real-time calibration method, It is characterized in that Step S5 specifically includes: Step S51: obtaining the pitch and horizontal installation angle errors of multiple frames through time accumulation, and establishing a corresponding installation angle error histogram; Step S52: Obtain the mean, mode and variance of the pitch and horizontal installation angle errors according to the installation angle error histogram statistics.

9. The method for synchronous online real-time calibration of the horizontal and elevation installation errors of a 4D millimeter wave radar according to claim 8, It is characterized in that The pitch and horizontal installation angle errors of multiple frames are obtained by time accumulation, specifically including: performing steps S1 to S4 once at a single time point to obtain the pitch installation angle error α of a single frame offset and horizontal installation angle error β offset , looping through steps S1 to S4 at multiple time points to accumulate multiple frames of pitch and horizontal installation angle errors to form a time series of installation angle errors; Among the multi-frame pitch and horizontal installation angle errors, the time series of the pitch installation angle error is: The time series of horizontal installation angle error is: Where L is the length of the time series; Establish the corresponding installation angle error histogram, specifically including: pre-opening a section of memory, in which the bin size and number of the histogram are set; offset and horizontal installation angle error β offset They are all projected into the pre-established histogram to obtain the corresponding installation angle error histogram; In step S52, when the length of the time series L ≥ the time series threshold T 3 When , calculate the mean of the pitch installation angle error and the horizontal installation angle error Mean α ,Mean β 、Mode α ,Mode β and variance Var α ,Var β ; The mean of the pitch installation angle error is α and variance Var α for: Among them, N bin is the number of histogram bins, m is the ordinal number of the histogram bin, is the number of projection angles corresponding to the mth histogram bin of the installation angle error histogram of the pitch installation angle error, S binα_m is the median value of the mth histogram bin of the installation angle error histogram of the pitch installation angle error; The mean value of the horizontal installation angle error is β and variance Var β for: Among them, N bin is the number of histogram bins, M binγ_l is the number of projection angles corresponding to the lth histogram bin of the installation angle error histogram of the horizontal installation angle error, S binβ_l is the median of the lth histogram bin of the installation angle error histogram of the horizontal installation angle error.

10. The 4D millimeter wave radar horizontal pitch installation error synchronous online real-time calibration method according to claim 1, It is characterized in that In step S6, if |Mean α -Mode α | <T 4 , and Var α ≤T 5 , at this time, the output pitch installation angle error is updated, where T 4 and T 5 are the error thresholds between the mean and mode of the pitch installation angle error, and the variance threshold, Mean α 、Mode α are the mean and mode of the pitch installation angle errors, Var α is the variance of the pitch installation angle error; otherwise, keep the output pitch installation angle error unchanged; If | Mean β -Mode β | <T 6 ,Var β ≤T 7 , at this time, the output horizontal installation angle error is updated, where T 6 and T 7 are the error threshold between the mean and mode of the horizontal installation angle error, and the variance threshold, Mean β 、Mode β are the mean and mode of the horizontal installation angle error, Var β is the variance of the horizontal installation angle error; otherwise, keep the output horizontal installation angle error unchanged.