Polar coordinate voxel division-based 4D millimeter wave radar point cloud registration method

Abstract

The invention discloses a polar coordinate voxel division-based 4D millimeter wave radar point cloud registration method, which comprises the following steps of: 1, calculating inter-frame point cloud histogram correlation by using fast Fourier transform based on a pose initial value estimation algorithm of statistical characteristics, and realizing efficient coarse pose estimation; 2, the radar target point cloud is converted into a polar coordinate space, and voxel division is carried out; and recording the mean value and the point cloud coordinates of each target point cloud voxel network. 3, calculating the covariance of each voxel grid by using a 33 neighborhood model, and copying the covariance to a point cloud in the grid; 4, matching the source point cloud with the target voxel, calculating an index of the source point cloud in the voxel grid, and performing neighborhood search to establish a matching relationship; and 5, calculating a loss function for each matching pair, and searching an optimal registration pose by using LM iterative optimization. The method is suitable for robust rapid registration under the sparse and large noise characteristics of the 4D millimeter wave radar, and can be used as the front end of a positioning algorithm, thereby carrying out rapid pose estimation.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous navigation and positioning of unmanned intelligent systems based on radar sensing information, and particularly relates to a 4D millimeter-wave radar point cloud registration method based on polar coordinate voxel division. Background Art

[0002] Point cloud registration is very important for unmanned systems to achieve intelligent navigation, positioning and mapping functions. Millimeter-wave radar has powerful all-weather measurement capabilities and has significant advantages compared with traditional lidar and vision sensing systems. With the progress of radio frequency technology and chip development, 4D radar (x, y, z, Doppler velocity) has been developed and has attracted wide attention in the field of autonomous driving. However, existing 2D / 3D radar registration methods are based on 2D information or projection-based images and cannot be directly migrated to 4D radar systems. In addition, due to the inherent sparsity and high-noise characteristics of 4D radar point clouds, registration on them faces unique challenges.

[0003] There are mainly two mainstream methods in the field of three-dimensional point cloud registration: Generalized Iterative Closest Point (GICP) and Normal Distribution Transform (NDT). GICP integrates point-to-point, point-to-plane and plane-to-plane matching by calculating the local covariance matrix and optimizes the transformation through MLE. The NDT algorithm divides the measurement space into voxel grids and calculates their probability density functions to achieve registration through likelihood maximization. However, GICP requires neighborhood search and covariance calculation for all points, resulting in a high computational complexity. At the same time, NDT is sensitive to voxel resolution and cannot calculate the probability density function when the number of points in a voxel is less than 3. To address these limitations, Voxelized Generalized Iterative Closest Point (VGICP) has been developed, which eliminates the need for neighborhood search through voxel matching and achieves robust voxel distribution estimation by aggregating the individual point distributions within each voxel.

[0004] However, directly applying the VGICP algorithm to 4D radar point cloud registration has some challenges: 1) VGICP is very sensitive to large pose changes between scans, causing the optimization process to fall into a local optimum. 2) The voxel division in Cartesian coordinates used by VGICP is not robust enough for radar. Specifically, since the noise of radar is higher than that of lidar, repeated measurements of the same target may fall into different voxels at a farther distance, resulting in matching failures. 3) Since radar point clouds are sparser than lidar, the covariance calculated using the kd-tree for nearest neighbor in VGICP cannot reflect the local geometric features of the target. Therefore, how to design a point cloud registration algorithm that is robust to 4D radar pose transformation, efficiently matches under large noise characteristics, and reflects the local features of point clouds under sparse conditions is an urgent problem to be solved in the current field of 4D radar point cloud registration.

[0005] The differences compared with the prior art are as follows:

[0006] The differences compared with the prior art are as follows:

[0007] Technical comparison with the patent CN118279356A, "An Iterative Registration Method for Millimeter-Wave Radar Point Cloud Data Based on Deep Learning"

[0008] The millimeter-wave radar point cloud registration proposed in the patent CN118279356A is based on deep learning. Using the spatial coordinates, polar coordinates, Doppler velocity, and neighborhood relationship of the point cloud, the point cloud features and registration parameters are trained through a neural network to determine the transformation parameters, thereby achieving point cloud registration.

