High-dynamic driving scene millimeter wave radar all-weather positioning method, medium and equipment

Abstract

The invention provides a millimeter-wave radar all-weather positioning method in a high-dynamic driving scene, a readable storage medium and computer equipment, and relates to the technical field of automatic driving, and the method comprises the steps: obtaining a plurality of frames of millimeter-wave radar historical scanning images and current scanning images, and sequentially carrying out the inter-frame feature extraction, inter-frame feature matching and motion distortion compensation; static landmarks and dynamic feature point clouds in the space are separated out, and meanwhile, a dynamic point cloud membership rigid body is determined; performing coarse configuration according to static landmark features to obtain coarse-grained inter-frame motion estimation, then binding state equations of two frames of dynamic landmarks to derive a dynamic feature motion equation, and further performing fine-grained optimization on relative change poses by combining static feature points and dynamic feature point constraints; and performing nonlinear optimization solution on the fine-grained optimization to obtain vehicle motion state estimation. According to the method, accurate, real-time and all-weather self-adaptive vehicle motion state estimation in a high-dynamic driving scene can be realized, and all-weather safe and reliable automatic driving is ensured.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a millimeter-wave radar all-weather positioning method, medium and equipment for high-dynamic driving scenarios. Background Art

[0002] With the promotion of the new industrialization process and the development of artificial intelligence technology, autonomous driving, as a key technology of the automotive industrial revolution, is accelerating the profound changes in industries such as transportation and automobiles. Simultaneous Localization and Mapping (SLAM) is one of the key technologies of autonomous driving, providing real-time pose estimation and key landmark features in the environment for intelligent vehicles. Existing solutions mainly rely on the Global Navigation Satellite System (GNSS), high-definition cameras, and high-precision LiDAR. However, GNSS signals are easily blocked by high-rise buildings in cities, resulting in meter-level positioning errors, making it difficult to meet high-precision positioning requirements. Although high-definition cameras and high-precision LiDAR can achieve decimeter-level high-precision positioning requirements, visual sensors are easily affected by ambient lighting and texture conditions, and LiDAR is difficult to penetrate smoke and haze, so it is difficult to achieve stable and reliable positioning estimation all day long. In contrast, millimeter-wave radar has the advantage of good penetration in severe weather conditions, providing a key driving force for intelligent vehicles to achieve all-weather autonomous navigation.

[0003] Accurate scanning and matching between millimeter-wave radar frames is the key to achieving all-weather autonomous positioning of intelligent vehicles. However, in highly dynamic driving scenarios, the vehicle's own motion will cause distortion in the measured distances in different directions, thus affecting the accuracy of scanning and matching between frames. In addition, when more dynamic vehicles are captured within the radar scanning field of view, the dynamic point cloud in the environment will cause deviations in scanning and matching during positioning, reducing the positioning performance of the system and affecting the quality of the map aggregated through the landmark point cloud, thereby affecting the relocation effect of the measured point cloud to the prior map. Therefore, how to design an autonomous positioning solution suitable for vehicles in highly dynamic scenarios and extract high-quality sustainable observable landmark features is still a difficult problem that needs to be solved urgently.

[0004] Traditional millimeter-wave radar self-localization solutions mainly focus on: 1) spatially aligning key feature points extracted from radar scan maps with a pre-constructed high-definition point cloud map. However, when the vehicle enters a completely new and unknown area or the point cloud map is not updated in a timely manner, this map-based relocalization solution will fail. 2) Decoupling the vehicle's self-motion state by aligning the same features between millimeter-wave radar scan frames. However, this solution is based on the assumption of static feature alignment, and the relative transformation of dynamic features will cause deviations in the self-motion decoupling process, thus affecting the vehicle's self-localization effect. Therefore, how to design a millimeter-wave radar self-localization solution suitable for high-dynamic driving scenarios is still a key problem to be solved in realizing an all-weather, safe and reliable autonomous driving system. Summary of the Invention

[0005] The purpose of the present invention is to provide a millimeter-wave radar all-weather localization method, medium and device for high-dynamic driving scenarios, which can realize accurate, real-time and all-weather adaptive vehicle motion state estimation in high-dynamic driving scenarios, and ensure all-weather, safe and reliable autonomous driving in high-dynamic driving scenarios. The specific technical solutions are as follows:

[0006] A millimeter-wave radar all-weather localization method for high-dynamic driving scenarios, characterized in that the method includes the following steps:

[0007] S100. Obtain multiple frames of millimeter-wave radar historical scan maps and the current scan map, and perform inter-frame feature extraction, inter-frame feature matching and motion distortion compensation in sequence;

[0008] S200. Model according to the sensing characteristics of the millimeter-wave radar to obtain the statistical characteristics of the anisotropic distribution of spatial sensing measurement information, and then calculate the probability distribution of the distance error between feature point clouds according to the rotation invariance of geometric transformation. Measure the likelihood that the feature point cloud comes from the same rigid body transformation through the statistical distribution characteristics of measurement noise, so as to separate the static landmarks and dynamic feature point clouds in space, and at the same time determine the rigid body to which the dynamic point cloud belongs;

