Improved NDT registration method for 4D millimeter wave radar point cloud

Abstract

The invention discloses an improved NDT registration method for a 4D millimeter wave radar point cloud, and the method is characterized in that the method comprises the following steps: S1, obtaining radar hardware parameters: obtaining the azimuth angle resolution, pitch angle resolution and distance resolution of a 4D millimeter wave radar; s2, calculating optimization covariance: fusing three hardware parameters of the radar to calculate the optimization covariance of each voxel grid; and S3, calculating an optimal transformation matrix: adding the optimized covariance into the covariance in the NDT registration algorithm to obtain the optimal transformation matrix. The improved NDT algorithm is provided based on the improved point cloud registration method of the NDT, the NDT registration algorithm is adjusted by using the resolution parameter of the hardware from the sensor hardware, and a more subdivided probability model is constructed for the point cloud sample, so that the registration performance is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of radar map modeling, and specifically to an improved NDT registration method for 4D millimeter-wave radar point clouds. Background Art

[0002] As a great achievement in solving the problems of autonomous mobile robots, the SLAM technology is widely applied by combining different types of sensors. Among them, the millimeter-wave radar has strong adaptability in harsh environments and has better robustness than vision sensors and lidar in extreme weather. With the rise of 4D millimeter-wave radar, it can additionally obtain the elevation angle information of the target compared with the traditional millimeter-wave radar, that is, the 4D millimeter-wave radar point cloud will contain the height information of the target, making it possible to construct a three-dimensional millimeter-wave point cloud map of the environment using these point clouds.

[0003] However, the working principle of the millimeter-wave radar is different from that of the mechanical lidar with a 360° scanning surface. The accuracy of the point clouds at different positions on the radar scanning surface will be affected by the coupling of the angle measurement accuracy and the ranging accuracy of the radar sensor itself. As the detection distance or the horizontal detection angle increases, the accuracy of the point clouds closer to the edge of the fan-shaped scan is lower. During the process of perceiving the environment, multipath reflection and false mapping make the point clouds obtained by the millimeter-wave radar carry more noise. In the SLAM, this means that as these errors accumulate continuously, the trajectory drifts, the point cloud map is distorted, and even the map building fails, and this phenomenon will be further exacerbated as the moving speed of the sensor and the data volume of the point clouds increase. Therefore, how to optimize the point cloud registration effect of the 4D millimeter-wave radar remains a technical challenge. Summary of the Invention

[0004] The purpose of the present invention is to overcome the deficiencies of the prior art and provide an improved NDT registration method for 4D millimeter-wave radar point clouds.

[0005] The present invention is realized through the following technical solutions:

[0006] An improved NDT registration method for 4D millimeter-wave radar point clouds includes the following steps:

[0007] S1. Obtain radar hardware parameters: Obtain the azimuth angle resolution θ res of the 4D millimeter-wave radar, the elevation angle resolution res and the distance resolution r;

[0008] S2. Calculate the optimized covariance: Calculate the optimized covariance of each voxel grid by fusing the three hardware parameters of the radar;

[0009] S3. Calculate the optimal transformation matrix: Add the optimized covariance to the covariance in the NDT registration algorithm to obtain the optimal transformation matrix.

[0010] Specifically included in step S2 is

[0011] S2.1. Convert the point cloud sample obtained by the radar to the spherical coordinate system. Specifically, it is represented by the distance, azimuth angle, and elevation angle information included in the point cloud as follows:

[0012]

[0013] Wherein, represents a frame of point cloud obtained by the radar, is the distance information of this point cloud, is the azimuth angle information of this point cloud, is the elevation angle information of this point cloud;

[0014] S2.2. Utilize the hardware parameters of the millimeter-wave radar for each frame of point cloud Utilize θ res , r res to construct the uncertainty existing in each dimension of the point cloud in the spherical coordinate system, specifically expressed as:

[0015]

[0016] Wherein, diff range,i is the distance uncertainty of this point cloud, diff azimuth,i is the azimuth angle uncertainty of this point cloud, diff elevation,i is the elevation angle uncertainty of this point cloud;

[0017] Transform the point cloud frame to the radar frame through the uncertainty to obtain Wherein, is the adjoint matrix of Cov radar , and Cov radar is the radar frame.

[0018] Specifically included in step S3 is