[0009] Compared with this patent, the present patent uses a mechanism model to perform interpretable modeling on point cloud registration and innovatively introduces a polar coordinate voxel division strategy. This method first uses the fast Fourier transform to calculate the histogram correlation for rough registration, then performs voxel division in the polar coordinate space, establishes a matching relationship based on the voxels, and finally obtains the optimal registration pose through LM iterative optimization. The advantages of this method are high computational efficiency, simple implementation, and special consideration of the sparsity and noise characteristics of millimeter-wave radar point clouds.

[0010] Technical comparison with the patent CN115220041B, "A Millimeter-Wave Radar Scale Positioning Method and System with Doppler Compensation"

[0011] The patent CN115220041B estimates the uncertainty distribution of the millimeter-wave radar point cloud in the Cartesian coordinate system through the measurement errors of the distance, velocity, and angle of the radar target in the polar coordinate system, performs point cloud matching based on the RSD descriptor, and finally uses the maximum likelihood estimation to obtain the inter-frame pose transformation relationship.

[0012] Compared with this patent, although the present patent also uses the polar coordinate system to estimate the uncertainty of the radar point cloud, the uncertainty estimation of the radar point cloud in the present patent is based on the statistical characteristics of the polar coordinate neighborhood, while CN115220041B is based on the error measurement model. Further, the point cloud matching in the present patent directly uses the nearest neighbor matching in the polar coordinate system and uses LM iterative optimization to achieve the registration of the point cloud. 3 The uncertainty of the radar point cloud in the present patent is estimated based on the statistical characteristics of the polar coordinate neighborhood, while CN115220041B is based on the error measurement model. Further, the point cloud matching in the present patent directly uses the nearest neighbor matching in the polar coordinate system and uses LM iterative optimization to achieve the registration of the point cloud.

[0013] Technical comparison with the patent CN117315268A, "A SLAM Method and System Based on Millimeter-Wave Radar"

[0014] The patent CN117315268A converts the radar information into a polar coordinate image, performs feature extraction, and finally uses the RANSAC algorithm to obtain the adjacent frame pose. Compared with this patent, the present patent converts the radar point cloud into the polar coordinate space, performs voxel division, and uses 3 3The neighborhood model calculates the covariance of each voxel grid, conducts neighborhood search in the voxel grid to establish a matching relationship, and uses the LM iteration to optimize the search for the optimal registration pose. Summary of the Invention

[0015] Aiming at the problems existing in the existing 4D radar point cloud registration technology, a 4D millimeter-wave radar point cloud registration method based on polar coordinate voxel division is provided, which integrates the initial pose estimation based on point cloud statistics, voxel division in polar coordinates, and 33-neighborhood voxel local feature estimation, realizing fast and robust registration of radar point clouds.

[0016] To achieve the above purpose, the 4D millimeter-wave radar point cloud registration method based on polar coordinate voxel division of the present invention includes the following method steps:

[0017] Step 1: Initial pose estimation:

[0018] Based on the pose initial value estimation algorithm of statistical features, the fast Fourier transform is used to calculate the histogram correlation of the point clouds between frames, realizing efficient rough pose estimation;

[0019] Step 2: Voxel division:

[0020] The target point cloud is converted to polar coordinates, and a voxel space is constructed based on the set resolution. Record the point cloud coordinates in each voxel grid and calculate their mean values;

[0021] Step 3: 3 3 Covariance estimation model:

[0022] For each voxel grid, calculate the number of point clouds in its 3 3 neighborhood. If the number of neighborhood point clouds is less than the set threshold, estimate the voxel covariance through the radar measurement error model, otherwise estimate the covariance through the neighborhood point clouds;

[0023] Step 4: Voxel matching:

[0024] The source point cloud is also converted to polar coordinates and the voxel index is calculated, and neighborhood search is performed in the target voxel space to achieve voxel matching of the source point cloud;

[0025] Step 5: LM iteration optimization:

[0026] For each matching pair, calculate its weighted Euclidean distance loss function, and obtain the optimal inter-frame registration pose through LM iteration optimization.

