A real-time positioning and mapping method based on 4D millimeter wave radar

Abstract

The present invention discloses a real-time positioning and mapping method based on 4D millimeter wave radar, including: obtaining the angular accuracy of the 4D millimeter wave radar and the point cloud data of the target in the current frame, calculating the coordinates of each point in the point cloud data considering the angular accuracy in the Cartesian coordinate system; for each point in the point cloud data, constructing a corresponding ellipsoid based on its coordinates, and using the ellipsoid as the ε-neighborhood of the point, using the DBSCAN algorithm to determine and filter out the noise points in the point cloud data; optimizing and solving the pose of each frame of the radar based on the adjustment theory; optimizing and adjusting the pose of each frame of the radar according to the pose change within each preset time length of the radar; constructing the corresponding map of the target according to the pose of each frame after optimization and adjustment and the point cloud data of each frame after denoising. The present invention improves the accuracy of the point cloud data by denoising the 4D point cloud data, thereby improving the positioning and mapping accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of measurement and positioning, and in particular relates to a real-time positioning and mapping method based on 4D millimeter wave radar. Background Art

[0002] In recent years, with the development of science and technology, Simultaneous Localization And Mapping (SLAM) based on technologies such as LiDAR or optical sensing has become a hot topic for researchers. SLAM technology has been widely used in various fields, such as intelligent mapping, assisted driving, and smart home. At the data acquisition level, methods based on optical cameras can achieve high resolution and high accuracy in many tasks, such as behavior detection and target recognition, but may infringe on privacy. In addition, methods based on optical cameras are not suitable for use in dim light, foggy or smoky conditions. Although infrared sensing does not have the problem of privacy leakage, due to the pyroelectric principle, it can only detect the presence or absence of objects and cannot accurately obtain distance and direction information. Although LiDAR solves some of the above problems, it is also inefficient in foggy or smoky environments and is costly.

[0003] Among various sensors, millimeter-wave radar is widely used in military, assisted driving and other fields due to its recognized advantages such as small size, low cost, all-weather operation, low risk of privacy leakage and speed measurement capability. However, millimeter-wave radar still has the following technical defects in the field of measurement and positioning:

[0004] (1) Clutter points and outliers reduce the accuracy of pose estimation.

[0005] Millimeter-wave radar has good adaptability to the environment, but it is prone to multipath effects and produces noise point clouds such as clutter points and outliers. Noise point clouds not only reduce the accuracy of pose estimation, but also appear on the environment map, affecting the accuracy of the map. Therefore, it is necessary to filter out the noise point clouds. At present, the filtering of noise point clouds is mainly for traditional 3D millimeter-wave radars, while there is a lack of filtering methods for 4D imaging millimeter-wave radar noise point clouds.

[0006] (2) The millimeter-wave radar point cloud has low accuracy.

[0007] At present, most millimeter-wave radar SLAM algorithms are modified and optimized based on the lidar SLAM algorithm. Due to the obvious difference in density and accuracy between lidar point clouds and millimeter-wave radar point clouds, the algorithm performance is poor. Compared with traditional 3D millimeter-wave radars, 4D imaging millimeter-wave radars have increased the number of antennas, and the point cloud accuracy and density have been significantly improved, but they still cannot meet the needs of high-precision SLAM systems. Therefore, it is necessary to combine the characteristics of millimeter-wave radar point clouds to study SLAM algorithms suitable for millimeter-wave radars.

[0008] (3) The error in elevation and posture estimation is large.

[0009] Compared with traditional 3D millimeter-wave radar, 4D imaging millimeter-wave radar has the ability to detect target height, but its measurement accuracy is low and the error is large, which leads to an increase in the error of elevation and posture estimation, seriously affecting the accuracy of positioning and mapping. Therefore, the elevation and posture need to be finely optimized. Summary of the invention

[0010] The present invention provides a real-time positioning and mapping method based on 4D millimeter wave radar, which improves the accuracy of point cloud data by denoising 4D point cloud data, thereby improving the positioning and mapping accuracy.

[0011] In order to achieve the above technical objectives, the present invention adopts the following technical solutions:

[0012] A real-time positioning and mapping method based on 4D millimeter wave radar, comprising:

[0013] Step 1, intra-frame denoising:

[0014] Step 1.1, obtain the angular accuracy of the 4D millimeter wave radar and the point cloud data of the target in the current frame, and calculate the point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system ;

[0015] Step 1.2: For each point in the point cloud data , based on its coordinates Construct the corresponding ellipsoid and use the ellipsoid as the point ε-neighborhood of ;

[0016] Step 1.3, based on the ε-neighborhood of each point, the DBSCAN algorithm is used to determine and filter out the noise points in the point cloud data;

[0017] Step 2, based on the adjustment theory, the position and posture of the 4D millimeter wave radar in each frame are obtained by optimizing and solving;

[0018] Step 3, optimizing and adjusting the pose of each frame of the 4D millimeter-wave radar according to the pose change of the 4D millimeter-wave radar within each preset time length;

[0019] Step 4: construct a corresponding map of the target based on the pose of each frame of the 4D millimeter-wave radar obtained in step 3 and the point cloud data of each frame after denoising in step 1.

[0020] Furthermore, the calculation of each point in the point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system , specifically:

[0021] (1) Point Coordinates in the radar local coordinate system , converted to Cartesian coordinates :

[0022]

[0023] In the formula, Points The distance, elevation angle and horizontal angle in the radar local coordinate system, Points The three-axis position in the Cartesian coordinate system;

[0024] (2) Calculate the point according to the angular accuracy of the 4D millimeter wave radar Precision error in Cartesian coordinate system:

[0025]

[0026] In the formula, The pitch angle accuracy and horizontal angle accuracy of the 4D millimeter wave radar. For point Three-axis accuracy error in Cartesian coordinate system;

[0027] (3) Comprehensive points Coordinates in Cartesian coordinate system and precision error, we get the point Coordinates considering angular accuracy in a Cartesian coordinate system :

[0028]

[0029] In the formula, For point Three-axis position considering angular accuracy in Cartesian coordinates.

[0030] Furthermore, based on the point cloud coordinates The corresponding ellipsoid is constructed as:

[0031]

[0032] In the formula, For point Considering the three-axis position of angular accuracy in the Cartesian coordinate system, Ellipsoid The coordinates of the point on .

[0033] Furthermore, step 1.3 specifically includes:

[0034] (1) Set all points in the frame as sample point sets and set the minimum number of included points MinPts;

[0035] (2) Statistical sample points The number of sample points in its ε-neighborhood, if the number is greater than or equal to MinPts, then the sample point As the core sample point;

[0036] (3) For each non-core sample point in the frame, determine whether it is in the ε-neighborhood of any core sample point. If so, the non-core sample point is a boundary sample; if not, the non-core sample point is a noise point.

[0037] (4) Discard the noise points from the point cloud data of the frame.

[0038] Furthermore, the minimum number of included points MinPts is set to 3.

[0039] Furthermore, step 2 specifically includes:

[0040] Step 2.1: Get the distance accuracy of 4D millimeter wave radar , Pitch angle accuracy and horizontal angle accuracy , construct the precision covariance matrix :

[0041]

[0042] Step 2.2, according to the precision covariance matrix , calculate each point The error matrix :

[0043]

[0044] In the formula, Points The pitch angle and horizontal angle, Reason The three-dimensional rotation matrix formed;

[0045] Step 2.3, according to the error matrix Calculation Point The error covariance matrix :

[0046]

[0047] Step 2.4, according to the point The radar coordinates and error covariance matrix in two adjacent frames are used to build an optimization model based on the adjustment theory:

[0048]

[0049] In the formula, They are respectively The covariance matrix constructed from the radar coordinates of any two adjacent frames A and B is: Points The error covariance matrix of two adjacent frames A and B is: is the position and posture of the 4D millimeter-wave radar in frame A; For point The distance between two adjacent frames A and B;

[0050] Step 2.5, by solving the optimization model established in step 2.4, the position and posture of the 4D millimeter-wave radar in each frame are obtained.

[0051] Furthermore, step 3 specifically includes:

[0052] Step 3.1, using a sliding window of a preset length and a preset step size, sliding to obtain the pose data of the 4D millimeter wave radar;

[0053] Step 3.2: Calculate the pose change of the last frame relative to the first frame in the sliding window, including the pose change in the forward direction. and elevation posture changes , and then calculate the slope within the sliding window ;

[0054] Step 3.3, if the slope in a sliding window When the preset slope threshold is exceeded, the elevation of the last frame in the sliding window is modified to the average elevation of all other frames in the sliding window.

[0055] Furthermore, the preset length of the sliding window is 100 frames, and the preset sliding step is 1 frame.

[0056] Furthermore, the preset slope threshold is 6%.

[0057] Compared with the prior art, the present invention has the following beneficial effects:

[0058] 1. By improving the traditional DBSCAN algorithm and combining the characteristics of millimeter-wave radar point cloud, a DBSCAN algorithm based on millimeter-wave radar point cloud accuracy is proposed, which improves the accuracy of DBSCAN and provides higher quality point cloud for subsequent point cloud matching.

[0059] 2. By improving the traditional ICP algorithm, the accuracy of point cloud matching is improved according to the millimeter-wave radar point cloud accuracy fusion adjustment theory. Compared with the traditional ICP algorithm directly used for millimeter-wave radar point cloud matching, the point cloud matching error is greatly reduced.

[0060] 3. Since the millimeter-wave radar pitch point cloud has low accuracy, it will lead to a large error in the Z (pitch) direction. The method based on elevation constraint of the present invention can greatly reduce the error. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a flow chart of the real-time positioning and mapping method based on 4D millimeter wave radar described in an embodiment of the present application. DETAILED DESCRIPTION

[0062] The following is a detailed description of an embodiment of the present invention. This embodiment is based on the technical solution of the present invention, and provides a detailed implementation method and a specific operation process to further explain the technical solution of the present invention.

[0063] This embodiment provides a real-time positioning and mapping method based on 4D millimeter wave radar, referring to Figure 1 As shown, the following steps are included:

[0064] Step 1: Intra-frame denoising.

[0065] Step 1.1, obtain the angular accuracy of the 4D millimeter wave radar and the point cloud data of the target in the current frame, and calculate the point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system . Specifically include:

[0066] (1) Point Coordinates in the radar local coordinate system , converted to Cartesian coordinates :

[0067]

[0068] In the formula, Points The distance, elevation angle and horizontal angle in the radar local coordinate system, Points The three-axis position in the Cartesian coordinate system;

[0069] (2) Calculate the point according to the angular accuracy of the 4D millimeter wave radar Precision error in Cartesian coordinate system:

[0070]

[0071] In the formula, The pitch angle accuracy and horizontal angle accuracy of the 4D millimeter wave radar. For point Three-axis accuracy error in Cartesian coordinate system;

[0072] (3) Comprehensive points Coordinates in Cartesian coordinate system and precision error, we get the point Coordinates considering angular accuracy in a Cartesian coordinate system :

[0073]

[0074] In the formula, For point Three-axis position considering angular accuracy in Cartesian coordinates.

[0075] Step 1.2: For each point in the point cloud data , based on its coordinates Construct the corresponding ellipsoid and use the ellipsoid as the point The ε-neighborhood of The corresponding ellipsoid is constructed as:

[0076]

[0077] In the formula, For point The corresponding ellipsoid The coordinates of the point on .

[0078] Step 1.3, based on the ε-neighborhood of each point, use the DBSCAN algorithm to determine and filter out the noise points in the point cloud data. Specifically include:

[0079] (1) All points in the frame are set as sample point sets, and the minimum number of included points MinPts is set. In this embodiment, the minimum number of included points MinPts is set to 3.

[0080] (2) Statistical sample points The number of sample points in its ε-neighborhood, if the number is greater than or equal to MinPts, then the sample point As the core sample point;

[0081] (3) For each non-core sample point in the frame, determine whether it is in the ε-neighborhood of any core sample point. If so, the non-core sample point is a boundary sample; if not, the non-core sample point is a noise point.

[0082] (4) Discard the noise points from the point cloud data of the frame.