[0009] S300. Obtain a coarse-grained inter-frame motion estimate according to the static landmark features, then bundle the state equations of two frames of dynamic landmarks to derive the dynamic feature motion equation, and then perform fine-grained optimization on the relative change pose in combination with the static feature points and dynamic feature point constraints, and perform non-linear optimization solution on the fine-grained optimization to obtain the vehicle motion state estimate.

[0010] Further, in the step S100, multiple frames of millimeter-wave radar historical scan maps and the current scan map are used as inputs, data cleaning is performed along each azimuth beam of each scan map, and the millimeter-wave radar point cloud corresponding to multiple pixels with the strongest reflection energy is retained and used as the candidate landmark feature point cloud to achieve inter-frame feature extraction.

[0011] Further, in the step S100, calculate the ORB descriptors of the pixels corresponding to each feature point cloud, and perform data association on the candidate landmark feature point clouds between adjacent frames by brute-force matching the descriptors, so as to obtain feature matching pairs.

[0012] Further, in the step S100, the compensation for the motion distortion of the radar scan map is specifically as follows: Let the point observed in the millimeter-wave radar coordinate system of the k-th frame of the vehicle be denoted as which corresponds to the azimuth angle α corresponding to the i-th range bin φ i in the scan map and the j-th angle bin, then: j

[0013]

[0014] where η, r i , N a and a j respectively represent the range resolution of the radar scan map, the range bin corresponding to the observation point, the total number of angle bins, and the angle bin corresponding to the observation point;

[0015] On the premise that the millimeter-wave radar performs constant-speed rotational scanning, compensate each point of the motion distortion:

[0016]

[0017] where is the coordinate representation of the compensated feature point cloud in the radar coordinate system of the k-th frame, v k-1 is the angular velocity and linear velocity of the millimeter-wave radar in the (k - 1)-th frame, δt represents the time interval between the k-th frame and the (k - 1)-th frame, and the exponential function Exp maps the rotation vector to the rotation matrix in the SO(2) Lie group space: [v] × is the skew-symmetric matrix corresponding to the vector v.

[0018] Further, in the step S200, the point cloud measurement error covariance in the radar scan map can be modeled as a joint distribution of angular error and range error that follows a normal distribution:

[0019]

