Vehicle speed state estimation method based on vehicle-mounted 4D millimeter-wave radar

Abstract

A vehicle speed state estimation method based on a vehicle-mounted 4D millimeter-wave radar comprises: 1. real-time reading of target data collected by a 4D millimeter-wave radar; 2. conversion of the target data, target clustering to obtain a target cluster and its attributes, initialization if the data is the first frame, and termination of the current calculation; 3. matching degree calculation of the target cluster of the current frame using the estimation result of the previous frame, and obtaining the target cluster confidence by combining the target cluster height confidence; 4. establishing a speed state and target cluster observation equation, establishing an optimization equation using the vehicle speed state and the target cluster of the current frame and solving it; 5. evaluating the estimated vehicle speed state to determine whether an output condition is met, outputting estimated information of the vehicle speed state if the condition is met, and reducing the target cluster height confidence if the condition is not met and returning to step 4; a good speed estimation effect can be achieved even in a multi-dynamic target scenario.

Description

Technical Field

[0001] The present disclosure relates to the field of autonomous driving technology, and specifically to a self-driving vehicle self-vehicle speed estimation technology, and more particularly to a self-vehicle speed state estimation method, device, electronic device and storage medium based on a vehicle-mounted 4D millimeter wave radar. Background Art

[0002] The continuous progress of measurement and perception technology has provided a foundation for the further development of autonomous driving technology. The operating state of the autonomous driving vehicle itself, especially its own vehicle speed, is an essential and important control parameter when the autonomous driving vehicle is operating autonomously. Real-time and accurate estimation of the self-vehicle speed state of the autonomous driving vehicle is a necessary prerequisite for improving driving safety and comfort.

[0003] At present, in the field of autonomous driving, the scenario of multiple dynamic targets is a challenge faced by the estimation of the speed of the vehicle, which seriously affects the accuracy of the estimation of the speed of the vehicle. The scenario of multiple dynamic targets is a common scenario in the actual autonomous driving process. In this scenario, the dynamic targets will become an interference factor for the estimation of the speed of the vehicle, seriously affecting the accuracy of the estimation of the speed of the vehicle, and even causing the estimation to fail. Although the existing technical methods have strategies for solving the problem of dynamic target interference and have certain robustness in scenarios with fewer dynamic targets, they are not ideal for processing scenarios with multiple dynamic targets, especially for open road scenarios with many pedestrians and vehicles and scenarios with many large vehicles on the opposite side of the road. Therefore, in the scenario of multiple dynamic targets, how to provide the speed parameters of the vehicle in real time, accurately and reliably has become a technical problem that needs to be solved urgently.

[0004] At present, the use of vehicle-mounted millimeter-wave radar is becoming more and more widespread, and many mass-produced civilian passenger cars are equipped with millimeter-wave radar sensors. Millimeter-wave radar has also evolved from traditional 3D millimeter-wave radar (providing target azimuth angle, speed, and distance) to 4D millimeter-wave radar (providing target pitch angle, azimuth angle, speed, and distance).

[0005] However, the existing millimeter wave radar vehicle speed state estimation technology mainly uses a 2D vehicle-mounted millimeter wave radar installed in front of the vehicle, and uses the Doppler velocity measurement of the target to calculate the speed of the vehicle relative to the ground. Compared with the existing SLAM (simultaneous localization and mapping) technology, vehicle speed state estimation is equivalent to a method of providing a speed odometer.

[0006] In order to understand the development status of the prior art, the present disclosure searches, compares and analyzes the existing patent documents and papers:

[0007] Solution 1: Patent document CN 111308458 A ("A method for estimating the speed of a self-vehicle based on a vehicle-mounted millimeter-wave radar") discloses an invention technology, which discloses a method for estimating the speed of a self-vehicle based on a vehicle-mounted millimeter-wave radar. The method requires that the radar be installed in a horizontal position in front of the vehicle to be tested, and there is a known fixed installation angle between the direction of the main transmitting beam of the radar and the direction of the central axis of the vehicle to be tested. The target clusters are classified by the target speed obtained by the vehicle-mounted millimeter-wave radar, and the targets with the same speed are divided into a cluster. The confidence of the target cluster is estimated according to the number of targets in each cluster, and the larger the number, the higher the confidence. If there is a target cluster with a confidence higher than a threshold (the number of targets in the target cluster is higher than a certain value), the speed of the target cluster is used as the result of the self-vehicle speed estimation. However, this method will inevitably fail in a multi-dynamic target scenario, such as a large vehicle traveling in the opposite lane on an open road. The large target will bring a large number of radar observation targets. The method will mistakenly output the speed of the large truck as the self-vehicle speed, thereby obtaining an erroneous result. Although a sliding window is used to filter the results of each frame, the error has been calculated, and as the calculation increases, the error will accumulate and increase. In addition, the speed estimation of this method only has the linear speed of the vehicle's driving direction, and no angular speed estimation.

[0008] Solution 2: The document "Joint Spatial-and Doppler-based Ego-Motion Estimation for Automotive Radars" introduces a method for estimating the speed of a vehicle based on an on-board millimeter-wave radar. It mainly uses a mixture-gaussian to jointly estimate the spatial position information and speed information of the millimeter-wave radar target, and uses a nonlinear optimization method to obtain the best result. Since this technical solution does not fully filter and eliminate dynamic targets, it directly brings dynamic targets into the algorithm, and the mixture-gaussian cannot perform special processing on dynamic targets. In a multi-dynamic target environment, the results of the algorithm will inevitably be affected by the dynamic targets, resulting in a decrease in calculation accuracy. When facing a multi-dynamic target scenario, the same erroneous results as in technology 1 will be encountered.

[0009] Solution 3: The document "Ego-Motion Estimation using Distributed Single-Channel Radar Sensors" uses multiple vehicle-mounted millimeter-wave radars to determine the position of the same target in different radars, eliminate erroneous targets, and use RANSAC to filter outliers after filtering. Then, the least squares nonlinear regression is iteratively calculated to finally obtain the estimation of the vehicle's horizontal motion speed and rotation speed. This technical solution relies on the RANSAC algorithm. The number of RANSAC iterations is fixed, and the correct result is probabilistic, so it cannot achieve consistent reliability in different multi-dynamic target scenarios, and will produce erroneous results. The least squares linear regression calculation is very sensitive to outliers. If RANSAC fails in complex scenarios, it will directly lead to the failure of the least squares linear regression, and ultimately obtain erroneous results. Therefore, this technical solution cannot output accurate vehicle state estimation in scenarios with multiple dynamic targets.

[0010] In summary, Solution 1 will inevitably fail in scenarios with multiple dynamic targets and relatively consistent speeds of the dynamic targets; Solution 2 does not distinguish dynamic targets well, but uses a mixed Gaussian calculation process to alleviate the impact of dynamic targets. In scenarios with a high proportion of dynamic targets, the algorithm will not be able to provide accurate estimates; Solution 3 uses the RANSAC algorithm, which is an abnormal data exclusion algorithm to address the impact of dynamic targets, but such algorithms are currently not adaptive and have a high degree of randomness in the results, which ultimately leads to low algorithm robustness.

[0011] It can be seen that the robustness of speed state estimation mainly lies in solving the problem of dynamic targets. The existing technology eliminates dynamic targets in radar observation data or reduces the influence of dynamic targets on calculation results through filtering technology or nonlinear regression algorithm; when the vehicle's environment is entirely static objects, the existing technology can obtain a relatively accurate vehicle speed estimation; that is, in an environment with fewer dynamic targets, the existing technology can have a certain robustness to dynamic targets, and can obtain accurate vehicle speed estimation when there are not many dynamic targets; however, for multi-dynamic target scenarios dominated by dynamic targets, the existing technology cannot completely eliminate the influence of dynamic targets on the results, and dynamic targets will cause excessive speed estimation errors or even estimation failure.

[0012] The existing technology is still unable to meet the requirements of self-driving vehicle speed state estimation in multi-dynamic target scenarios. Therefore, how to provide a self-driving vehicle speed state estimation method that is suitable for multi-dynamic target scenarios and can still give accurate results in complex traffic multi-dynamic target environments; in scenarios where multiple large vehicles are traveling at the same speed in opposite lanes of the road, effectively reducing the impact of a large amount of dynamic target data generated by large vehicles on the results, and obtaining the self-vehicle speed in real time, accurately and reliably, has become a very practical research topic at present. Summary of the invention

[0013] In order to achieve the above-mentioned invention object, the present invention has made improvements in the following aspects:

[0014] Using one or more 4D millimeter-wave radars installed around the vehicle, through processes such as target cluster recognition, target cluster matching, height confidence calculation, and Doppler estimation based on target clusters, it effectively solves the problem of inaccurate speed estimation in multi-dynamic target scenarios, adapts to various practical scenarios from multi-static targets to multi-dynamic targets, and has high real-time, adaptability, and robustness;

[0015] In view of the technical problem that most vehicle speed estimation methods based on millimeter-wave radar targets are limited to a single millimeter-wave radar and have strict requirements on the radar installation position, the method of converting the millimeter-wave radar target to the vehicle coordinate system for unified processing is adopted to effectively solve the problem of limited number of radars. Therefore, any number of millimeter-wave radars can be used. Since the target is converted to the vehicle coordinate system for unified processing, there is no requirement for the radar installation azimuth angle, and it is more universal.