[0083] In step 1 of this embodiment, the range of clustering is determined in combination with the point cloud accuracy. Since there are differences in the horizontal angle accuracy and the pitch angle accuracy of the point cloud, the range of clustering is no longer a sphere determined by a certain radius in the traditional DBSCAN algorithm, but an ellipsoid range determined by the angular accuracy, so as to more accurately find noise point clouds such as clutter points and outliers, thereby improving the accuracy of denoising.

[0084] Step 2: Optimize and solve the position and posture of the 4D millimeter-wave radar in each frame based on the adjustment theory. Specifically include:

[0085] Step 2.1: Get the distance accuracy of 4D millimeter wave radar , Pitch angle accuracy and horizontal angle accuracy , construct the precision covariance matrix :

[0086]

[0087] Step 2.2, according to the precision covariance matrix , calculate each point The error matrix :

[0088]

[0089] In the formula, Points The pitch angle and horizontal angle, Reason The three-dimensional rotation matrix formed;

[0090] Step 2.3, according to the error matrix Calculation Point The error covariance matrix :

[0091]

[0092] Step 2.4, according to the point The radar coordinates and error covariance matrix in two adjacent frames are used to build an optimization model based on the adjustment theory:

[0093]

[0094] In the formula, They are respectively The covariance matrix constructed from the radar coordinates of any two adjacent frames A and B is: Points The error covariance matrix of two adjacent frames A and B is: is the position and posture of the 4D millimeter-wave radar in frame A; For point The distance between two adjacent frames A and B;

[0095] Step 2.5, by solving the optimization model established in step 2.4, the position and posture of the 4D millimeter-wave radar in each frame are obtained.

[0096] Although step 1 filters out noise point clouds such as clutter points and outliers by improving the DBSCAN algorithm, the accuracy of the point cloud cannot be changed. Therefore, the influence of the point cloud accuracy must still be considered when performing front-end matching. Therefore, the present invention proposes pose estimation optimization based on adjustment theory.

[0097] Step 3, according to the posture change of the 4D millimeter wave radar within each preset time length, optimize and adjust the posture of each frame of the 4D millimeter wave radar. Step 3 specifically includes:

[0098] Step 3.1, use a sliding window of preset length and a preset step size to slide and obtain the posture data of the 4D millimeter wave radar; in this embodiment, the preset length of the sliding window is 100 frames, and the preset sliding step size is 1 frame.

[0099] Step 3.2: Calculate the pose change of the last frame relative to the first frame in the sliding window, including the pose change in the forward direction. and elevation posture changes , and then calculate the slope within the sliding window .

[0100] Step 3.3, if the slope in a sliding window If the slope exceeds a preset threshold, the elevation of the last frame in the sliding window is modified to the average elevation of all other frames in the sliding window; in this embodiment, the preset slope threshold is 6%.

[0101] Since the millimeter-wave radar has a low azimuth angle accuracy, resulting in a large error in the pose estimation in the elevation direction, this step 3 proposes to adopt a fine pose optimization method based on elevation constraints to make the pose of the D millimeter-wave radar more accurate.

[0102] Step 4: According to the pose of each frame of the 4D millimeter-wave radar obtained in step 3 and the point cloud data of each frame after denoising in step 1, a corresponding map of the target is constructed by matching.

[0103] Assume the position of the radar in frame A is , is the point cloud in frame A, where are rotation matrix and translation matrix respectively, then use the formula The point cloud in frame A can be converted to frame B, which can be expressed as , the point clouds of all frames can be matched together to obtain the corresponding map.

[0104] The above embodiments are preferred embodiments of the present application. Ordinary technicians in this field can also make various changes or improvements on this basis. Without departing from the overall concept of the present application, these changes or improvements should fall within the scope of protection required by the present application.

Claims