[0020] where n = [cosα j sinα j T is the direction vector, are the variances of the angular error and range error distributions respectively, is defined as the observable quantity​​ The corresponding nominal state quantity, with a magnitude equal to the difference between the observed quantity and the random error ; and are briefly denoted as and nominal state strictly satisfies the static feature registration constraint where and respectively represent the rotation and translation from the (k + 1)-th frame radar reference system to the k-th frame radar reference system; thus, the coordinates of the i-th static feature observation point in the k-th and (k + 1)-th frames after motion compensation and correspondingly satisfy:

[0021]

[0022] wherein, is the random error corresponding to the static feature observation point ;

[0023] The coordinates of the i-th feature point of the same dynamic object reflected in the k-th and (k + 1)-th frames and satisfy the rigid body motion transformation consistency:

[0024]

[0025] wherein, are respectively the rotation matrix and translation component of the transformation matrix of the dynamic object from time k to (k + 1), and is the random error corresponding to the dynamic feature observation point ;

[0026] Since the feature points returned from the same rigid body have a consistent relative motion change, the L-2 norm distance between the feature point clouds i and j remains rotation invariant:

[0027]

[0028] wherein,

[0029] The above equation can be approximated as:

[0030]

[0031] where A′ is a binary coefficient matrix, representing in the linear combination coefficients of the error basis vectors;

[0032] From this, it can be obtained Variance of measurement noise for:

[0033]

[0034] in, are the uncertainty variances of millimeter-wave radar angle and range measurement, respectively.

[0035] Furthermore, the 90% upper quantile of the normal distribution is calculated based on the uncertainty variance and used as the threshold to identify the coordinates The feature point cloud i and coordinates of the kth frame are The likelihood of whether the feature point cloud j of the kth frame comes from the same rigid body cluster is obtained, and some isolated points can be eliminated from the dynamic or static point cloud based on this; then, the Bron-Kerbosch algorithm is used to obtain the largest cluster from such a filtered inner group; then, according to the number of vertices, the largest cluster is used as a static point, and the other clusters are used as dynamic points. By extracting the second largest cluster, the dynamic feature point cloud clusters from different rigid bodies can be separated, thereby realizing the decoupling of the dynamic feature space.

[0036] Furthermore, in step S300, the dynamic group from the same vehicle is obtained through the radar dynamic point cloud spatial decoupling model, and is further used as a geometric constraint for solving the self-motion state. The transformation matrix of the moving vehicle can be obtained through theoretical derivation. The following analytical relationship is satisfied between and dynamic feature transformation:

[0037]

[0038] in, is the global posture state of the millimeter-wave radar at the (k+1)th frame;

[0039] The geometric consistency constraint of dynamic feature points is used to optimize the motion estimation between adjacent key frames. Under the assumption of a constant velocity model, the relative change matrix equation of two key frames is combined to derive the dynamic feature transformation between adjacent frames and the dynamic feature coupling constraint of the pose to be optimized:

[0040]

[0041] in, and They represent the relative transformation matrices from the kth frame to the (k+1)th frame and from the kth frame to the (k-1)th frame of the radar coordinate system, respectively. Respectively represent the transformation matrix of the dynamic object from time k to (k+1) and from (k-1) to k;

[0042] Consider optimizing the vehicle motion states of the (k - 1)-th, k-th, and (k + 1)-th frames simultaneously, and perform fine-grained optimization of the vehicle pose by combining the registration constraints of static feature point pairs and the coupling constraints of dynamic feature point pairs:

[0043]

[0044] Among them, the first two terms are static registration constraints, and the third term is a dynamic feature coupling constraint. represents all static feature points in the k-th and (k + 1)-th frames. represents the optimized spatial transformation matrix of the millimeter-wave radar. represents all dynamic feature points in the k-th and (k + 1)-th frames, ∑ p and ∑ D respectively represent the error uncertainties corresponding to each feature point. represents and perform matrix multiplication with the homogeneous coordinates of, Log is the exponential mapping, which maps the SO(2) orthogonal group to its corresponding Lie algebra space, and β is used to balance the static registration constraint and the dynamic feature coupling constraint.

[0045] By using a non-linear optimization algorithm to solve the above fine-grained optimization, the optimal spatial transformation matrix is calculated, thereby realizing the inter-frame motion estimation of dynamic feature optimization.

[0046] Furthermore, the non-linear optimization algorithm is the Levenberg-Marquardt non-linear optimization algorithm.

[0047] The present invention also provides a computer-readable storage medium, on which a computer program is stored, and is characterized in that when the computer program is executed by a processor, the steps of the millimeter-wave radar all-weather positioning method for high-dynamic driving scenarios as described in any one of claims 1-8 are implemented.

[0048] The present invention also provides a computer device, including a memory, a processor, and a computer program stored on the memory and executable on the processor, and is characterized in that when the processor executes the program, the steps of the millimeter-wave radar all-weather positioning method for high-dynamic driving scenarios as described in any one of claims 1-8 are implemented.

[0049] The millimeter-wave radar all-weather positioning method, medium, and device provided by the present invention have the following beneficial effects:

[0050] The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios provided by the present invention obtains multiple frames of historical millimeter-wave radar scan maps and the current scan map, and sequentially performs inter-frame feature extraction, inter-frame feature matching, and motion distortion compensation; models the statistical characteristics of the anisotropic distribution of spatial perception measurement information based on the sensing characteristics of the millimeter-wave radar, and then calculates the probability distribution of the distance error between feature point clouds according to the rotation invariance of geometric transformation. The likelihood that the feature point cloud comes from the same rigid body transformation is measured through the statistical distribution characteristics of measurement noise, so as to separate static landmarks and dynamic feature point clouds in space, and at the same time determine the rigid body to which the dynamic point cloud belongs; obtains a coarse-grained inter-frame motion estimate based on the characteristics of static landmarks, then bundles the state equations of two frames of dynamic landmarks to derive the dynamic feature motion equation, and further combines the static feature points and dynamic feature point constraints to perform fine-grained optimization on the relative change pose, and solves the fine-grained optimization through non-linear optimization to obtain the vehicle motion state estimate; thus, in high-dynamic and low-texture autonomous driving scenarios, real-time and accurate vehicle motion state estimation can be achieved by using the geometric consistency constraint of dynamic feature points. In addition, according to the anisotropic statistical distribution characteristics of millimeter-wave radar spatial perception information, a radar dynamic point cloud space decoupling model is designed to separate static landmarks and dynamic landmark point clouds in space, determine the rigid body to which the dynamic point cloud belongs, and eliminate the deviation brought by dynamic feature points to self-motion estimation. Moreover, a vehicle motion estimation scheme enhanced by dynamic transformation is proposed, and the dynamic feature motion equation is theoretically derived. Then, the relative change pose is finely optimized by combining static feature points and dynamic feature point constraints. Compared with existing vehicle autonomous positioning schemes, the millimeter-wave radar autonomous positioning scheme based on dynamic feature transformation enhancement proposed by the present invention can achieve accurate, real-time, all-weather and adaptive vehicle motion state estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 FIG. is a schematic flowchart of an all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios provided by the present invention;

[0052] Figure 2 FIG. is the overall flowchart of an embodiment of the present invention;

[0053] Figure 3 FIG. is the specific flowchart of an embodiment of the present invention;

[0054] Figure 4 FIG. is the structural block diagram of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0055] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings provided by the present invention. According to the following description, the advantages and features of the present invention will be clearer. It should be noted that the accompanying drawings are all in a very simplified form and use non-precise scales, only for the purpose of facilitating and clearly assisting in explaining the objectives of the embodiments of the present invention.