[0019] S3.2. Grid the point cloud space and calculate the mean μ voxel and covariance Cov voxel in each voxel grid as follows:

[0020]

[0021] S3.4. Calculate the normal probability density function of this grid according to the reference point cloud and the point cloud to be registered as follows:

[0022]

[0023] Among them

[0024] S3.5. Add the one obtained in step S2.2 to the one obtained in step S3.4 to get:

[0025]

[0026] S3.6. Change the normal probability density function obtained in step S3.5 into a mixed probability density function to obtain:

[0027]

[0028] S3.7. Take the negative log-likelihood of the mixed probability density function obtained in step S3.6 to get:

[0029]

[0030] Among them, the parameter d 1 , d 2 , d 3 is:

[0031] d 1 =-log(c 1 +c 2 )-d 3

[0032] d 2 =-2log((-log(c 1 exp(-0.5)+c 2 )-d 3 ) / d 1 )

[0033] d 3 =-log(c 2 )

[0034] S3.8. Sum up to get

[0035] S3.9. Set an expected value of s(ξ), and use the Gauss-Newton method to iteratively solve the minimum value of s(ξ). When the minimum value is less than the set expected value, the η n at this time, that is, R n and t n are the optimal transformation parameters.

[0036] The present invention discloses an improved NDT registration method for 4D millimeter-wave radar point clouds. Compared with the prior art:

[0037] Based on the improved point cloud registration method of NDT, the present invention proposes an improved NDT algorithm. Starting from the sensor hardware, the resolution parameters of the hardware are used to adjust the NDT registration algorithm, and a more refined probability model is constructed for the point cloud sample, thereby improving the registration performance; the present invention integrates the hardware characteristics of the radar into the algorithm model, improving the registration accuracy of the 4D millimeter-wave radar. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 It is a schematic diagram of the radar hardware parameters of the present invention.

[0039] Figure 2 It is a schematic diagram of the flow of the present invention.

[0040] Figure 3 It is a schematic diagram for comparing the registration effects of the point clouds of a road with green belts planted on both sides by the present invention.

[0041] Figure 4 It is a comparison of the absolute trajectory errors between the method of the present invention and the standard NDT algorithm Figure 1 .

[0042] Figure 5 It is a comparison of the absolute trajectory errors between the method of the present invention and the standard NDT algorithm Figure 2 . DETAILED DESCRIPTION OF THE EMBODIMENTS

[0043] The technical solution of the present invention will be further described below in conjunction with the drawings and through specific embodiments.

[0044] Among them, the drawings are only for illustrative purposes, showing only schematic diagrams, not physical diagrams, and should not be construed as limiting the present invention; in order to better illustrate the embodiments of the present invention, some components in the drawings will be omitted, enlarged or reduced, and do not represent the dimensions of actual products; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0045] In the drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components; in the description of the present invention, it should be understood that if terms such as "upper", "lower", "left", "right", "inner", "outer", etc. are used to indicate the orientation or positional relationship, it is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, so the terms describing the positional relationship in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those of ordinary skill in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0046] Refer to Figure 1 and Figure 2As shown in the figure, an improved NDT registration method for 4D millimeter-wave radar point cloud, the method comprising the following steps:

[0047] S1. Obtain radar hardware parameters: obtain the azimuth resolution θ from the user manual of the 4D millimeter-wave radar res , the pitch resolution range resolution r res ;

[0048] S2. Calculate the optimized covariance: fuse the three hardware parameters of the radar to calculate the optimized covariance of each voxel grid;

[0049] S3. Calculate the optimal transformation matrix: add the optimized covariance to the covariance in the NDT (Normal Distributions Transform) registration algorithm to obtain the optimal transformation matrix.

[0050] As a further preferred solution of this technical solution, the step S2 specifically includes:

[0051] S2.1. Convert the point cloud sample obtained by the radar to the spherical coordinate system, and represent it specifically through the distance, azimuth and pitch angle information included in the point cloud:

[0052]

[0053] wherein, represents a frame of point cloud obtained by the radar, is the distance information of the point cloud, is the azimuth information of the point cloud, is the pitch angle information of the point cloud;

[0054] S2.2. Since the millimeter-wave radar has resolution limits in terms of distance, azimuth and pitch angle, it is necessary to optimize the error caused by the radar resolution. For each frame of point cloud use θ res , r res to construct the uncertainty existing in each dimension of the point cloud in the spherical coordinate system, specifically expressed as:

[0055]

[0056] wherein, diff range,i is the distance uncertainty of the point cloud, diff azimuth,i is the azimuth uncertainty of the point cloud, diff elevation,i is the pitch angle uncertainty of the point cloud;

[0057] S2.3. Based on step S2.2, construct the current point cloud Covariance matrix It is used to measure the degree to which three dimensions deviate from their means, specifically expressed as:

[0058]

[0059] S2.4. Through the relationship of linear transformation, transform Through From the point cloud frame to the radar frame, where the transformation relationship R 0 Use The information of is expressed as:

[0060]

[0061] Among them, Cov radar Is the radar frame;

[0062] S2.5. Multiply the covariance matrix Cov radar In the radar frame by its own matrix to obtain

[0063] Among them, Is the adjoint matrix of Cov radar .

[0064] As a further preferred solution of this technical solution, the step S3 specifically includes:

[0065] S3.1. First, divide the space where the point cloud is located into multiple cubes with the same side length to establish a voxel network;

[0066] S3.2. Since the radar point cloud is discrete data and not differentiable, many mathematical tools cannot be used. To solve this problem, we hope to establish a relatively continuous representation form for the discrete data form and obtain a continuously differentiable function. Based on this, the NDT algorithm is proposed. The point cloud P used as the registration reference is called the reference point cloud Calculate the mean μ voxel And covariance Cov voxel In each voxel grid:

[0067]

[0068] Among them, n is the number of point clouds in each cube, and p i Is the position of a certain point in the voxel grid.

[0069] S3.3. The point cloud to be registered

[0070] After the point cloud space is meshed and the mean μ and covariance Σ in each grid are obtained, the normal probability density function of this grid is obtained:

[0071]

[0072] Among them, η n represents the transformation parameters between point clouds estimated by the algorithm after n iterations, that is, R n and t n ; R n represents the rotation matrix parameter in the transformation parameters, and t n represents the translation matrix parameter in the transformation parameters;

[0073] S3.5. Add the obtained in step S2.5 to the following to get:

[0074]

[0075] S3.6. To facilitate calculation and avoid the function from crashing during iterative calculation and the influence of outliers on the calculation results, change the probability density function of each voxel grid from the original normal probability density function to a mixed probability density function, that is, to get:

[0076]

[0077] Among them, r outlier is the proportion of outliers in the point cloud, c 1 , c 2 is a constant determined according to r outlier . S3.7. To simplify the calculation, the negative log-likelihood obtained in S3.5 is transformed as follows:

[0078]

[0079] Among them, the parameter d 1 , d 2 , d 3 is

[0080] d 1 =-log(c 1 +c 2 )-d 3

[0081] d 2 =-2log((-log(c 1 exp(-0.5)+c 2 )-d 3 ) / d 1 )

[0082] d 3 =-log(c 2 )

[0083] 3.8. Sum to obtain

[0084] 3.9. Set an expected value of s(ξ), and use the Gauss - Newton method to iteratively solve for the minimum value of s(ξ). When the minimum value is less than the set expected value, the η at this time n i.e., R n and t n are the optimal transformation parameters.

[0085] In the above step S3, finding the maximum value in step S3.6 is equivalent to finding the minimum value in step S73.7. For specific details, reference can be made to the content disclosed in https: / / blog.csdn.net / xinxiangwangzhi_ / article / details / 125030492.

[0086] Refer to Figure 3 as shown Figure 3 is the effect diagram of registering the point cloud of a road with green belts on both sides using the NDT registration algorithm ( Figure 3 left) and an improved NDT registration method for 4D millimeter - wave radar point cloud of the present invention ( Figure 3 right). The red ellipse shows a green belt. Under the registration of NDT, there is a ghosting phenomenon in the point cloud of the same area on the right side compared with the point cloud of the same area on the right side, while this phenomenon is weakened in the improved NDT registration method.

[0087] Refer to Figure 4 as shown Figure 4 is the evaluation and test of an improved NDT registration method for 4D millimeter - wave radar point cloud of the present invention using two scenarios of a left - turning road and a snowy road from a publicly available dataset [23, 24]. The present invention is superior to the standard NDT algorithm in the rotation matrix parameter R n , and the translation matrix parameter t n in terms of the absolute trajectory error; for the publicly available dataset, refer to the following:

[0088]

[23] Choi M, Yang S, Han S, et al. MSC - RAD4R: ROS - Based Automotive DatasetWith 4D Radar[J]. IEEE Robotics and Automation Letters, 2023, 8(11):7194 - 7201.

[0089]

[24] Zhang J, Zhuge H, Liu Y, et al. NTU4DRadLM: 4D Radar-centric Multi-Modal Dataset for Localization and Mapping[M]. arXiv, 2023.

[0090] See Figure 5 as shown Figure 5 To evaluate and test an improved NDT registration method for 4D millimeter-wave radar point clouds of the present invention using two scenarios of a left-turning road and a snowy road from the publicly available datasets [23, 24], the present invention calculates the rotation matrix parameter R n , and the translation matrix parameter t n is superior to the standard NDT algorithm in terms of relative trajectory error; see the publicly available datasets below:

[0091]

[23] Choi M, Yang S, Han S, et al. MSC-RAD4R: ROS-Based Automotive Dataset With 4D Radar[J]. IEEE Robotics and Automation Letters, 2023, 8(11): 7194 - 7201.

[0092]

[24] Zhang J, Zhuge H, Liu Y, et al. NTU4DRadLM: 4D Radar-centric Multi-Modal Dataset for Localization and Mapping[M]. arXiv, 2023.

[0093] To evaluate and test an improved NDT registration method for 4D millimeter-wave radar point clouds of the present invention using two scenarios of a left-turning road and a snowy road from the publicly available datasets [23, 24], the present invention calculates the rotation matrix parameter R n , and the translation matrix parameter t n is superior to the standard NDT algorithm in terms of relative trajectory error.

[0094] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. An improved NDT registration method for 4D millimeter wave radar point cloud, characterized in that: The steps include: S1. Get radar hardware parameters: Get the azimuth resolution θ of the 4D millimeter-wave radar res , pitch angle resolution and distance resolution r res ; S2. Calculate optimized covariance: Calculate the optimized covariance of each voxel grid by integrating the three hardware parameters of the radar; S3. Calculate the optimal transformation matrix: add the optimized covariance to the covariance in the NDT registration algorithm to obtain the optimal transformation matrix.

2. The improved NDT registration method for 4D millimeter wave radar point cloud according to claim 1, characterized in that: The step S2 specifically includes: S2.

1. Convert the point cloud samples acquired by the radar into a spherical coordinate system. The distance, azimuth and elevation information contained in the point cloud can be expressed as follows: in, Represents a frame of point cloud acquired by the radar. is the distance information of the point cloud, is the azimuth information of the point cloud, is the pitch angle information of the point cloud; S2.

2. Using the hardware parameters of the millimeter wave radar, for each frame of point cloud Using θ res , r res The uncertainty in each dimension of the point cloud in the spherical coordinate system is constructed, which is specifically expressed as: Among them, diff range,i is the distance uncertainty of the point cloud, diff azimuth,i The azimuth of the point cloud is uncertain, diff elevation,i is the pitch angle uncertainty of the point cloud; By changing the point cloud frame to the radar frame through uncertainty, we get in, For Cov radar The adjoint matrix, Cov radar is the radar frame.

3. The improved NDT registration method for 4D millimeter wave radar point cloud according to claim 2, characterized in that: The step S3 specifically includes: S3.

2. Grid the point cloud space and calculate the mean μ in each voxel grid voxel and covariance Cov voxel : S3.

4. Based on the reference point cloud And the point cloud to be registered ), calculate the normal probability density function of the grid: in S3.5, the result obtained in step S2.2 Adding step S3.4 gives: S3.

6. Convert the normal probability density function obtained in step S3.5 into a mixed probability density function to obtain: S3.

7. Take the mixed probability density function obtained in step S3.6 and perform negative log-likelihood to obtain: Among them, the parameters d1, d2, d3 are: d1=-log(c1+c2)-d3 d2=-2log((-log(c1exp(-0.5)+c2)-d3) / d1) d3=-log(c2) S3.8, Yes Summing to get S3.

9. Set an expected value of s(ξ) and use the Gauss-Newton method to iteratively solve the minimum value of s(ξ). When the minimum value is less than the expected value, η n That is R n and t n is the optimal transformation parameter.

Images