[0027] As a further improvement of the present invention, the step 1 initial pose estimation includes the following steps:

[0028] (1-1) The construction of the point cloud histogram is defined as follows;

[0029] Project all the point clouds onto the horizontal plane, divide the horizontal space into N intervals, and count the number of point clouds in each interval.

[0030] Two real number sequences y A [n], y B [n], where n = 0, 1, ..., N - 1.

[0031] (1 - 2) The calculation of the correlation of the histogram sequence is positioned as follows;

[0032] Perform a discrete Fourier transform DFT on the input sequence:

[0033]

[0034] Calculate the correlation in the frequency domain, that is, perform a dot product of the conjugate:

[0035]

[0036] Where represents the complex conjugate of Y B [x];

[0037] Perform an inverse discrete Fourier transform IDFT on the correlation result to obtain the correlation function in the time domain:

[0038]

[0039] Find the displacement corresponding to the maximum correlation:

[0040]

[0041] (1 - 3) Convert the histogram correlation to a pose transformation:

[0042] Convert the maximum correlation displacement m obtained by the Li - Sang Fourier transform to an angle:

[0043]

[0044] Thus, obtain the initial value of the pose estimate:

[0045]

[0046] As a further improvement of the present invention, the voxel division in step 2 is defined as follows:

[0047] Consider the source point cloud and the target point cloud The pose transformation between them is

[0048] (2 - 1) The definition of the point cloud polar coordinate transformation is as follows;

[0049] For the target point cloud Transform it into the polar coordinate space

[0050]

[0051] (2-2) The definition of point cloud voxel grid division is as follows;

[0052] Calculate the voxel index of each point cloud:

[0053]

[0054] Among them, is the voxel index, is the voxel grid resolution;

[0055] (2-3) The voxel mean value is calculated as follows;

[0056] Group the point clouds with the same index into the same voxel:

[0057]

[0058] For each non-empty voxel O k , calculate the mean value of the internal point cloud:

[0059]

[0060] Finally, obtain the target point cloud voxel space:

[0061] V = {v k = (μ k , C k , O k ), |O k | > 0, k = 0, 1,..., N v},

[0062] Among them, C k is the voxel covariance, which is obtained by calculation in the next section.

[0063] As a further improvement of the present invention, step 33 3 Neighborhood covariance estimation model, the steps are as follows;

[0064] (3-1) 3 3 Neighborhood point cloud search:

[0065] Set the 3 3 neighborhood search interval, and count the number of point clouds in each voxel grid neighborhood interval:

[0066] cnt k = |N k |, N k = 3 3 -neighborsset(vk )

[0067] (3 - 2) Covariance estimation:

[0068] If 3 3 The number of point clouds within the neighborhood search interval is less than the set value, then covariance estimation is performed through the radar measurement error model. Specifically, by referring to the radar manual, the radar measurement uncertainty is:

[0069]

[0070] Where σ r , σ θ and σ φ are the uncertainties of the radar's radial direction, horizontal angle, and pitch angle respectively.

[0071] Then transform it to the radar coordinate system D = RS. D is the covariance in the radar coordinate system, and R is the rotation matrix from the measurement system to the radar:

[0072]

[0073] Finally, the covariance is expressed as C k = DD T ;

[0074] If 3 3 The number of point clouds within the neighborhood search interval is greater than the set value, then the voxel covariance is calculated through the neighborhood points:

[0075]

[0076] Copy the covariance to all point clouds within the voxel:

[0077]

[0078] As a further improvement of the present invention, the step 4 voxel matching is as follows;

[0079] (4 - 1) For each source point cloud, transform it to the polar coordinate system through (2 - 1) and calculate its voxel index and find its neighborhood voxels:

[0080]

[0081] (4 - 2) Thus, establish the matching relationship

[0082] As a further improvement of the present invention, the step 5 LM iterative optimization is as follows;

[0083] (5 - 1) Loss function construction:

[0084] For each pair of matching pairs Define the loss function:

[0085]

[0086] (5-2) Pose solution:

[0087] d i obeys the Gaussian probability distribution:

[0088]

[0089] The pose T is obtained by minimizing the log-likelihood:

[0090]

[0091] Through the voxel parameters, the above formula can be simplified to:

[0092]

[0093] where is the number of inlier point clouds within the voxel.

[0094] Compared with the prior art, the beneficial effects of the present invention are:

[0095] (1) Design a pose initial value estimation algorithm based on statistical features, and use the fast Fourier transform technology to calculate the cross-correlation of the histograms of adjacent frame point clouds, realizing efficient initial pose estimation.

[0096] (2) Aiming at the problem of voxel matching failure caused by radar noise, a polar coordinate voxel division strategy is adopted. Based on the basic imaging principle that the radar measurement uncertainty increases with the distance, an adaptive voxel scale division is realized in the polar coordinate system, effectively fitting the inherent noise distribution characteristics of the radar, thereby improving the robustness and accuracy of the point cloud registration process.

[0097] (3) Establish a point cloud covariance calculation model based on 3×3×3 cubic voxels. Through the statistical analysis of the local space structure, this model effectively solves the problem of insufficient local feature expression caused by the sparsity of radar point clouds. Brief description of the drawings

[0098] Figure 1 is the flowchart of the disclosed method of the present invention;

[0099] Figure 2 is the schematic diagram of the radar point cloud histogram;

[0100] Figure 3 is the schematic diagram of the voxel neighborhood search space. Detailed implementation manners

[0101] The present invention will be further illustrated below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and not to limit the scope of the present invention. It should be noted that the terms "front", "rear", "left", "right", "upper" and "lower" used in the following description refer to the directions in the accompanying drawings, and the terms "inner" and "outer" refer to the directions towards or away from the geometric center of a specific component, respectively.

[0102] As Figure 1 shown, the present invention discloses a 4D millimeter-wave radar point cloud registration algorithm based on polar coordinate voxel division, including the following steps:

[0103] A 4D millimeter-wave radar point cloud registration algorithm based on polar coordinate voxel division, including the following steps:

[0104] Step 1: Initial pose estimation. Based on the statistical feature-based initial pose estimation algorithm, the fast Fourier transform is used to calculate the histogram correlation between frames of point clouds to achieve efficient coarse pose estimation:

[0105] The initial pose estimation includes the following steps

[0106] (1-1) The construction of the point cloud histogram is defined as follows;

[0107] As Figure 2 shown, all point clouds are projected onto the horizontal plane, and the horizontal space is divided into N intervals. The number of point clouds in each interval is counted to obtain two real number sequences y A [n], y B [n], n = 0, 1,..., N-1.

[0108] (1-2) The calculation of the histogram sequence correlation is located as follows;

[0109] Perform a discrete Fourier transform (DFT) on the input sequence:

[0110]

[0111] Calculate the correlation (dot product conjugate) in the frequency domain:

[0112]

[0113] where represents the complex conjugate of Y B [x].

[0114] Perform an inverse discrete Fourier transform (IDFT) on the correlation result to obtain the correlation function in the time domain:

[0115]

[0116] Find the displacement corresponding to the maximum correlation:

[0117]

[0118] (1 - 3) Convert the histogram correlation into a pose transformation:

[0119] Convert the maximum correlation displacement m obtained by the Li - Sang Fourier transform into an angle:

[0120]

[0121] Thus, obtain the initial value of pose estimation:

[0122]

[0123] Step 2: Voxel division: Convert the target point cloud into polar coordinates and construct a voxel space based on the set resolution. Record the point cloud coordinates within each voxel grid and calculate their mean values;

[0124] Voxel division is defined as follows:

[0125] Consider the source point cloud and the target point cloud The pose transformation between them is

[0126] (2 - 1) The definition of point cloud polar coordinate transformation is as follows;

[0127] For the target point cloud Convert it into the polar coordinate space

[0128]

[0129] (2 - 2) The definition of point cloud voxel grid division is as follows;

[0130] Calculate the voxel index of each point cloud:

[0131]

[0132] Among them, is the voxel index, is the voxel grid resolution.

[0133] (2 - 3) The voxel mean value is calculated as follows;