[0056] Embodiment 1

[0057] This embodiment provides a millimeter-wave radar all-weather positioning method for high-dynamic driving scenarios. Referring to Figures 1-3 as shown, the method includes the following steps:

[0058] S100. Obtain multiple frames of millimeter-wave radar historical scan maps and the current scan map, and sequentially perform inter-frame feature extraction, inter-frame feature matching, and motion distortion compensation.

[0059] In one embodiment, multiple frames of millimeter-wave radar historical scan maps and the current scan map are used as inputs. Data cleaning is performed along each azimuth beam of each scan map, and the millimeter-wave radar point cloud corresponding to the multiple pixels with the strongest reflection energy is retained and used as the candidate landmark feature point cloud to achieve inter-frame feature extraction.

[0060] Assume that a rotary millimeter-wave radar provides an environmental scan map for an autonomous vehicle at a data rate of f = 10 Hz. Considering the ghost noise caused by the multipath effect and the speckle noise introduced by the random coherent interference of reflected waves, if the rough surface features are simply used, it will introduce biases to the subsequent feature alignment, thus affecting the accuracy of the subsequent millimeter-wave radar self-motion estimation. Therefore, specifically, the present invention first uses the first two frames of millimeter-wave radar historical scan maps and the current scan map of the current scan map as inputs. Data cleaning is performed along each azimuth beam of each scan map, and the millimeter-wave radar point cloud corresponding to 30 pixels with the strongest reflection energy is retained and used as the candidate landmark feature point cloud, thereby achieving inter-frame feature extraction. By filtering clutter along the azimuth beam, on the one hand, random weak-energy speckle noise can be filtered out, and the ghost point cloud caused by the multipath effect can be alleviated; on the other hand, more feature information can be retained for subsequent scan matching.

[0061] In one embodiment, calculate the ORB descriptors of the pixels corresponding to each feature point cloud, and perform data association on the candidate landmark feature point clouds between adjacent frames and frames by brute-force matching the descriptors to obtain feature matching pairs.

[0062] Considering that the self-motion of an intelligent vehicle in a high-speed motion scenario will cause the point clouds scanned along different azimuths of the same scan map to be out of sync in time, the present invention compensates for the motion distortion of the radar scan map.

[0063] In one embodiment, the compensation for the motion distortion of the radar scan map is as follows: Assume that the points observed by the vehicle in the millimeter-wave radar coordinate system at the k-th frame are denoted as which corresponds to the azimuth angle α corresponding to the i-th range cell φ i in the scan map and the j-th angle cell, then: j That is:

[0064]

[0065] where η, r i , N a and a j respectively represent the range resolution of the radar scan map, the mileage unit corresponding to the observation point, the total number of angle units, and the angle unit corresponding to the observation point; on the premise that the millimeter-wave radar performs constant-speed rotation scanning, each point of the motion distortion is compensated:

[0066]

[0067] where is the coordinate representation of the compensated feature point cloud in the k-th frame radar coordinate system, v k-1 is the angular velocity and linear velocity of the millimeter-wave radar in the (k - 1)-th frame, δt represents the time interval between the k-th frame and the (k - 1)-th frame, and the exponential function Exp maps the rotation vector to the rotation matrix in the SO(2) Lie group space: [v] × is the skew-symmetric matrix corresponding to the vector v.

[0068] In the embodiment of the present invention, due to the influence of the signal multipath effect and the distortion of the spatial information measurement caused by the high-speed movement of the millimeter-wave radar, simply using the rough surface features will introduce deviations to the subsequent feature alignment, affecting the accuracy of the subsequent millimeter-wave radar self-motion estimation. For this reason, the present invention designs a radar scan data cleaning scheme to clean the data of the original millimeter-wave radar scan map, filter the ghost noise caused by the multipath effect and the speckle noise introduced by the random coherent interference of the reflected wave, and compensate for the measurement distortion based on the change of the millimeter-wave radar motion state, so as to provide accurate spatial point cloud feature information for the subsequent program.

[0069] S200. According to the sensing characteristics of the millimeter-wave radar, model the statistical characteristics of the anisotropic distribution of the spatial sensing measurement information, and then calculate the probability distribution of the distance error between the feature point clouds according to the rotation invariance of the geometric transformation, and measure the likelihood that the feature point clouds come from the same rigid body transformation through the statistical distribution characteristics of the measurement noise, so as to separate the static landmarks and dynamic feature point clouds in the space, and at the same time determine the rigid body to which the dynamic point cloud belongs.