[0016] In order to solve the problem of large interference caused by multiple dynamic targets during the detection process, conventional filtering algorithms and common abnormal data elimination methods (such as RANSAC) cannot effectively eliminate the technical problem of interference. The historical estimation results are used to predict the distribution of the current target cluster, and the matching calculation is performed with the actual distribution of the target cluster in the current frame; the matching degree of the target cluster is independent of the number of targets in the target cluster itself, but only related to the position matching degree and the change in the number of the same target cluster in the two frames. This method effectively distinguishes the target clusters formed by large dynamic targets (such as large trucks traveling in opposite directions), ensures the accuracy and reliability of the speed estimation results, and greatly improves the safety of autonomous driving. Since the distribution of the current target cluster is predicted by the historical estimation results and matched with the actual distribution of the target cluster in the current frame, it does not rely on the setting of fixed parameters (compared with RANSAC, for example), so it has a wider range of applications, is suitable for multi-dynamic target scenarios, and has high robustness.

[0017] In order to solve the problem of large deviations in speed estimation results caused by complex and changeable environmental scenes during autonomous driving, the height confidence of the target in three-dimensional space is used. When the algorithm cannot obtain sufficiently reliable results, the spatial confidence within the height range will be reduced according to the height of most pedestrians and vehicles from the ground, thereby reducing the impact of dynamic targets on vehicles in common scenes with a high probability and improving the accuracy and real-time performance of speed estimation. Compared with existing technical solutions, the present disclosure is more adaptive.

[0018] Specifically, in order to solve the above technical problems, the present disclosure provides a vehicle speed state estimation method, device, electronic device and storage medium based on a vehicle-mounted 4D millimeter-wave radar.

[0019] According to one aspect of the present disclosure, the present disclosure provides a vehicle speed state estimation method based on a vehicle-mounted 4D millimeter wave radar, comprising the following steps:

[0020] Step 1: Read the target data collected by the 4D millimeter wave radar in real time, wherein the target data includes distance, direction angle, pitch angle and radial Doppler velocity;

[0021] Step 2: convert the target data, cluster the target to obtain the target cluster and its attributes, initialize if the data is the first frame, and end the calculation;

[0022] Step 3: Use the estimation result of the previous frame to calculate the matching degree of the target cluster of the current frame, and combine the target cluster height confidence to obtain the target cluster confidence;

[0023] Step 4: Establish the speed state and target cluster observation equation, use the vehicle speed state and the current frame target cluster to establish the optimization equation and solve it;

[0024] Step 5: Evaluate the estimated vehicle speed state to determine whether it meets the output conditions. If so, output the estimated information of the vehicle speed state. If not, reduce the target cluster height confidence and return to step 4.

[0025] Preferably, the number of the 4D millimeter-wave radars is one or more.

[0026] Preferably, set:

[0027] The number of the 4D millimeter wave radars is: N r ;

[0028] The number of target clusters in the jth radar is: N j ;

[0029] The time interval between two frames is: τ;

[0030] The speed state of the vehicle is based on the vehicle coordinate system;

[0031] The speed state includes: the longitudinal linear speed V along the driving direction in the vehicle driving plane x , and V x Vertical transverse linear velocity V y , the angular velocity ω of the vehicle coordinate system generated during the vehicle steering process, and the lateral velocity of the vehicle coordinate system is 0, then the velocity state to be estimated is expressed as:

[0032]

[0033] Where S t is the vehicle speed state at the tth frame, is the longitudinal linear velocity of the tth frame, ω t is the rotation angular velocity of the tth frame;

[0034] The one or more 4D millimeter wave radars are horizontally installed around the vehicle body to observe the vehicle surrounding environment, and the target data is relative to the radar coordinate system.

[0035] Preferably, if there are multiple radars, data time synchronization is performed on the multiple 4D millimeter-wave radars to ensure that each frame of data contains all data of all radars near the same time, and each frame of data is output at a stable frequency.

[0036] Preferably, the system is initialized after the vehicle is started. At this time, the vehicle is in a stationary state. After the system is powered on and the program is run, the first frame of radar data is received. At this time, the speed state of the vehicle is zero, that is, S0{v x =0,ω=0}.

[0037] Preferably, the target data of each radar is converted from the radar coordinate system to the vehicle coordinate system;

[0038] The three-dimensional spatial coordinates of each target in the jth radar in the vehicle coordinate system It is expressed as follows:

[0039]

[0040] Get the target of each radar in the vehicle coordinate system;

[0041] in, is the distance of the i-th target of the j-th radar, is the horizontal angle of the i-th target of the j-th radar, is the elevation angle of the i-th target of the j-th radar, is the Doppler velocity of the i-th target of the j-th radar;

[0042] is the installation external parameter of the jth radar relative to the vehicle coordinate system, where are the positions of the jth radar coordinate system relative to the vehicle coordinate system on the x, y, and z axes, respectively. are the angles of the jth radar relative to the vehicle coordinate system around the z-axis, y-axis, and x-axis; when the radar is installed horizontally, If both are 0, the installation external parameters are simplified to

[0043] Preferably, the DBSCAN (Density-based spatial clustering of applications with noise) algorithm is used to cluster the target points of each radar. Perform clustering calculations and use the kd-tree method to speed up the distance search in three-dimensional space and improve calculation efficiency. Use the DBSCAN algorithm to obtain the core points, core reachable points, and outliers of the target. Take each core point and its core reachable points as a target cluster, and the coordinates of the core point are the target cluster coordinates, expressed as Each outlier is regarded as a target cluster, and the coordinates of the outlier are the coordinates of the target cluster. Finally, the target cluster of each radar in the vehicle coordinate system is expressed as follows:

[0044]

[0045] in is the kth target cluster of the jth radar; N r is the number of radars, N j is the number of target clusters of the jth radar;

[0046] The target set in each target cluster is: in is the i-th target of the k-th target cluster of the j-th radar, is the total number of targets of the kth target cluster of the jth radar;

[0047] The speed of the target cluster is the weighted average of the speeds of all targets contained in the target cluster and the distance distribution from the target to the center of the target cluster, i.e., the coordinate point of the target cluster;

[0048] Calculate the velocity of each target cluster;

[0049] The distance between the target in the target cluster and the center of the target cluster is expressed as follows:

[0050]

[0051] in, is the distance between the ith target of the kth target cluster of the jth radar and the center of the target cluster; middle is the position of the i-th target of the k-th target cluster of the j-th radar, and its coordinates are (x, y, z); is the center coordinate of the kth target cluster of the jth radar;

[0052] Represents the maximum distance, then calculate the k-th target cluster velocity of the j-th radar The formula is as follows:

[0053]

[0054] in is the speed of the ith target of the kth target cluster of the jth radar; the position coordinates and target cluster speed of each target cluster of each radar in the vehicle coordinate system are obtained, thereby obtaining the target cluster and target cluster speed of each radar in the vehicle coordinate system.

[0055] Preferably, the using the estimation result of the previous frame to calculate the matching degree of the target cluster of the current frame includes:

[0056] Firstly, the predicted target cluster is calculated in the vehicle coordinate system of the current frame through the vehicle speed state estimation of the previous frame and the target cluster of the previous frame to obtain the predicted target cluster;

[0057] Then, search for the predicted target cluster within a certain range of the current frame target cluster;

[0058] Finally, the target cluster matching degree is calculated by the target cluster matching degree.

[0059] Preferably, S t-1 {V x t-1 ,ω t-1} is the velocity state estimation of the previous frame, V x t-1 is the longitudinal linear velocity estimated in the previous frame, ω t - 1 The estimated vehicle rotation speed for the previous frame;

[0060] is the target cluster set of the previous frame, and the coordinate position of the kth target cluster of the jth radar relative to the vehicle coordinate system is is the kth target cluster of the jth radar in the t-1th frame, and its three-dimensional coordinates are {g x ,g y ,g z};

[0061] The transformation parameters obtained according to the vehicle speed state of the previous frame are: where t x =V x t-1τ, t y =0,φ=ω t-1 τ; Since the time interval τ between two adjacent frames is usually very small, only t x and Φ; the transformation parameters are as follows:

[0062]

[0063] Where V x t-1 is the longitudinal linear velocity estimated in the previous frame, ω t-1 is the estimated vehicle rotation speed of the previous frame, τ is the time interval between two frames;

[0064] The prediction process is as follows:

[0065]

[0066]

[0067] in, is the set of target clusters of the jth radar in the previous frame. The target clusters in the set are: Total N j Each target cluster is transformed by the transformation function T to obtain is the set of predicted target clusters of the jth radar in the previous frame in the vehicle coordinate system of the current frame

[0068] is the predicted target cluster of the kth target cluster of the jth radar in the tth frame, and its three-dimensional coordinates are {g px ,g py ,g pz}; The expression of the transformation function T is as follows:

[0069]

[0070] in is the position coordinate after transformation, is the position coordinate before transformation;

[0071] is the three-dimensional rotation matrix, Φ is the rotation angle, is the translation vector along the vehicle’s forward direction;

[0072] Get the predicted target cluster in the current vehicle coordinate system.

[0073] Preferably, searching for a predicted target cluster within a certain range of a target cluster of the current frame includes:

[0074] Search whether there is a predicted target cluster within a certain range near the three-dimensional space of the target cluster of the current frame. The search range of each target cluster in the current frame is different, which is expressed as Its value can be obtained by the following formula:

[0075]

[0076] in is the distance threshold between the kth target cluster of the jth radar and the predicted target cluster, V x t-1 ,ω t-1 is the state estimation of the previous frame, that is, the longitudinal linear velocity of the previous frame and the rotational angular velocity of the previous frame.