[0134] Group the point clouds with the same index into the same voxel:

[0135]

[0136] For each non - empty voxel O k , calculate the mean value of the internal point cloud:

[0137]

[0138] Finally, the target point cloud voxel space is obtained:

[0139] V = {v k = (μ k , C k , O k ), |O k | > 0, k = 0, 1,..., N v}.

[0140] Among them, C k is the voxel covariance, which is calculated in the next section.

[0141] Step 3: 3 Covariance estimation model: For each voxel grid, calculate the number of point clouds in its 3 neighborhood. If the number of point clouds in the neighborhood is less than the set threshold, estimate the voxel covariance through the radar measurement error model; otherwise, estimate the covariance through the neighborhood point clouds.

[0142] 3 Neighborhood covariance estimation model, the steps are as follows;

[0143] (3-1) 3 Neighborhood point cloud search:

[0144] As Figure 3 (d) shows, set the 3 neighborhood search interval, and count the number of point clouds in the neighborhood interval of each voxel grid:

[0145] cnt k = |N k |, N k = 3 - neighbors set(v k ).

[0146] (3-2) Covariance estimation:

[0147] If the number of point clouds in the 3 neighborhood search interval is less than the set value, estimate the covariance through the radar measurement error model. Specifically, by referring to the radar manual, the radar measurement uncertainty is:

[0148]

[0149] Among them, σ r , σ θ and σ φ are the radar radial, horizontal angle, and pitch angle uncertainties respectively.

[0150] ​Then it is transformed into the radar coordinate system as D = RS. D is the covariance in the radar coordinate system, and R is the rotation matrix from the measurement system to the radar:

[0151]

[0152] Finally, the covariance is expressed as C k = DD T .

[0153] If the number of point clouds within the 3 3 neighborhood search interval is greater than the set value, the voxel covariance is calculated through the neighborhood points:

[0154]

[0155] Copy the covariance to all the point clouds within the voxel:

[0156]

[0157] Step 4: Voxel matching: Transform the source point cloud into the polar coordinate system as well and calculate the voxel index, and conduct a neighborhood search in the target voxel space to achieve voxel matching of the source point cloud;

[0158] Voxel matching is as follows.

[0159] (4-1) For each source point cloud, transform it into the polar coordinate system through (2-1) and calculate its voxel index As Figure 3 (c) shows, find its neighborhood voxels:

[0160]

[0161] (4-2) Thus, establish the matching relationship

[0162] Step 5: LM iterative optimization: For each matching pair, calculate its weighted Euclidean distance loss function, and obtain the optimal inter-frame registration pose through LM iterative optimization.

[0163] The LM iterative optimization can be described as:

[0164] (5-1) Loss function construction:

[0165] For each pair of matching pairs Define the loss function:

[0166]

[0167] (5-2) Pose solution:

[0168] d i obeys the Gaussian probability distribution:

[0169]

[0170] The pose T is obtained by minimizing the log-likelihood:

[0171]

[0172]

[0173] With the voxel parameters, the above formula can be simplified to:

[0174]

[0175] where is the number of point clouds within the voxel.

[0176] It should be noted that the above content only illustrates the technical idea of the present invention and cannot be used to limit the protection scope of the present invention. For those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can still be made, and these improvements and refinements all fall within the protection scope of the claims of the present invention.

Claims

1. A 4D millimeter wave radar point cloud registration method based on polar coordinate voxel division, characterized by: The method comprises the following steps: Step 1: Initial pose estimation: The pose initial value estimation algorithm based on statistical features uses fast Fourier transform to calculate the correlation of point cloud histograms between frames to achieve efficient coarse pose estimation; Step 2: Voxel division: Convert the target point cloud to polar coordinates and construct the voxel space based on the set resolution. Record the point cloud coordinates in each voxel grid and calculate their mean; Step 3:3 3 Covariance estimation model: For each voxel grid, calculate its 3 3 The number of point clouds in the neighborhood. If the number of neighborhood point clouds is less than the set threshold, the voxel covariance is estimated through the radar measurement error model, otherwise the covariance is estimated through the neighborhood point clouds; Step 4: Voxel matching: The source point cloud is also transformed into polar coordinates and the voxel index is calculated, and a neighborhood search is performed in the target voxel space to achieve voxel matching of the source point cloud; Step 5: LM iterative optimization: For each matching pair, its weighted Euclidean distance loss function is calculated, and the optimal inter-frame registration pose is obtained through LM iterative optimization.