[0070] Since the dynamic feature points in the candidate landmark feature point cloud corresponding to the radar scan map will affect the point cloud registration effect, it is necessary to detect whether the point cloud is dynamic. At the same time, in order to effectively utilize the dynamic feature point cloud, the present invention also needs to determine whether the candidate feature points come from the same dynamic object. Since the mechanical millimeter-wave radar lacks Doppler dimension information, it is impossible to directly realize dynamic point cloud recognition. Therefore, the present invention analyzes and deduces the anisotropic measurement uncertainty characteristics of the millimeter-wave radar spatial perception information in the Cartesian coordinate system, calculates the probability distribution of the distance error between candidate feature point pairs by using the rotational invariance of the same rigid body geometric transformation, and uses its statistical feature quantity as the likelihood that the candidate feature point cloud comes from the same rigid body transformation.

[0071] Specifically, the point cloud measurement error covariance in the radar scan map can be modeled as a joint distribution of angle error and distance error that follows a normal distribution:

[0072]

[0073] where \(n = [\cos\alpha\) j \(\sin\alpha\) j \) T is the direction vector, \(\sigma_{\alpha}^{2}\) and \(\sigma_{r}^{2}\) are the variances of the angle error and distance error distributions respectively, \(\hat{x}\) is defined as the nominal state quantity corresponding to the observed quantity \(x\), and its magnitude is the difference between the observed quantity and the random error \(w\). For the sake of convenience in description, \(\sigma_{\alpha}^{2}\) and \(\sigma_{r}^{2}\) are abbreviated as \(\sigma_{\alpha}\) and \(\sigma_{r}\) respectively. According to the definition, the nominal state \(\hat{x}\) strictly satisfies the static feature registration constraint where \(R_{k}^{k + 1}\) and \(t_{k}^{k + 1}\) represent the rotation and translation from the \((k + 1)\)-th frame radar reference system to the \(k\)-th frame radar reference system respectively. Therefore, after motion compensation, the coordinates \(\hat{x}_{i}^{k}\) and \(\hat{x}_{i}^{k + 1}\) of the \(i\)-th static feature observation point in the \(k\)-th and \((k + 1)\)-th frames correspondingly satisfy:

[0074]

[0075] where \(w_{i}^{k}\) is the random error corresponding to the static feature observation point \(\hat{x}_{i}^{k}\).

[0076] While the coordinates \(x_{i}^{k}\) and \(x_{i}^{k + 1}\) of the \(i\)-th feature point that reflects from the same dynamic object in the \(k\)-th and \((k + 1)\)-th frames satisfy the rigid body motion transformation consistency:

[0077]

[0078] Among them, are the transformation matrices of the dynamic object from time k to time (k + 1), the rotation matrix and translation component of are the dynamic feature observation points the corresponding random errors. Since the feature points returned from the same rigid body have consistent relative motion changes, the L-2 norm distance between the point clouds i and j of the feature points remains rotation invariant:

[0079]

[0080] Among them, The above equation can be approximated as:

[0081]

[0082] Among them, A′ is a binary coefficient matrix, representing at the linear combination coefficients of the error basis vectors.

[0083] From this, we can obtain the variance of the measurement noise as:

[0084]

[0085] Among them, are the variances of the angular measurement and range measurement uncertainties of the millimeter-wave radar, respectively.

[0086] Considering that the uncertainty distribution of the measurement error in the above equation still follows a normal distribution, the 90% upper quantile of the normal distribution is calculated according to the uncertainty variance and used as a threshold to identify whether the k-th frame feature point cloud i with coordinates and the k-th frame feature point cloud j with coordinates come from the same rigid body cluster likelihood. At the same time, some isolated points can be eliminated from the dynamic or static point clouds based on this. Then, the Bron-Kerbosch algorithm is used to obtain the maximum clique from such a filtered inner group; then, according to the number of vertices, the maximum clique is used as a static point, and other cliques are used as dynamic points. By extracting the second-largest clique, the dynamic feature point cloud clusters from different rigid bodies can be separated, thus realizing the decoupling of the dynamic feature space.

[0087] S300, perform rough configuration based on static landmark features to obtain coarse-grained inter-frame motion estimation, then bundle the state equations of two frames of dynamic landmarks to derive the dynamic feature motion equation, and then combine the static feature point and dynamic feature point constraints to perform fine-grained optimization on the relative change posture, and perform nonlinear optimization solution on the fine-grained optimization to obtain the vehicle motion state estimation.

[0088] Different from directly using the iterative closest point algorithm to align the static feature set and use it as the pose transformation of two key frames, the present invention theoretically analyzes the analytical relationship between the static feature transformation and the motion state, and turns the dynamic feature group into treasure to enhance the self-motion state estimation. Specifically, the dynamic group from the same vehicle is obtained through the radar dynamic point cloud spatial decoupling model, and is further used as a geometric constraint for solving the self-motion state to improve the robustness of the system. The transformation matrix of the moving vehicle can be obtained through theoretical derivation The analytical relationship between and dynamic feature transformation:

[0089]

[0090] because is the global pose state of the millimeter-wave radar in the (k+1)th frame. It cannot be directly optimized in the front-end design based on sliding window scan matching. In order to improve the robustness of the adjacent pose transformation estimation of the front-end odometer, the geometric consistency constraint of the dynamic feature points is used to optimize the motion estimation between adjacent key frames. Under the assumption of the constant speed model, the relative change matrix equation of the two key frames is jointly derived. The dynamic feature transformation between adjacent frames and the dynamic feature coupling constraint of the pose to be optimized can be derived as follows:

[0091]

[0092] in, and They represent the relative transformation matrices from the kth frame to the (k+1)th frame and from the kth frame to the (k-1)th frame of the radar coordinate system, respectively. They represent the transformation matrices of the dynamic object from time k to (k+1) and from time (k-1) to k respectively. Consider optimizing the vehicle motion state of the (k-1), k, and (k+1) frames simultaneously, and combine the registration constraints of static feature point pairs and the coupling constraints of dynamic feature point pairs to perform fine-grained optimization of the vehicle posture:

[0093]

[0094] Among them, the first two are static registration constraints, and the third is the dynamic feature coupling constraint. represents all static feature points in the kth and (k+1)th frames, represents the optimized millimeter-wave radar spatial transformation matrix, Denote all dynamic feature points in the k-th and (k + 1)-th frames, ∑ p and ∑ D respectively represent the error uncertainties corresponding to each feature point. Denote Multiply the homogeneous coordinates of by a matrix. Log is the exponential map that maps the SO(2) orthogonal group to its corresponding Lie algebra space. β is used to balance the static registration constraint and the dynamic feature coupling constraint. By using the Levenberg-Marquardt nonlinear optimization algorithm to solve the above nonlinear optimization problem, the optimal spatial transformation matrix is calculated, thus realizing the inter-frame motion estimation with optimized dynamic features. The motion estimation enhanced by dynamic transformation not only eliminates the bias brought by dynamic feature points to the self-motion estimation, but also fully explores the coupling relationship between the dynamic feature transformation and the pose to be optimized, improving the robustness and accuracy of the autonomous positioning system in high-dynamic and low-texture driving scenarios.

[0095] The millimeter-wave radar all-weather positioning method for high-dynamic driving scenarios provided by the present invention obtains multiple frames of millimeter-wave radar historical scan maps and the current scan map, and sequentially performs inter-frame feature extraction, inter-frame feature matching, and motion distortion compensation; models the statistical characteristics of the anisotropic distribution of spatial perception measurement information according to the sensing characteristics of the millimeter-wave radar, and then calculates the probability distribution of the distance error between feature point clouds according to the rotational invariance of geometric transformation. The likelihood that the feature point cloud comes from the same rigid body transformation is measured by the statistical distribution characteristics of the measurement noise, so as to separate the static landmarks and dynamic feature point clouds in space, and at the same time determine the rigid body to which the dynamic point cloud belongs; obtains a coarse-grained inter-frame motion estimation according to the static landmark features, then bundles the state equations of two frames of dynamic landmarks to deduce the dynamic feature motion equation, and further combines the static feature points and dynamic feature point constraints to perform fine-grained optimization on the relative change pose, and performs nonlinear optimization solution on the fine-grained optimization to obtain the vehicle motion state estimation; thus, in high-dynamic and low-texture autonomous driving scenarios, the real-time and accurate vehicle motion state estimation can be realized by using the geometric consistency constraint of dynamic feature points. In addition, according to the anisotropic statistical distribution characteristics of the millimeter-wave radar spatial perception information, a radar dynamic point cloud space decoupling model is designed to separate the static landmarks and dynamic landmark point clouds in space, and determine the rigid body to which the dynamic point cloud belongs, eliminating the bias brought by dynamic feature points to the self-motion estimation. Moreover, a vehicle motion estimation scheme enhanced by dynamic transformation is proposed, and the dynamic feature motion equation is theoretically deduced. Then, the relative change pose is finely optimized by combining the static feature points and dynamic feature point constraints. Compared with the existing vehicle autonomous positioning schemes, the millimeter-wave radar autonomous positioning scheme based on dynamic feature transformation enhancement proposed by the present invention can achieve accurate, real-time, all-weather and adaptive vehicle motion state estimation.

[0096] Example 2

[0097] This embodiment provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the millimeter-wave radar all-weather positioning method in the high-dynamic driving scenario described above are implemented.

[0098] Among them, the storage medium can be a magnetic disk, an optical disc, a read-only memory (ROM), a random access memory (RAM), a flash memory, a hard disk drive (abbreviation: HDD), or a solid-state drive (SSD), etc.; the storage medium can also include a combination of the above types of memories.

[0099] Example 3