[0077] Preferably, the target cluster matching degree is calculated according to the search results: if there is no predicted target cluster within the target cluster range, the matching degree is set to zero, that is, M = 0; if there are f (f = 1, 2, 3 ...) predicted target clusters, the matching degrees M with these f predicted target clusters are calculated respectively. f , and sort the matching degrees, taking the maximum matching degree M f ,f=argMax(M f ) as the matching degree of the target cluster in the current frame.

[0078] Preferably, the matching degree of the target cluster is calculated: the distance matching degree between the predicted target cluster and the target cluster of the current frame is calculated Then calculate the target cluster target quantity matching degree Q, through Combined with Q, the matching degree M of the target cluster of the current frame radar is obtained;

[0079] Among them, the calculation formula of the target cluster distance matching degree is expressed as follows using the normal distribution of the predicted target cluster:

[0080]

[0081] in is the prediction of the kth target cluster of the jth radar, is relative to the predicted target cluster The distance matching degree, x is the coordinate of any position in three-dimensional space, is a three-dimensional Euclidean space;

[0082] The calculation formula of the target quantity matching degree is expressed as follows using the quantity difference distribution:

[0083]

[0084] Where x1, x2 are the number of targets in any two target clusters, Q is the matching degree of the number of targets, is the space of positive integers;

[0085] Combining the above two formulas (11) and (12), the matching formula of the target cluster is as follows:

[0086]

[0087] Thus, the confidence of the target cluster of the jth radar in the current frame is obtained

[0088] Preferably, an optimization equation is established based on the vehicle speed state and the radar Doppler observation, and solved using the radar target cluster, including:

[0089] is the speed state of the vehicle in the current frame, and the installation external parameter of the jth radar is The Doppler velocity observation equation for the current frame is as follows:

[0090]

[0091] where v d is the radial observed velocity of the target in the direction of the j-th radar azimuth angle α;

[0092] The radial observation velocity of the kth target cluster of the jth radar is The error function is as follows:

[0093]

[0094] where e j is the error of the jth radar, N j is the number of target clusters in the jth radar, is the confidence of the kth target cluster in the current frame of the jth radar. The error function associates the speed of the target cluster observed by the radar with the speed state of the vehicle, and transforms the solution of the current frame speed state into the solution of the optimization process. The current frame speed state is as follows:

[0095]

[0096] Where N r is the number of radars, N j is the number of target clusters of the jth radar;

[0097] Finally, the nonlinear least square method is used to solve the current frame vehicle speed state S t =(V x t ,ω t ) and the residual value e rest =min(e).

[0098] Preferably, the estimated vehicle speed state is evaluated to determine whether it meets expectations, including:

[0099] Evaluate the vehicle speed state of the current frame using an evaluation function, and the evaluation function is as follows:

[0100] E = T(S t-1 , S t )W(S t-1 , S t )e rest (17)

[0101]

[0102]

[0103] The evaluation value is E, T{S t-1 , S t ) is the evaluation value of the longitudinal linear velocity, W(S t-1 , S t ) is the evaluation value of the rotational angular velocity, a max is the threshold of the maximum longitudinal acceleration of the vehicle, Φ max is the threshold of the maximum rotational angular acceleration of the vehicle; if the evaluation value E reaches the threshold Et, directly output the result S of the vehicle speed state t = (V x t , ω t ) If the evaluation value does not reach the threshold Et, then increase the target height confidence on the basis of the matching degree calculation and recalculate the target cluster matching degree.

[0104] Preferably, the target height confidence formula is:

[0105]

[0106] where ξ h is the confidence parameter, and the initial value is ξ h = 0.8, h target is the height where the target is located, h1 and h2 are the upper and lower limits of the height interval respectively, and h2 > h1; that is, when h target is in 2h target < h1 or 2h target > h2, the P h value is 1; when h target is in 2h target ∈[h1, h2], the P h value is ξ h ;

[0107] h m is the height of the rear axle center from the ground, H is the average value of the heights of pedestrians and vehicles, where h1 = -h m , h2 = H - hm ; The confidence of targets within the h1 and h2 intervals will be reduced, while h2 and the highest target at height h w The static object confidence of the interval will remain unchanged.

[0108] Preferably, the height confidence distribution of each radar is:

[0109]

[0110] in is the kth target cluster of the jth radar, for The z-axis coordinate value in;

[0111] Finally, the confidence of the kth target cluster of the jth radar is obtained for:

[0112]

[0113] Iterate the calculation, if the evaluation value E reaches the threshold Et, output the result S t =(V x t ,ω t ); if it is still not satisfied, adjust the allocation parameter ξ h , halved, that is, ξ h =0.5ξ h , continue to return iteration;

[0114] If the evaluation value E still does not reach the threshold Et after a limited number of iterations, the previous frame estimation result S is used t-1 =(V x t-1 ,ω t-1 ) is output as the result of the estimation of this frame, even if S t =S t-1 .

[0115] Preferably, a spectral clustering algorithm is used to cluster the target points of each radar. Perform clustering calculation, including the following steps:

[0116] (1) Use the three-dimensional spatial coordinates of the target in each radar in the vehicle coordinate system to construct an undirected graph:

[0117] Use the set of points and the set of edges to describe and construct an undirected graph G, that is, G)V,E); where V is the target of the radar in the vehicle coordinate system (v1, v2, ..., v n ); E is the set of edges; weight w ij For point v i and point v j The weight betweenij =w ji ;

[0118] (2) Construct the adjacency matrix W and degree matrix D based on the similarity matrix:

[0119] Degree i It is defined as the sum of the weights of all edges connected to it, that is,

[0120]

[0121] Using the definition of each point degree, we get an n×n degree matrix D, which is a diagonal matrix with values ​​only on the main diagonal, corresponding to the degree of the jth point in the i-th row, expressed as follows:

[0122]

[0123] The adjacency matrix W is defined using the full connection method, and its matrix elements are expressed as follows:

[0124]

[0125] where v i and v j are the targets of the two radars in the vehicle coordinate system, and σ is the variance value;

[0126] (3) Calculate the Laplace matrix L:

[0127] The Laplacian matrix of the undirected graph G is:

[0128] L=DW (26)

[0129] According to the definition of the Laplace matrix, for any vector f:

[0130]

[0131] Suppose the subgraph set is A1, A2, …, A k ,satisfy A1∪A2∪…∪A k =V;

[0132] The weight of the cut graph is:

[0133]

[0134] Where A and B are two subgraph sets, i and j are the subset numbers of A and B respectively;

[0135] Use NCut to cut the image:

[0136]

[0137]

[0138] in A i The complement of i The union of all subsets of V except ;

[0139] The optimization objective function is:

[0140]

[0141] The normalized Laplacian matrix is:

[0142]

[0143] (4) Calculate the eigenvectors f corresponding to the smallest k1 eigenvalues ​​of L, and normalize the matrix composed of f by row; obtain the n×k1-dimensional eigenmatrix F;

[0144] (5) Take each row in the feature matrix F as a k1-dimensional sample, a total of n samples, and use the k-means clustering method to cluster, with a clustering dimension of k2;

[0145] (6) Finally, we get the cluster partition C(c1,c2,…,c k2 ), the mean of the target positions in each target cluster is taken as the target cluster position, and the target cluster is obtained.

[0146] According to another aspect of the present disclosure, the present disclosure provides a vehicle speed state estimation device based on a vehicle-mounted 4D millimeter wave radar, comprising:

[0147] A data acquisition device reads target data collected by the 4D millimeter wave radar in real time, wherein the target data includes distance, direction angle, pitch angle and radial Doppler velocity;

[0148] A data conversion device converts the target data, clusters the target data to obtain the target cluster and its attributes, initializes the data if it is the first frame, and ends the calculation;

[0149] A matching degree calculation device is used to calculate the matching degree of the target cluster of the current frame using the estimation result of the previous frame, and obtain the target cluster confidence degree in combination with the target cluster height confidence degree;

[0150] The optimization solving device establishes the speed state and the target cluster observation equation, and uses the vehicle speed state and the current frame target cluster to establish the optimization equation and solve it;

[0151] The evaluation device evaluates the estimated vehicle speed state and determines whether the output condition is met. If so, the estimated information of the vehicle speed state is output. If not, the target cluster height confidence is reduced and returned to the optimization solution device for recalculation.

[0152] According to another aspect of the present disclosure, the present disclosure provides an electronic device, including:

[0153] a memory storing execution instructions; and

[0154] A processor executes the execution instructions stored in the memory, so that the processor executes the above method.

[0155] According to one aspect of the present disclosure, the present disclosure provides a readable storage medium, wherein the readable storage medium stores execution instructions, and the execution instructions are used to implement the above method when executed by a processor.

[0156] Beneficial effects of the present disclosure:

[0157] 1. It is independent of the number and installation angle of millimeter-wave radars, has strong adaptability and a wide range of applications;

[0158] 2. Dynamic target recognition and calculation are performed at the target cluster level, effectively reducing the impact of dynamic targets on speed estimation in multi-dynamic target scenarios;

[0159] 3. Make full use of the pitch angle feature of 4D millimeter wave radar, reduce the weight of targets within the height range of pedestrians and vehicles by adjusting the height confidence, quickly improve the estimation effect from common scenarios, and be adaptive;

[0160] 4. For vehicles traveling on open roads, the speed state of the vehicle can be reliably estimated just like on fixed closed roads, and accurate speed estimation results can still be given in complex traffic environments with multiple dynamic targets;