2. The 4D millimeter wave radar point cloud registration method based on polar coordinate voxel division according to claim 1, characterized in that: The step 1, estimating the initial value of the pose, comprises the following steps: (1-1) The point cloud histogram is constructed and defined as follows; Project all point clouds onto the horizontal plane, divide the horizontal space into N intervals, and count the number of point clouds in each interval. Get two real number sequences y of equal length A [n],y B [n],n=0,1,...,N-1. (1-2) The calculation and positioning of the histogram sequence correlation are as follows; Perform discrete Fourier transform DFT on the input sequence: Computing the correlation in the frequency domain is the dot product conjugate: in Represents Y B The complex conjugate of [x]; Perform inverse discrete Fourier transform IDFT on the correlation result to obtain the correlation function in the time domain: Find the displacement corresponding to the maximum correlation: (1-3) Convert histogram correlation to pose transformation: Convert the maximum correlation displacement m calculated by Lissing Fourier transform into an angle: Thus, the initial value of pose estimation is obtained:

3. The 4D millimeter wave radar point cloud registration method based on polar coordinate voxel division according to claim 1, characterized in that: The step 2 voxel division is defined as follows: Consider the source point cloud and target point cloud The pose transformation between (2-1) The point cloud polar coordinate transformation is defined as follows; For the target point cloud Convert it to polar coordinate space (2-2) The point cloud voxel grid division is defined as follows; Compute the voxel index for each point cloud: in, is the voxel index, is the voxel grid resolution; (2-3) The voxel mean is calculated as follows; Group points with the same index into the same voxel: For each non-empty voxel O k , calculate the mean of its inner point cloud: Finally, the target point cloud voxel space is obtained: V={v k =(μ k ,C k ,O k ),|O k |>0,k=0,1,...,N v }, Among them, C k is the voxel covariance, which is calculated in the next section.

4. The 4D millimeter wave radar point cloud registration method based on polar coordinate voxel division according to claim 1, characterized in that: Step 33 3 Neighborhood covariance estimation model, the steps are as follows; (3-1)3 3 Neighborhood point cloud search: Setup 3 3 Neighborhood search interval, count the number of point clouds within each voxel grid area: cnt k =|N k |,N k =3 3 -neighborsset(v k ), (3-2) Covariance estimation: If 3 3 If the number of point clouds in the neighborhood search interval is less than the set value, the covariance estimation is performed through the radar measurement error model. Specifically, by consulting the radar manual, it can be known that the radar measurement uncertainty is: Among them, σ r , σ θ and σ φ are the radar radial, horizontal angle and elevation angle uncertainties respectively. Then transform it to the radar coordinate system D = RS. D is the covariance in the radar coordinate system, and R is the rotation matrix from the measurement system to the radar: Finally, the covariance is expressed as C k =DD T ; If 3 3 If the number of point clouds in the neighborhood search interval is greater than the set value, the voxel covariance is calculated through the neighborhood point cloud: Copy the covariance to all point clouds within a voxel:

5. The 4D millimeter wave radar point cloud registration method based on polar coordinate voxel division according to claim 1, characterized in that: The step 4 voxel matching is as follows: (4-1) For each source point cloud, convert it to the polar coordinate system through (2-1) and calculate its voxel index And find its neighboring voxels: (4-2) Thus establishing a matching relationship 6. The 4D millimeter wave radar point cloud registration method based on polar coordinate voxel division according to claim 1, characterized in that: The step 5LM iterative optimization is as follows: (5-1) Loss function construction: For each matching pair Define the loss function: (5-2) Posture solution: d i Obey Gaussian probability distribution: The pose T is obtained by minimizing the log-likelihood: Through voxel parameters, the above formula can be simplified to: in, is the number of point clouds within a voxel.

Images