[0100] This embodiment provides a computer device, which includes: a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, the steps of the millimeter-wave radar all-weather positioning method in the high-dynamic driving scenario described above are implemented.

[0101] As Figure 4 shown, the computer device may include: at least one processor 71, such as a CPU (Central Processing Unit), at least one communication interface 73, a memory 74, and at least one communication bus 72. Among them, the communication bus 72 is used to implement connection communication between these components. Among them, the communication interface 73 may include a display screen and a keyboard. Optionally, the communication interface 73 may further include a standard wired interface and a wireless interface. The memory 74 may be a high-speed RAM memory (Random Access Memory, volatile random access memory), or a non-volatile memory, such as at least one disk memory. Optionally, the memory 74 may further be at least one storage device located far from the aforementioned processor 71. Among them, an application program is stored in the memory 74, and the processor 71 calls the program code stored in the memory 74 to execute any of the above method steps.

[0102] Among them, the communication bus 72 can be a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The communication bus 72 can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience of representation, Figure 4 only a thick line is used to represent it in Figure 4 , but it does not mean that there is only one bus or one type of bus.

[0103] Among them, the memory 74 can include volatile memory, such as random-access memory (RAM); the memory can also include non-volatile memory, such as flash memory, hard disk drive (HDD), or solid-state drive (SSD); the memory 74 can also include a combination of the above types of memory.

[0104] Among them, the processor 71 can be a Central Processing Unit (CPU), a Network Processor (NP), or a combination of a CPU and an NP.

[0105] Among them, the processor 71 can further include a hardware chip. The above hardware chip can be an Application-Specific Integrated Circuit (ASIC), a Programmable Logic Device (PLD), or a combination thereof. The above PLD can be a Complex Programmable Logic Device (CPLD), a Field-Programmable Gate Array (FPGA), a Generic Array Logic (GAL), or any combination thereof.

[0106] Optionally, the memory 74 is also used to store program instructions. The processor 71 can call the program instructions to implement the all-weather positioning method of the millimeter-wave radar in the high-dynamic driving scenario of the present invention.

[0107] Those skilled in the art should understand that the present invention can be implemented in many other specific forms without departing from the spirit and scope of the present invention. Based on the embodiments of the present invention, any changes and modifications made by those of ordinary skill in the art of the present invention according to the above disclosure fall within the protection scope of the claims.

Claims

1. A millimeter-wave radar all-weather positioning method for high-dynamic driving scenarios, characterized in that, The method comprises the following steps: S100, obtaining a plurality of frames of millimeter wave radar historical scan images and a current scan image, and sequentially performing inter-frame feature extraction, inter-frame feature matching, and motion distortion compensation; S200, deriving statistical characteristics of anisotropic distribution of spatial perception measurement information based on modeling of millimeter wave radar perception characteristics, and then calculating probability distribution of distance errors between feature point clouds based on rotational invariance of geometric transformation, and measuring the likelihood that feature point clouds come from the same rigid body transformation by measuring statistical distribution characteristics of noise, thereby separating static landmarks and dynamic feature point clouds in space, and determining that dynamic point clouds belong to a rigid body; S300, perform rough configuration based on static landmark features to obtain coarse-grained inter-frame motion estimation, then bundle the state equations of two frames of dynamic landmarks to derive the dynamic feature motion equation, and then combine the static feature point and dynamic feature point constraints to perform fine-grained optimization on the relative change posture, and perform nonlinear optimization solution on the fine-grained optimization to obtain the vehicle motion state estimation.

2. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 1, characterized in that, In step S100, multiple frames of millimeter-wave radar historical scan images and current scan images are used as input, data cleaning is performed along each azimuth line beam of each scan image, and millimeter-wave radar point clouds corresponding to multiple pixels with the strongest reflection energy are retained and used as candidate landmark feature point clouds to achieve inter-frame feature extraction.

3. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 1, wherein, In the step S100, the ORB descriptor of the pixel corresponding to each feature point cloud is calculated, and the data of the candidate landmark feature point clouds between adjacent frames are associated by brute force matching of the descriptors, so as to obtain a feature matching pair.

4. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 1, wherein, In the step S100, the motion distortion of the radar scan map is compensated as follows: Let the point observed by the vehicle in the millimeter-wave radar coordinate system at the k-th frame be denoted as which corresponds to the i-th range cell φ i and the azimuth angle α corresponding to the j-th angle cell in the scan map j , then: Among them, η, r i , N a and a j respectively represent the range resolution of the radar scan map, the mileage unit corresponding to the observation point, the total number of angular units, and the angular unit corresponding to the observation point; Under the premise of constant speed rotation scanning of the millimeter wave radar, each point of motion distortion is compensated: Among them, is the coordinate representation of the compensated feature point cloud in the radar coordinate system of the k-th frame, v k-1 is the angular velocity and linear velocity of the millimeter-wave radar in the (k - 1)-th frame, δt represents the time interval between the k-th frame and the (k - 1)-th frame, and the exponential function Exp maps the rotation vector to the rotation matrix in the SO(2) Lie group space: [v] × is the skew-symmetric matrix corresponding to the vector v.

5. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 4, characterized in that, In step S200, the point cloud measurement error covariance in the radar scan image can be modeled as a joint distribution of angle error and distance error that follows a normal distribution: where \(n = [\cos\alpha j \sin\alpha j \) T is the direction vector, are the variances of the angle and distance error distributions respectively, is defined as the observed quantity corresponding nominal state quantity, with a magnitude equal to the difference between the observed quantity and the random error ; and are abbreviated as and The nominal state strictly satisfies the static feature registration constraint where and represent the rotation and translation from the \((k + 1)\)-th frame radar reference system to the \(k\)-th frame radar reference system respectively; therefore, the coordinates and of the \(i\)-th static feature observation point after motion compensation in the \(k\)-th and \((k + 1)\)-th frames correspondingly satisfy: Among them, is the static feature observation point corresponding random error; Reflect the coordinates of the i-th feature point of the same dynamic object in the k-th and (k + 1)-th frames and Meet the consistency of rigid body motion transformation: Among them, are respectively the transformation matrix of the dynamic object from time k to time (k + 1), the rotation matrix and the translation component of is the dynamic feature observation point the corresponding random error; Since the feature points returned from the same rigid body have consistent relative motion changes, the L-2 norm distance between feature point clouds i and j remains rotationally invariant: Among them, The above formula can be approximated as: where A′ is a binary coefficient matrix, representing in the linear combination coefficients of the error basis vectors; Thus, the variance of the measurement noise is: Among them, are respectively the uncertainty variances of angle measurement and distance measurement of the millimeter-wave radar.

6. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 5, wherein Calculate the 90% upper quantile of the normal distribution based on the uncertainty variance, and use it as a threshold to identify whether the k-th frame feature point cloud i with coordinates and the k-th frame feature point cloud j with coordinates come from the same rigid body cluster likelihood. At the same time, some isolated points can be eliminated from the dynamic or static point cloud based on this; then, use the Bron-Kerbosch algorithm to obtain the maximum clique from such a filtered in-group; Then, the largest cluster is used as static point according to the number of vertices, while other clusters are used as dynamic points. By extracting the second largest cluster, the dynamic feature point cloud clusters from different rigid bodies can be separated, thereby achieving decoupling of the dynamic feature space.

7. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 6, characterized in that, In the step S300, a dynamic cluster from the same vehicle is obtained through the radar dynamic point cloud space decoupling model, and further used as a geometric constraint for calculating the self-motion state. Through theoretical derivation, the transformation matrix of the moving vehicle can be obtained and the following analytical relationship satisfied between the dynamic feature transformation: Among them, is the global pose state quantity of the millimeter-wave radar in the (k + 1)-th frame; The geometric consistency constraint of dynamic feature points is used to optimize the motion estimation between adjacent key frames. Under the assumption of a constant velocity model, the relative change matrix equation of two key frames is combined to derive the dynamic feature transformation between adjacent frames and the dynamic feature coupling constraint of the pose to be optimized: Among them, and respectively represent the relative transformation matrices from the k-th frame to the (k + 1)-th frame and from the k-th frame to the (k - 1)-th frame in the radar coordinate system, respectively represent the transformation matrices of the dynamic object from time k to time (k + 1) and from time (k - 1) to time k; Consider optimizing the vehicle motion state of the (k-1), k, and (k+1) frames simultaneously, and combine the registration constraints of static feature point pairs and the coupling constraints of dynamic feature point pairs to perform fine-grained optimization of the vehicle pose: Among them, the first two items are static registration constraints, and the third item is dynamic feature coupling constraint. denotes all static feature points in the k-th and (k + 1)-th frames, denotes the optimized spatial transformation matrix of the millimeter-wave radar, denotes all dynamic feature points in the k-th and (k + 1)-th frames, ∑ p and ∑ D respectively denote the error uncertainties corresponding to each feature point, denotes and perform matrix multiplication with the homogeneous coordinates of, Log is the exponential mapping, which maps the SO(2) orthogonal group to its corresponding Lie algebra space, and β is used to balance the static registration constraint and the dynamic feature coupling constraint; By using a non-linear optimization algorithm to solve the above fine-grained optimization, the optimal spatial transformation matrix is calculated. Thus, the inter-frame motion estimation of dynamic feature optimization is achieved.

8. The all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to claim 7, wherein, The nonlinear optimization algorithm is the Levenberg-Marquardt nonlinear optimization algorithm.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, the steps of the high-dynamic driving scenario millimeter-wave radar all-weather positioning method as described in any one of claims 1-8 are implemented.

10. A computer device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the all-weather positioning method for millimeter-wave radar in high-dynamic driving scenarios according to any one of claims 1-8.

Images