[0161] 5. In the scenario where there are multiple large vehicles traveling at the same speed in the opposite lane of the road, the impact of a large amount of dynamic target data generated by the large vehicles on the results is effectively reduced, and the vehicle speed estimation result is accurate and reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0162] The accompanying drawings illustrate exemplary embodiments of the present disclosure and are used together with the description thereof to explain the principles of the present disclosure, wherein these drawings are included to provide a further understanding of the present disclosure, and the drawings are included in and constitute a part of this specification. By describing the embodiments of the present disclosure in detail in conjunction with the accompanying drawings, the above and other purposes, features, and advantages of the present disclosure will become more apparent.

[0163] Figure 1 Schematic diagram of vehicle coordinate system

[0164] Figure 2 Main flow chart of the vehicle speed state estimation method based on vehicle-mounted 4D millimeter-wave radar

[0165] Figure 3 Detailed flow chart of the vehicle speed state estimation method based on vehicle-mounted 4D millimeter-wave radar

[0166] Figure 4 Relationship diagram between radar coordinate system and vehicle coordinate system

[0167] Figure 5 Schematic diagram of the predicted target cluster

[0168] Figure 6 Refinement flowchart of using the previous frame estimation result to calculate the matching degree of the current frame target cluster

[0169] Figure 7 Schematic diagram of high confidence expression DETAILED DESCRIPTION

[0170] The present disclosure is further described in detail below in conjunction with the accompanying drawings and implementations. It is understood that the specific implementations described herein are only used to explain the relevant content, rather than to limit the present disclosure. It should also be noted that, for ease of description, only the parts related to the present disclosure are shown in the accompanying drawings.

[0171] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present disclosure can be combined with each other. The technical solution of the present disclosure will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0172] Unless otherwise specified, the exemplary embodiments / embodiments shown will be understood as providing exemplary features of various details of some ways in which the technical concept of the present disclosure can be implemented in practice. Therefore, unless otherwise specified, the features of the various embodiments / embodiments can be combined, separated, interchanged and / or rearranged without departing from the technical concept of the present disclosure.

[0173] The use of cross-hatching and / or shading in the accompanying drawings is generally used to make the boundaries between adjacent components clear. As such, unless otherwise specified, the presence or absence of cross-hatching or shading does not convey or indicate any preference or requirement for the specific materials, material properties, dimensions, proportions, commonalities between the components shown, and / or any other characteristics, attributes, properties, etc. of the components. In addition, in the accompanying drawings, the sizes and relative sizes of the components may be exaggerated for clarity and / or descriptive purposes. When the exemplary embodiments can be implemented differently, the specific process sequence can be performed in a different order than described. For example, two successively described processes can be performed substantially simultaneously or in an order opposite to the described order. In addition, the same figure numbers represent the same components.

[0174] When a component is referred to as being "on," "over," "connected to," or "coupled to" another component, the component may be directly on, directly connected to, or directly coupled to the other component, or intervening components may be present. However, when a component is referred to as being "directly on," "directly connected to," or "directly coupled to" another component, there are no intervening components. For this purpose, the term "connected" may refer to a physical connection, an electrical connection, etc., with or without intervening components.

[0175] The terms used in the present disclosure are for the purpose of describing specific embodiments, and are not intended to be restrictive. As used herein, unless the context clearly indicates otherwise, the singular forms "one (kind, person)" and "said (the)" are also intended to include plural forms. In addition, when the terms "comprise" and / or "include" and their variations are used in this specification, it is explained that there are stated features, integral bodies, steps, operations, parts, assemblies and / or their groups, but one or more other features, integral bodies, steps, operations, parts, assemblies and / or their groups are not excluded from existence or addition. It should also be noted that, as used herein, the terms "substantially", "approximately" and other similar terms are used as approximate terms and not as degree terms, so that they are used to explain the inherent deviations of the measured values, calculated values ​​and / or the values ​​provided that will be recognized by those of ordinary skill in the art.

[0176] In the description of this specification, the description with reference to the terms "one embodiment / method", "some embodiments / methods", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment / method or example are included in at least one embodiment / method or example of the present application. In this specification, the schematic representations of the above terms are not necessarily the same embodiment / method or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments / methods or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments / methods or examples described in this specification and the features of the different embodiments / methods or examples, unless they are contradictory.

[0177] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0178] One of the objectives of the present disclosure is to provide a vehicle speed state estimation method based on a vehicle-mounted 4D millimeter-wave radar.

[0179] The transmitting antenna of the vehicle-mounted millimeter-wave radar emits periodic FMCW radar waves (FMCW, Frequency Modulated Continuous Wave). When the radar waves encounter an object, they return to form an echo. After receiving the echo, the receiving antenna performs a fast Fourier transform (FFT, Fast Fourier Transform) to calculate the distance, angle and speed of the object. Each calculated result is regarded as a target. The 4D millimeter-wave radar uses receiving antennas in both horizontal and vertical directions, which can obtain the horizontal and pitch angles of the target, plus the distance and radial speed of the target, a total of four pieces of information.