1. A real-time positioning and mapping method based on 4D millimeter wave radar, characterized in that: include: Step 1, intra-frame denoising: Step 1.1, obtain the angular accuracy of the 4D millimeter wave radar and the point cloud data of the target in the current frame, and calculate the point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system ; Step 1.2, for each point in the point cloud data , based on its coordinates Construct the corresponding ellipsoid and use the ellipsoid as the point ε-neighborhood of ; Step 1.3, based on the ε-neighborhood of each point, the DBSCAN algorithm is used to determine and filter out the noise points in the point cloud data; Step 2, based on the adjustment theory, the position and posture of the 4D millimeter wave radar in each frame are obtained by optimizing and solving; Step 2.1: Get the distance accuracy of 4D millimeter wave radar , Pitch angle accuracy and horizontal angle accuracy , construct the precision covariance matrix : ； Step 2.2, according to the precision covariance matrix , calculate each point The error matrix : ； In the formula, Points The pitch angle and horizontal angle, Reason The three-dimensional rotation matrix formed; Step 2.3, according to the error matrix Calculation Point The error covariance matrix : ； Step 2.4, according to the point The radar coordinates and error covariance matrix in two adjacent frames are used to build an optimization model based on the adjustment theory: ； In the formula, They are respectively The covariance matrix constructed from the radar coordinates of any two adjacent frames A and B is: Points The error covariance matrix between two adjacent frames A and B is: is the position and posture of the 4D millimeter-wave radar in frame A; For point The distance between two adjacent frames A and B; Step 2.5, by solving the optimization model established in step 2.4, the position and posture of the 4D millimeter-wave radar in each frame are obtained; Step 3, optimizing and adjusting the pose of each frame of the 4D millimeter-wave radar according to the pose change of the 4D millimeter-wave radar within each preset time length; Step 4: construct a corresponding map of the target based on the pose of each frame of the 4D millimeter-wave radar obtained in step 3 and the point cloud data of each frame after denoising in step 1.

2. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 1 is characterized in that: Each point in the calculation point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system , specifically: (1) Point Coordinates in the radar local coordinate system , converted to Cartesian coordinates : ； In the formula, Points The distance, elevation angle and horizontal angle in the radar local coordinate system, Points The three-axis position in the Cartesian coordinate system; (2) Calculate the point according to the angular accuracy of the 4D millimeter wave radar Precision error in Cartesian coordinate system: ； In the formula, The pitch angle accuracy and horizontal angle accuracy of the 4D millimeter wave radar. For point Three-axis accuracy error in Cartesian coordinate system; (3) Comprehensive points Coordinates in Cartesian coordinate system and precision error, we get the point Coordinates considering angular accuracy in a Cartesian coordinate system : ； In the formula, For point Three-axis position considering angular accuracy in Cartesian coordinates.

3. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 1 is characterized in that: Based on point cloud coordinates The corresponding ellipsoid is constructed as: ； In the formula, For point Considering the three-axis position of angular accuracy in the Cartesian coordinate system, Ellipsoid The coordinates of the point on .

4. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 1 is characterized in that: Step 1.3 specifically includes: (1) Set all points in the frame as sample point sets and set the minimum number of included points MinPts; (2) Statistical sample points The number of sample points in its ε-neighborhood, if the number is greater than or equal to MinPts, then the sample point are the core sample points; (3) For each non-core sample point in the frame, determine whether it is in the ε-neighborhood of any core sample point. If so, the non-core sample point is a boundary sample; if not, the non-core sample point is a noise point. (4) Discard the noise points from the point cloud data of the frame.

5. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 4 is characterized in that: The minimum number of included points MinPts is set to 3.

6. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 1 is characterized in that: Step 3 specifically includes: Step 3.1, using a sliding window of a preset length and a preset step size, sliding to obtain the pose data of the 4D millimeter wave radar; Step 3.2: Calculate the pose change of the last frame relative to the first frame in the sliding window, including the pose change in the forward direction. and elevation posture changes , and then calculate the slope within the sliding window ; Step 3.3, if the slope in a sliding window When the preset slope threshold is exceeded, the elevation of the last frame in the sliding window is modified to the average elevation of all the remaining frames in the sliding window.

7. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 6 is characterized in that: The preset length of the sliding window is 100 frames, and the preset sliding step is 1 frame.

8. The instant positioning and mapping method based on 4D millimeter wave radar according to claim 6 is characterized in that: The preset slope threshold is 6%.

Images