[0180] The present invention uses one or more vehicle-mounted 4D millimeter-wave radars installed around the vehicle body to estimate the speed state of the vehicle body in real time. Figure 1 is a schematic diagram of the vehicle coordinate system. After the vehicle is initialized, refer to the velocity state estimation of the previous frame and calculate the coordinate system of the vehicle (with the center of the rear axle of the vehicle as the origin, such as Figure 1 The targets detected by the millimeter-wave radar are classified into target clusters under the following conditions: (as shown in the figure), and the matching degree and confidence level of the target clusters are evaluated, which effectively reduces the impact of dynamic targets, especially multiple dynamic targets with relatively consistent speeds, on the estimation of the vehicle's speed state. The Doppler speed information of the target cluster with confidence is then used for optimization and solution, and the result of the vehicle's speed state estimation is obtained.

[0181] The present invention uses target information provided by one or more 4D millimeter-wave radars installed around the vehicle to estimate the vehicle's speed state in real time and adapt to different scenarios from multiple static objects to multiple dynamic objects to ensure the robustness of the estimation results.

[0182] Figure 2 The main flow chart of the vehicle speed state estimation method based on the vehicle-mounted 4D millimeter wave radar is described. Figure 2 As shown in (S100), the vehicle speed state estimation method based on the vehicle-mounted 4D millimeter wave radar includes the following steps:

[0183] The first step (S110) is radar data conversion, target clustering to obtain target clusters and their attributes. If the data is the first frame, it is initialized and the calculation is terminated.

[0184] In the second step (S120), the matching degree of the target cluster of the current frame is calculated using the estimation result of the previous frame, and the confidence of the target cluster is obtained by combining the high confidence of the target cluster;

[0185] The third step (S130) is to establish the speed state and target cluster observation equation, and use the vehicle speed state and the current frame target cluster to establish the optimization equation and solve it;

[0186] In the fourth step (S140), the estimated vehicle speed state is evaluated to determine whether the output condition is met. If so, the estimated information of the vehicle speed state is output. If not, the target cluster height confidence is reduced and the process returns to the third step (S130).

[0187] Before proceeding to the step description, define the following values:

[0188] N r : The number of radars;

[0189] N j : The number of target clusters in the jth radar;

[0190] τ: time interval between two frames;

[0191] The present disclosure estimates the vehicle speed state, and the vehicle speed state is based on the vehicle coordinate system (the center of the vehicle rear axle is the coordinate origin, such as Figure 1 The speed state includes the longitudinal linear speed V along the driving direction in the vehicle driving plane. x , and V x Vertical transverse linear velocity V y , the rotational angular velocity ω around the vehicle coordinate system generated during the vehicle steering process. According to the Ackerman vehicle motion model, the lateral velocity of the vehicle coordinate system is 0, so the velocity state to be estimated is:

[0192] S t = {V x t ,ω t |t=0,1,2…} (1)

[0193] Where S t is the vehicle speed state at the tth frame, V x t is the longitudinal linear velocity of the tth frame, ω t is the rotational angular velocity of the tth frame. The frame is defined according to the data release of the radar. The radar sends all target data at a fixed frequency (period) (for example, data is released every 50 milliseconds). The data from the system startup to the first receipt of the radar is the 0th frame data. At this time, the state is S0, which is the initial state of the vehicle. After that, the number of radar data frames received increases successively. The time interval of each frame is the period of radar data release.

[0194] The present invention uses one or more 4D millimeter-wave radars (hereinafter referred to as "radars") installed horizontally around the vehicle body to observe the vehicle's surrounding environment. The radar outputs target data. The target data output by each 4D millimeter-wave radar is relative to the radar coordinate system, including the target's distance, direction angle, pitch angle and radial Doppler velocity (scalar velocity along the radar observation direction).

[0195] Multiple radars need to be synchronized in time (usually by the radar hardware mechanism) to ensure that each frame of data contains all data from all radars at the same time, and each frame of data is output at a stable frequency, and the time interval between two frames is τ (for example, 50 milliseconds). Then the radar target data in each frame is the data sent by all radars at the same time.

[0196] The normal use of the present disclosure requires system initialization after the vehicle is started. The vehicle should be in a stationary state. After the system is powered on and the program is run, the first frame of radar data is received. At this time, the speed state of the vehicle is zero, that is, S0{v x =0,ω=0}. Since the calculation of each frame is based on the relevant results of the previous frame data, it is a Markov process, so initialization is necessary. The normal use of the vehicle starts from the vehicle being stationary, so the conditions of the initial process are reasonable.

[0197] Figure 3 This is a detailed flow chart of the vehicle speed state estimation method based on the vehicle-mounted 4D millimeter wave radar. Figure 2 A detailed description of the specific steps. Figure 3 As shown, the process is as follows:

[0198] (1) Determine whether it is the first frame of data (S111)

[0199] If it is the first frame data, the initial vehicle state is: S0{v x =0, ω=0}(S112), if it is not the first frame data, directly proceed to the next step of calculation.

[0200] (2) Radar data conversion, generating target clusters and their attributes (S113)

[0201] Next, we start the radar data conversion, converting the target data of each radar from the radar coordinate system to the vehicle coordinate system. Let the distance of the i-th target of the j-th radar be The horizontal angle is The pitch angle is The Doppler velocity is Assume that the installation external parameters of the jth radar relative to the vehicle coordinate system are in are the positions of the jth radar coordinate system relative to the vehicle coordinate system on the x, y, and z axes, respectively. are the angles of the jth radar relative to the vehicle coordinate system around the z-axis, y-axis, and x-axis. Since the radars are all installed horizontally, If both are 0, the installation external parameters are simplified to Calculate the three-dimensional spatial coordinates of each target in the jth radar in the vehicle coordinate system based on the external parameter relationship and radar target information

[0202]

[0203] The target of each radar in the vehicle coordinate system is obtained (1101).

[0204] The relationship between the radar coordinate system and the vehicle coordinate system is as follows: Figure 4 As shown, Figure 4 This is a top view of the vehicle, which only illustrates the plane relationship between the coordinate systems. The coordinate z axis is perpendicular to the figure and faces out of the figure. O is the vehicle coordinate system, O s1 and O s2 are the coordinate systems of the two radars respectively. are the external parameters of the two radars, and For the two targets in the first radar, is the target in the second radar, They are all coordinates relative to the vehicle coordinate system O.

[0205] Use DBSCAN (Density-based spatial clustering of applications with noise) algorithm to cluster the target points of each radar Perform clustering calculation (S114) and use kd-tree to speed up the distance search in three-dimensional space and improve calculation efficiency. The core points, core reachable points and outlier points of the target are obtained through the DBSCAN algorithm. Core points are generally generated by larger objects in the environment, such as buildings and large vehicles; outliers are usually generated by smaller objects in the environment, such as street lights and pedestrians. Each core point and its core reachable points are regarded as a target cluster, and the coordinates of the core point are the coordinates of the target cluster. Each outlier point is regarded as a target cluster, and the coordinates of the outlier point are the coordinates of the target cluster. Finally, the target cluster of each radar in the vehicle coordinate system is obtained (1102):

[0206]

[0207] in is the kth target cluster of the jth radar; N r is the number of radars, Nj is the number of target clusters of the j-th radar.

[0208] The target set in each target cluster is: in is the i-th target of the k-th target cluster of the j-th radar, is the total number of targets in the kth target cluster of the jth radar. The coordinates of each target are expressed as

[0209] The speed of each target cluster is calculated (S115), where the target cluster speed is a weighted average of the speeds of all targets included in the target cluster and the distance distribution from the target to the center of the target cluster (ie, the coordinate point of the target cluster).

[0210] The distance between the target in the target cluster and the center of the target cluster is:

[0211]

[0212] in is the distance between the i-th target of the k-th target cluster of the j-th radar and the center of the target cluster, is the position coordinate of the i-th target of the k-th target cluster of the j-th radar, is the center coordinate of the kth target cluster of the jth radar.

[0213] Assume the maximum distance is Calculate the kth target cluster velocity of the jth radar The formula is as follows:

[0214]

[0215] in is the speed of the i-th target of the k-th target cluster of the j-th radar. The position coordinates and target cluster speed of each target cluster of each radar in the vehicle coordinate system are obtained.

[0216] In summary, the target cluster and target cluster speed (1103) of each radar in the vehicle coordinate system can be obtained.

[0217] (3) Using the estimation result of the previous frame to calculate the matching degree of the target cluster of the current frame (S120)

[0218] The process of this step is as follows Figure 6 As shown, Figure 2 Detailed flowchart of step S120 in .

[0219] First, the predicted target cluster (S121) is calculated in the vehicle coordinate system of the current frame by estimating the vehicle speed state of the previous frame and the target cluster of the previous frame to obtain the predicted target cluster (1201), and then the predicted target cluster within a certain range of the target cluster of the current frame is searched (S122), and finally the target cluster matching degree is calculated (S123, S124, S125) to obtain the target cluster matching degree (1202).

[0220] The main purpose of the target cluster matching calculation is to distinguish the dynamic target cluster and reduce its weight in subsequent calculations. Different from the outlier removal algorithm (such as RANSAC) and filtering algorithm in the prior art, the method in this step can minimize the impact of large targets in the dynamic target (large number of targets in the target cluster) on the calculation accuracy.

[0221] (2.1) Prediction calculation process of target cluster (S121).

[0222] Assume the velocity state of the previous frame is estimated to be S t-1 {V x t-1 ,ω t-1}.

[0223] Suppose the target cluster of the previous frame is The coordinate position of each target cluster relative to the vehicle coordinate system is

[0224] The transformation parameters obtained according to the vehicle speed state of the previous frame are: where t x =V x t-1 τ, t y =0,φ=ω t-1 τ. Since the time interval τ between two adjacent frames is usually very small, only t x and φ. Then the transformation parameters are:

[0225]

[0226] Where V x t-1 is the longitudinal linear velocity estimated in the previous frame, ω t-1 is the estimated vehicle rotation speed of the previous frame. As mentioned before, τ is the interval time between two frames.

[0227] The prediction process is as follows:

[0228]

[0229]

[0230] in, is the set of target clusters of the jth radar in the previous frame. The target clusters in the set are: Total N j Each target cluster is transformed by the transformation function T to obtain is the set of predicted target clusters of the jth radar in the previous frame in the vehicle coordinate system of the current frame

[0231] The i-th target cluster of the j-th radar in the t-1-th frame The three-dimensional coordinates of its position are {g x ,g y ,g z}; The predicted target cluster of the i-th target cluster of the j-th radar in the t-1-th frame in the t-th frame The three-dimensional coordinates of its position are {g px ,g py ,g pz}, the expression of the transformation function T is as follows:

[0232]

[0233] in is the position coordinate after transformation, is the position coordinate before transformation;

[0234] is the three-dimensional rotation matrix, φ is the rotation angle, is the translation vector along the forward direction of the vehicle.

[0235] In summary, the predicted target cluster (1201) in the current vehicle coordinate system is obtained.

[0236] like Figure 5 As shown in the figure, in the vehicle coordinate system O, 1, 2, and 3 are the target clusters of the previous frame, the points in the solid box are the targets contained in the target clusters, 1 and 2 are target clusters containing multiple targets, and 3 is the target cluster of outliers; the dotted box 1 prediction, 2 prediction, and 3 prediction are the predicted target clusters of 1, 2, and 3 in the current frame respectively; the target clusters of 1', 2', and 3' in the current frame correspond to the target clusters 1, 2, and 3 of the previous frame respectively, that is, 1 and 1', 2 and 2', and 3 and 3' are the three target clusters of the previous frame and the three target clusters of the current frame generated by the same three objects respectively. Among them, the positions of 2 prediction and 2', and 3 prediction and 3' are close, 2 and 2' are stationary vehicles, and 3 and 3' are stationary objects with small reflection areas; among them, the position of 1' and 1 prediction is quite different, which is a target cluster of a large vehicle traveling in the opposite direction, and can be identified as a dynamic target.

[0237] (2.2) Search for a predicted target cluster within a certain range of the current frame target cluster (S122).

[0238] First, we search whether there is a predicted target cluster within a certain range near the three-dimensional space of the target cluster of the current frame. The search range of each target cluster in the current frame is different, set to Its value can be obtained by the following formula:

[0239]

[0240] in is the distance threshold between the kth target cluster of the jth radar and the predicted target cluster, V x t-1 ,ω t-1 is the state estimation of the previous frame, that is, the longitudinal linear velocity of the previous frame and the rotational angular velocity of the previous frame.

[0241] (2.3) Calculate the target cluster matching degree based on the search results.

[0242] If there is no predicted target cluster within the target cluster range, the matching degree is set to zero, that is, M = 0 (S123); if there are predicted target clusters, for example, f (f = 1, 2, 3 ...), the matching degrees M with these f predicted target clusters are calculated respectively. f (S125), sort the matching degrees and take the maximum matching degree M f ,f=argMax(M f ) as the matching degree of the current frame target cluster (S126).

[0243] (2.4) Calculate the matching degree of the target cluster (S124)

[0244] Calculate the distance matching degree between the predicted target cluster and the current frame target cluster Then calculate the target cluster target quantity matching degree Q, through Combined with Q, the matching degree M of the target cluster of the current frame radar is obtained.

[0245] The calculation formula for the target cluster distance matching degree uses the normal distribution of the predicted target cluster as follows:

[0246]

[0247] in is the prediction of the kth target cluster of the jth radar, is relative to the predicted target cluster The distance matching degree, x is the coordinate of any position in three-dimensional space, is a three-dimensional Euclidean space;

[0248] The calculation formula for the target quantity matching degree and quantity difference distribution is:

[0249]

[0250] Where x1, x2 are the number of targets in any two target clusters, and Q is the matching degree of the number of targets. is the space of positive integers.

[0251] The above two formulas are combined to obtain the matching formula of the target cluster:

[0252]

[0253] After the above calculation, the confidence of the target cluster in the current frame is obtained (1202)

[0254] (4) Establishing an optimization equation based on the vehicle speed state and radar Doppler observation, and solving it using the radar target cluster (S130)

[0255] The calculations in this step are all based on the current frame time. Assume that the speed state of the vehicle in the current frame is S t =(V x t ,ω t ), the installation external parameters of the jth radar are Then the Doppler velocity observation equation of the current frame is:

[0256]

[0257] where v d is the radial observed velocity of the target in the direction of azimuth angle α of the j-th radar.

[0258] The radial observation velocity of the kth target cluster of the jth radar is The error function is:

[0259]

[0260] where e j is the error of the jth radar, N j is the number of target clusters in the jth radar, is the confidence of the kth target cluster in the current frame of the jth radar calculated in the previous step. The error function associates the speed of the target cluster observed by the radar with the speed state of the vehicle, and transforms the solution of the current frame speed state into the solution of the optimization process:

[0261]

[0262] Where N r is the number of radars, N j is the number of target clusters of the jth radar, where the estimates of multiple radars are combined together.

[0263] Finally, the vehicle speed state S of the current frame is obtained by using nonlinear least squares and other methods. t=(V x t , ω t ) and the residual value e rest = min(e).

[0264] (5) Evaluate the estimated vehicle speed state to determine whether it meets the expectation (S140)

[0265] Evaluate the current frame vehicle speed state using an evaluation function. The evaluation function is:

[0266] E = T(S t-1 , S t )W(S t-1 , S t )e rest (17)

[0267]

[0268]

[0269] Its evaluation value is E. T(S t-1 , S t ) is the longitudinal linear speed evaluation value, W(S t-1 , S t ) is the rotational angular speed evaluation value, a max is the vehicle longitudinal maximum acceleration threshold, Φ max is the vehicle maximum rotational angular acceleration threshold.

[0270] If the evaluation value E reaches the threshold Et, directly output the result S of the vehicle speed state t =(V x t , ω t ) If the evaluation value does not reach the threshold, increase the target height confidence on the basis of the target cluster matching degree calculation, and recalculate the target cluster matching degree. The calculation method is as follows:

[0271] The target height confidence formula is:

[0272]

[0273] where ξ h is the confidence parameter, and the initial value is ξ h = 0.8, h target is the height where the target is located, h1 and h2 are the upper and lower limits of the height interval respectively, and h2 > h1; that is, when h target is between 2h target < h1 or 2h target > h2, the P h value is 1; when h targetAt 2h target ∈[h1,h2], P h The value is ξ h .

[0274] like Figure 7 As shown, h m is the height of the rear axle center from the ground, H is the average height of pedestrians and vehicles, H is 2 meters. Let h1 = -h m , h2=Hh m The confidence of the target in the interval between h1 and h2 will be reduced. This interval is the height where pedestrians and vehicles are located, while h2 and the highest target are located at the height h w For static objects in the interval, such as buildings, the confidence will remain unchanged.

[0275] According to the above formula, the height confidence distribution of each radar is:

[0276]

[0277] in is the kth target cluster of the jth radar, for Since the coordinates of the target cluster are based on the vehicle coordinate system, there is no need to use radar external parameter information.

[0278] Finally, the confidence of the kth target cluster of the jth radar is obtained for:

[0279]

[0280] According to the above method with high confidence, the iterative calculation is returned to (S130). If the evaluation value E reaches the threshold value Et, the output result S t =(V x t ,ω t ); if it is still not satisfied, adjust the allocation parameter ξ h , halving the value of h =0.5ξ h , and continue to return to (S130) iteration. In each iteration of this process, the confidence level within the height range of pedestrians and vehicles is reduced, that is, the possible impact of dynamic obstacles is gradually reduced. This process makes the algorithm adaptive.

[0281] If the evaluation value E does not reach the threshold Et after a limited number of iterations K = 3 times, then the previous frame estimation result S is used. t -1=(V x t-1 ,ω t-1 ) is output as the result of the estimation of this frame, even if S t=S t-1 Normally, this step will not be reached, but the extreme case that the millimeter-wave radar is completely blocked cannot be ruled out.

[0282] At this point, the velocity estimation result in the vehicle coordinate system is obtained.

[0283] Optionally, use a spectral clustering algorithm to classify the target points of each radar. Performing clustering calculation (S114) includes the following steps:

[0284] (1) Construct an undirected graph using the three-dimensional spatial coordinates of the target in each radar in the vehicle coordinate system.

[0285] Construct an undirected graph G, which is described by a set of points V and a set of edges E. That is, G(V,E). V is the target of the radar in the vehicle coordinate system (v1, v2, …, v n ). Define the weight w ij For point v i and point v j Since we are an undirected graph, w ij =w ji .

[0286] (2) Construct the adjacency matrix W and degree matrix D based on the similarity matrix.

[0287] Degree i It is defined as the sum of the weights of all edges connected to it, that is,

[0288]

[0289] Using the definition of each point degree, we can get an n×n degree matrix D, which is a diagonal matrix with values ​​only on the main diagonal, corresponding to the degree of the jth point in the i-th row, defined as follows:

[0290]

[0291] The adjacency matrix W is defined using the full connection method, and its matrix elements are expressed as follows:

[0292]

[0293] where v i and v j are the targets of the two radars in the vehicle coordinate system, and σ is the variance value.

[0294] (3) Calculate the Laplace matrix L.

[0295] The Laplacian matrix of graph G is defined as:

[0296] L=DW (26)

[0297] According to the definition of the Laplace matrix, for any vector f:

[0298]

[0299] Suppose the subgraph set is A1, A2, …, A k ,satisfy A1∪A2∪…∪A k =V.

[0300] The weight of the cut graph is:

[0301]

[0302] A and B are two subgraph sets, and i and j are the subset numbers of A and B respectively.

[0303] Use NCut to cut the image:

[0304]

[0305]

[0306] in A i The complement of i The union of all subsets of V except .

[0307] The optimization objective function is:

[0308]

[0309] The normalized Laplacian matrix is:

[0310]

[0311] (4) Calculate the eigenvectors f corresponding to the smallest k1 eigenvalues ​​of L, and normalize the matrix composed of f by row.

[0312] After calculation, we will get the feature matrix F of n×k1 dimensions.

[0313] (5) Take each row in F as a k1-dimensional sample, a total of n samples, and cluster them using the k-means clustering method with a clustering dimension of k2.

[0314] (6) Finally, we get the cluster partition C(c1,c2,…,c k2 ), the mean of the target positions in each target cluster is taken as the target cluster position, and the target cluster (1102) is obtained.

[0315] Any process or method description in the flowchart of the present disclosure or described in other ways herein can be understood as a module, fragment or part of a code representing 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 disclosure includes other implementations, in which the functions may not be performed in the order shown or discussed, including performing the functions in a substantially simultaneous manner or in a reverse order according to the functions involved, which should be understood by a person skilled in the art of the technical field to which the embodiments of the present disclosure belong. The processor performs the various methods and processes described above. For example, the method implementation in the present disclosure can be implemented as a software program, which is tangibly contained in a machine-readable medium, such as a memory. In some embodiments, part or all of the software program can be loaded and / or installed via a memory and / or a communication interface. When the software program is loaded into the memory and executed by the processor, one or more steps in the method described above can be performed. Alternatively, in other embodiments, the processor can be configured to perform one of the above methods in any other appropriate manner (e.g., by means of firmware).

[0316] The logic and / or steps represented in the flowchart or otherwise described herein may be embodied in any readable storage medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other system that can fetch instructions from and execute instructions on an instruction execution system, apparatus, or device), or in combination with such instruction execution systems, apparatuses, or devices.

[0317] For the purposes of this specification, a "readable storage medium" may be any device that can contain, store, communicate, propagate or transmit a program for use with or in conjunction with an instruction execution system, device or apparatus. More specific examples of readable storage media (a non-exhaustive list) include the following: an electrical connection with one or more wirings (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and editable read-only memory (EPROM or flash memory), a fiber optic device, and a portable read-only memory (CDROM). In addition, the readable storage medium may even be paper or other suitable medium on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or, if necessary, processing in another suitable manner, and then stored in a memory.

[0318] It should be understood that the various parts of the present disclosure can be implemented with hardware, software or a combination thereof. In the above-mentioned embodiments, a plurality of steps or methods can be implemented with software stored in a memory and executed by a suitable instruction execution system. For example, if implemented with hardware, as in another embodiment, it can be implemented with any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0319] A person of ordinary skill in the art of the present disclosure may understand that all or part of the steps of implementing the above-mentioned implementation method may be completed by instructing related hardware through a program, and the program may be stored in a readable storage medium, which, when executed, includes one or a combination of the steps of the implementation method.

[0320] In addition, each functional unit in each embodiment of the present disclosure may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a readable storage medium. The storage medium may be a read-only memory, a disk or an optical disk, etc.

[0321] The present disclosure also provides an electronic device, including: a memory, the memory storing execution instructions; and a processor or other hardware module, the processor or other hardware module executes the execution instructions stored in the memory, so that the processor or other hardware module executes the above method.

[0322] The present disclosure also provides a readable storage medium, in which execution instructions are stored. When the execution instructions are executed by a processor, they are used to implement the above method.

[0323] In summary, the method disclosed in the present invention has the following advantages:

[0324] (1) Different from most vehicle speed estimation methods based on millimeter-wave radar targets, this method converts the millimeter-wave radar targets into the vehicle coordinate system for unified processing. Therefore, any number of millimeter-wave radars can be used, and there is no requirement for the radar installation azimuth angle, which makes it more versatile.

[0325] (2) Different from using filtering algorithms and common abnormal data elimination methods (such as RANSAC), the present disclosure uses historical estimation results to predict the distribution of the current target cluster and performs matching calculations with the actual distribution of the target cluster in the current frame. The matching degree of the target cluster has nothing to do with the number of targets in the target cluster itself, but is only related to the position matching degree and the change in the number of the same target cluster in the two frames. It can effectively distinguish the target clusters formed by large dynamic targets (such as large trucks traveling in opposite directions). The present disclosure does not rely on the setting of fixed parameters (compared to, for example, RANSAC), and has a wider range of applications. Compared with existing technical methods, the present disclosure can be used in multi-dynamic target scenarios and has higher robustness.

[0326] (3) Using the height confidence of the target in three-dimensional space, when the algorithm cannot obtain sufficiently reliable results, the spatial confidence within the height range will be reduced according to the height of most pedestrians and vehicles from the ground, thereby reducing the impact of dynamic targets of vehicles in common scenes with a high probability. Compared with existing technical methods, the present disclosure is more adaptive.

[0327] It can be seen that based on the 4D millimeter wave radar measurement results, through the above-mentioned vehicle speed state estimation method, the autonomous driving vehicle can eliminate the influence of dynamic targets on the vehicle speed estimation results even in the multi-dynamic target scene, and its vehicle speed estimation results are real-time, accurate and highly reliable. Whether the vehicle is in an environment with all static objects or in a multi-dynamic target scene with dynamic targets as the main ones, a good speed estimation effect can be achieved. It has a very broad industrial application prospect.

[0328] So far, the technical solutions of the present disclosure have been described in conjunction with the preferred implementation methods shown in the accompanying drawings. However, those skilled in the art should understand that the above implementations are only for the purpose of clearly illustrating the present disclosure, and are not intended to limit the scope of the present disclosure. The protection scope of the present disclosure is obviously not limited to these specific implementations. Without departing from the principles of the present disclosure, those skilled in the art may make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions are still within the scope of the present disclosure.

Claims

1. A vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar, characterized in that: include: Step 1: Read the target data collected by the 4D millimeter wave radar in real time, wherein the target data includes distance, direction angle, pitch angle and radial Doppler velocity; Step 2: convert the target data, cluster the target to obtain the target cluster and its attributes, initialize if the data is the first frame, and end the calculation; Step 3: Use the estimation result of the previous frame to calculate the matching degree of the target cluster of the current frame, and combine the target cluster height confidence to obtain the target cluster confidence; Step 4: Establish the speed state and target cluster observation equation, use the vehicle speed state and the current frame target cluster to establish the optimization equation and solve it; Step 5: Evaluate the estimated vehicle speed state to determine whether the output condition is met. If so, output the estimated information of the vehicle speed state. If not, reduce the target cluster height confidence and return to step 4. in, The using the estimation result of the previous frame to calculate the matching degree of the target cluster of the current frame includes: Firstly, the predicted target cluster in the vehicle coordinate system of the current frame is calculated by estimating the vehicle speed state of the previous frame and the target cluster of the previous frame to obtain the predicted target cluster; Then, search for the predicted target cluster within a certain range of the current frame target cluster; Finally, the target cluster matching degree is calculated by the target cluster matching degree; The matching degree of the target cluster is calculated as follows: the distance matching degree between the predicted target cluster and the target cluster of the current frame is calculated. Then calculate the target cluster target quantity matching degree Q, through Combined with Q, the matching degree M of the target cluster of the current frame radar is obtained; Among them, the calculation formula of the target cluster distance matching degree is expressed as follows using the normal distribution of the predicted target cluster: in is the prediction of the kth target cluster of the jth radar, is relative to the predicted target cluster The distance matching degree, x is the coordinate of any position in three-dimensional space, is a three-dimensional Euclidean space; The calculation formula of the target quantity matching degree is expressed as follows using the quantity difference distribution: Where x1, x2 are the number of targets in any two target clusters, Q is the matching degree of the number of targets, is the space of positive integers; Combining the above two formulas (11) and (12), the matching formula of the target cluster is as follows: Thus, the confidence of the target cluster of the jth radar in the current frame is obtained Optimization equations are established based on vehicle speed states and radar Doppler observations and solved using radar target clusters; Evaluate the estimated vehicle speed state to determine whether it meets expectations; Use spectral clustering algorithm to classify the target points of each radar Perform clustering calculations.

2. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 1 is characterized in that: The number of the 4D millimeter-wave radars is one or more.

3. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 2 is characterized in that: set up: The number of the 4D millimeter wave radars is: N r ; The number of target clusters in the jth radar is: N j ; The time interval between two frames is: τ; The speed state of the vehicle is based on the vehicle coordinate system; The speed state includes: the longitudinal linear speed V along the driving direction in the vehicle driving plane x , and V x Vertical transverse linear velocity V y , the angular velocity ω of the vehicle coordinate system generated during the vehicle steering process, and the lateral velocity of the vehicle coordinate system is 0, then the velocity state to be estimated is expressed as: Where S t is the vehicle speed state at the tth frame, is the longitudinal linear velocity of the tth frame, ω t is the rotation angular velocity of the tth frame; The one or more 4D millimeter wave radars are horizontally installed around the vehicle body to observe the vehicle surrounding environment, and the target data is relative to the radar coordinate system.

4. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 3 is characterized in that: If there are multiple radars, data time synchronization is performed on the multiple 4D millimeter-wave radars to ensure that each frame of data contains all data of all radars near the same time, and each frame of data is output at a stable frequency.

5. A vehicle speed state estimation method based on a vehicle-mounted 4D millimeter-wave radar according to any one of claims 1 to 4, characterized in that: After the vehicle is started, the system is initialized. At this time, the vehicle is stationary. After the system is powered on and the program is run, the first frame of radar data is received. At this time, the speed state of the vehicle is zero, that is, S0{v x =0,ω=0}.

6. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 5 is characterized in that: Convert the target data of each radar from the radar coordinate system to the vehicle coordinate system; The three-dimensional spatial coordinates of each target in the jth radar in the vehicle coordinate system It is expressed as follows: Get the target of each radar in the vehicle coordinate system; Among them, r i j is the distance of the i-th target of the j-th radar, is the horizontal angle of the i-th target of the j-th radar, is the elevation angle of the i-th target of the j-th radar, is the Doppler velocity of the i-th target of the j-th radar; is the installation external parameter of the jth radar relative to the vehicle coordinate system, where are the positions of the jth radar coordinate system relative to the vehicle coordinate system on the x, y, and z axes, respectively. are the angles of the jth radar relative to the vehicle coordinate system around the z-axis, y-axis, and x-axis; when the radar is installed horizontally, If both are 0, the installation external parameters are simplified to 7. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 6 is characterized in that: Use DBSCAN (Density-based spatial clustering of applications with noise) algorithm to cluster the target points of each radar Perform clustering calculations and use the kd-tree method to speed up distance searches in three-dimensional space and improve computational efficiency; obtain the core points, core reachable points, and outlier points of the target through the DBSCAN algorithm; Each core point and its core reachable points are regarded as a target cluster, and the coordinates of the core point are the target cluster coordinates, expressed as Each outlier is regarded as a target cluster, and the coordinates of the outlier are the coordinates of the target cluster. Finally, the target cluster of each radar in the vehicle coordinate system is expressed as follows: in is the kth target cluster of the jth radar; N r is the number of radars, N j is the number of target clusters of the jth radar; The target set in each target cluster is: Where P i jk is the i-th target of the k-th target cluster of the j-th radar, is the total number of targets of the kth target cluster of the jth radar; The speed of the target cluster is the weighted average of the speeds of all targets contained in the target cluster and the distance distribution from the target to the center of the target cluster, i.e., the coordinate point of the target cluster; Calculate the velocity of each target cluster; The distance between the target in the target cluster and the center of the target cluster is expressed as follows: in, is the distance between the ith target of the kth target cluster of the jth radar and the center of the target cluster; middle is the position of the i-th target of the k-th target cluster of the j-th radar, and its coordinates are (x, y, z); is the center coordinate of the kth target cluster of the jth radar; Represents the maximum distance, then calculate the k-th target cluster velocity of the j-th radar The formula is as follows: in is the speed of the ith target of the kth target cluster of the jth radar; the position coordinates and target cluster speed of each target cluster of each radar in the vehicle coordinate system are obtained, thereby obtaining the target cluster and target cluster speed of each radar in the vehicle coordinate system.

8. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 1 is characterized in that: is the velocity state estimation of the previous frame, V x t-1 is the longitudinal linear velocity estimated in the previous frame, ω t-1 The estimated vehicle rotation speed for the previous frame; is the target cluster set of the previous frame, and the coordinate position of the kth target cluster of the jth radar relative to the vehicle coordinate system is is the kth target cluster of the jth radar in the t-1th frame, and its three-dimensional coordinates are {g x ,g y ,g z }; The transformation parameters obtained according to the vehicle speed state of the previous frame are: where t x =V x t-1 τ, t y =0,φ=ω t-1 τ; Since the time interval τ between two adjacent frames is usually very small, only t x and φ; the transformation parameters are as follows: Where V x t-1 is the longitudinal linear velocity estimated in the previous frame, ω t-1 is the estimated vehicle rotation speed of the previous frame, τ is the time interval between two frames; The prediction process is as follows: in, is the set of target clusters of the jth radar in the previous frame. The target clusters in the set are: Total N j Each target cluster is transformed by the transformation function T to obtain is the set of predicted target clusters of the jth radar in the previous frame in the vehicle coordinate system of the current frame is the predicted target cluster of the kth target cluster of the jth radar in the tth frame, and its three-dimensional coordinates are {g px ,g py ,g pz }; The expression of the transformation function T is as follows: in is the position coordinate after transformation, is the position coordinate before transformation; is the three-dimensional rotation matrix, φ is the rotation angle, is the translation vector along the vehicle’s forward direction; Get the predicted target cluster in the current vehicle coordinate system.

9. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 8, characterized in that: The searching for a predicted target cluster within a certain range of the current frame target cluster includes: Search whether there is a predicted target cluster within a certain range near the three-dimensional space of the target cluster of the current frame. The search range of each target cluster in the current frame is different, which is expressed as Its value can be obtained by the following formula: in is the distance threshold between the kth target cluster of the jth radar and the predicted target cluster, V x t-1 ,ω t-1 is the state estimation of the previous frame, that is, the longitudinal linear velocity of the previous frame and the rotational angular velocity of the previous frame.

10. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 8, characterized in that: Calculate the target cluster matching degree based on the search results: if there is no predicted target cluster within the target cluster range, set the matching degree to zero, i.e., M = 0; if there are f (f = 1, 2, 3...) predicted target clusters, calculate the matching degree M with these f predicted target clusters respectively. f , and sort the matching degrees, taking the maximum matching degree M f ,f=argMax(M f ) as the matching degree of the target cluster of the current frame.

11. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 1, characterized in that: The optimization equation is established according to the vehicle speed state and the radar Doppler observation, and solved using the radar target cluster, including: is the speed state of the vehicle in the current frame, and the installation external parameter of the jth radar is The Doppler velocity observation equation for the current frame is as follows: where v d is the radial observed velocity of the target in the direction of the j-th radar azimuth angle α; The radial observation velocity of the kth target cluster of the jth radar is The error function is as follows: where e j is the error of the jth radar, N j is the number of target clusters in the jth radar, is the confidence of the kth target cluster in the current frame of the jth radar. The error function associates the speed of the target cluster observed by the radar with the speed state of the vehicle, and transforms the solution of the current frame speed state into the solution of the optimization process. The current frame speed state is as follows: Where N r is the number of radars, N j is the number of target clusters of the jth radar; Finally, the nonlinear least squares method is used to solve the vehicle speed state of the current frame and the residual value e rest =min(e).

12. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 11, characterized in that: The step of evaluating the estimated vehicle speed state to determine whether it meets expectations includes: Use the evaluation function to evaluate the vehicle speed state of the current frame. The evaluation function is as follows: E=T(S t-1 ,S t )W(S t-1 ,S t )e rest (17) Its evaluation value is E, T(S t-1 ,S t ) is the longitudinal linear velocity evaluation value, W(S t-1 ,S t ) is the rotation angular velocity evaluation value, a max is the maximum longitudinal acceleration threshold of the vehicle, Φ max is the maximum rotation angular acceleration threshold of the vehicle; if the evaluation value E reaches the threshold Et, the result of the vehicle speed state is directly output If the evaluation value does not reach the threshold Et, the target cluster is added with the target height confidence on the basis of the matching degree calculation, and the matching degree of the target cluster is recalculated.

13. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 12, characterized in that: The target height confidence formula is: where ξ h is the confidence parameter, and its initial value is ξ h =0.8,h target is the target height, h1 and h2 are the upper and lower limits of the height range, respectively, satisfying h2>h1; that is, when h target At 2h target target >h2, P h The value is 1; when h target At 2h target ∈[h1,h2], P h The value is ξ h ;​ h m is the height of the rear axle center from the ground, H is the average height of pedestrians and vehicles, where h1 = -h m , h2=Hh m ; The confidence of targets within the h1 and h2 intervals will be reduced, while h2 and the highest target at height h w The static object confidence of the interval will remain unchanged.

14. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 13, characterized in that: The height confidence distribution of each radar is: in is the kth target cluster of the jth radar, for The z-axis coordinate value in; Finally, the confidence of the kth target cluster of the jth radar is obtained for: Iterate the calculation, if the evaluation value E reaches the threshold Et, output the result If it is still not satisfied, adjust the allocation parameter ξ h , halved, that is, ξ h =0.5ξ h , continue to return iteration; If the evaluation value E does not reach the threshold Et after a limited number of iterations, the estimation result of the previous frame is used As the result of the estimation of this frame, even if S t =S t-1 .

15. The vehicle speed state estimation method based on vehicle-mounted 4D millimeter wave radar according to claim 6, characterized in that: The spectral clustering algorithm is used to cluster the target points of each radar. Perform clustering calculation, including the following steps: (1) Use the three-dimensional spatial coordinates of the target in each radar in the vehicle coordinate system to construct an undirected graph: The undirected graph G is constructed by describing the set of points and the set of edges, that is, G(V,E); where V is the target of the radar in the vehicle coordinate system (v1, v2, ..., v n ); E is the set of edges; weight w ij For point v i and point v j The weight between ij =w ji ; (2) Construct the adjacency matrix W and degree matrix D based on the similarity matrix: Degree i It is defined as the sum of the weights of all edges connected to it, that is, Using the definition of each point degree, we get an n×n degree matrix D, which is a diagonal matrix with values ​​only on the main diagonal, corresponding to the degree of the jth point in the i-th row, expressed as follows: The adjacency matrix W is defined using the full connection method, and its matrix elements are expressed as follows: where v i and v j are the targets of the two radars in the vehicle coordinate system, and σ is the variance value; (3) Calculate the Laplace matrix L: The Laplacian matrix of the undirected graph G is: L=DW (26) According to the definition of the Laplace matrix, for any vector f: Suppose the subgraph set is A1, A2, …, A k ,satisfy A1∪A2∪…∪A k =V; The weight of the cut graph is: Where A and B are two subgraph sets, i and j are the subset numbers of A and B respectively; Use NCut to cut the image: in A i The complement of i The union of all subsets of V except ; The optimization objective function is: The normalized Laplacian matrix is: (4) Calculate the eigenvectors f corresponding to the smallest k1 eigenvalues ​​of L, and normalize the matrix composed of f by row; obtain the n×k1-dimensional eigenmatrix F; (5) Take each row in the feature matrix F as a k1-dimensional sample, a total of n samples, and use the k-means clustering method to cluster, with a clustering dimension of k2; (6) Finally, we get the cluster partition C(c1,c2,…,c k2 ), the mean of the target positions in each target cluster is taken as the target cluster position, and the target cluster is obtained.

16. A vehicle speed state estimation method and device based on vehicle-mounted 4D millimeter wave radar, characterized in that: include: A data acquisition device reads target data collected by the 4D millimeter wave radar in real time, wherein the target data includes distance, direction angle, pitch angle and radial Doppler velocity; A data conversion device converts the target data, clusters the target data to obtain the target cluster and its attributes, initializes the data if it is the first frame, and ends the calculation; A matching degree calculation device is used to calculate the matching degree of the target cluster of the current frame using the estimation result of the previous frame, and obtain the target cluster confidence degree in combination with the target cluster height confidence degree; The optimization solving device establishes the speed state and the target cluster observation equation, and uses the vehicle speed state and the current frame target cluster to establish the optimization equation and solve it; An evaluation device evaluates the estimated vehicle speed state to determine whether the output condition is met, and if so, outputs the estimated information of the vehicle speed state; if not, reduces the target cluster height confidence and returns to the optimization solution device for recalculation; in, The using the estimation result of the previous frame to calculate the matching degree of the target cluster of the current frame includes: Firstly, the predicted target cluster in the vehicle coordinate system of the current frame is calculated by estimating the vehicle speed state of the previous frame and the target cluster of the previous frame to obtain the predicted target cluster; Then, search for the predicted target cluster within a certain range of the current frame target cluster; Finally, the target cluster matching degree is calculated by the target cluster matching degree; The matching degree of the target cluster is calculated as follows: the distance matching degree between the predicted target cluster and the target cluster of the current frame is calculated. Then calculate the target cluster target quantity matching degree Q, through Combined with Q, the matching degree M of the target cluster of the current frame radar is obtained; Among them, the calculation formula of the target cluster distance matching degree is expressed as follows using the normal distribution of the predicted target cluster: in is the prediction of the kth target cluster of the jth radar, is relative to the predicted target cluster The distance matching degree, x is the coordinate of any position in three-dimensional space, is a three-dimensional Euclidean space; The calculation formula of the target quantity matching degree is expressed as follows using the quantity difference distribution: Where x1, x2 are the number of targets in any two target clusters, Q is the matching degree of the number of targets, is the space of positive integers; Combining the above two formulas (11) and (12), the matching formula of the target cluster is as follows: Thus, the confidence of the target cluster of the jth radar in the current frame is obtained Optimization equations are established based on vehicle speed states and radar Doppler observations and solved using radar target clusters; Evaluate the estimated vehicle speed state to determine whether it meets expectations; Use spectral clustering algorithm to classify the target points of each radar Perform clustering calculations.

17. An electronic device, characterized in that: include: A memory storing execution instructions; as well as A processor, wherein the processor executes the execution instruction stored in the memory so that the processor executes the method according to any one of claims 1 to 15.

18. A readable storage medium, characterized in that: The readable storage medium stores execution instructions, which are used to implement the method according to any one of claims 1 to 15 when executed by a processor.

Images