Map-based 4d millimeter wave radar positioning method and system

Abstract

The application provides a kind of 4D millimeter wave radar positioning method and system based on map, comprising: step S1: extracting ground point cloud, comparing two consecutive frames of point cloud, remove the interference point under ground;Step S2: remove dynamic point in point cloud, and estimate the linear velocity and angular velocity of equipment;Step S3: relative pose transformation estimation is obtained using 4D millimeter wave radar point cloud and the registration of local submap, and pose graph is constructed, and prior positioning is estimated, and the optimal pose is estimated using graph optimization;Step S4: if it is detected that equipment is still in the range of prior map, then the positioning pose estimation of step S3 is output, if it is judged that equipment moves to the area unknown to map, then pre-integration is carried out using estimated ego speed, local map is constructed, and after moving out of the area unknown to map, pose graph optimization is carried out, so as to output positioning pose and update global map.The application can use 4D millimeter wave radar to carry out accurate, robust positioning and map updating on the basis of prior map.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more specifically, to a map-based 4D millimeter-wave radar positioning method and system. Background Technology

[0002] Millimeter-wave radar has been widely used in positioning technology for autonomous driving. Currently, the most commonly used sensors for robots are optical sensors such as cameras and LiDAR. However, cameras typically have poor ranging capabilities and are easily affected by harsh environments and extreme lighting conditions. Furthermore, single cameras suffer from scale uncertainty, often requiring additional algorithm design for environmental depth estimation. LiDAR, on the other hand, is relatively expensive and performs poorly in harsh environments such as rain, snow, and smoke.

[0003] Besides cameras and LiDAR, research on millimeter-wave radar sensors has become increasingly popular in recent years, especially in the field of autonomous driving. Compared with optical sensors such as cameras or LiDAR, millimeter-wave radar has the advantages of lower sensitivity to extreme weather and lighting conditions, and it also has a long history of development, mature technology, and is much cheaper. However, despite these advantages, millimeter-wave radar point clouds are usually sparser and noisier than LiDAR, which poses a challenge to accurate localization tasks. Nevertheless, in recent years, the latest millimeter-wave radar sensors, namely 4D millimeter-wave radar, have seen increasing applications in autonomous driving due to their advantages over traditional radar.

[0004] Traditional millimeter-wave radar can be divided into two types: scanning radar and automotive radar. Scanning radar can obtain a two-dimensional radar energy image by scanning 360 degrees, while automotive radar, compared to scanning radar, typically has a smaller field of view but can provide sparse two-dimensional point clouds with Doppler velocity. Compared to traditional millimeter-wave radar sensors, 4D millimeter-wave radar and automotive radar share some similarities, such as similar field of view and the ability to obtain point clouds with Doppler velocity information. However, 4D millimeter-wave radar can provide a denser and more accurate three-dimensional point cloud. The 4D point cloud provided by 4D millimeter-wave radar has four dimensions of information: range, azimuth, elevation, and Doppler velocity. Because of this additional information and higher resolution, 4D millimeter-wave radar offers new opportunities for its application in robot localization technology.

[0005] For the positioning problem based on millimeter-wave radar, researchers both at home and abroad have conducted a great deal of research. Based on the type of millimeter-wave radar used, it can be roughly divided into two categories:

[0006] One type is the scanning radar-based localization algorithm. This type of algorithm uses 2D images obtained by scanning radar as input. It usually uses some feature extraction algorithms to extract feature points from the image and then performs scanning registration based on feature points. Among them, the Under the Radar algorithm proposed by Barnes et al. [1] (Barnes D, Posner I. Under the radar: Learning to predict robust keypoints for odometry estimation and metric localization in radar [C] / / 2020 IEEE international conference on robotics and automation (ICRA). IEEE, 2020: 9484-9490.) put forward a self-supervised learning framework for learning robust keypoints for detecting radar odometry and localization. By embedding a differentiable point-based motion estimator, it learns the keypoint position, score and descriptor only from localization errors. This method avoids introducing any artificial assumptions about robust keypoints and is optimized according to the actual application. In addition, the architecture is sensor-agnostic and can be applied to most modalities. However, these methods are not applicable to 4D millimeter-wave radar point clouds because they take 2D scanned radar images as input.

[0007] Second, there are positioning algorithms based on automotive radar. These algorithms use sparse radar point clouds obtained by traditional automotive radar as input and estimate the vehicle's motion by utilizing the Doppler velocity information of the point cloud or by utilizing the scanning registration of the point cloud. Kellner et al. [2] (D. Kellner, M. Barjenbruch, J. Klappstein, J. Dickmann, and K. Dietmayer, “Instantaneous ego-motion estimation using Doppler radar,” in 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013), Oct. 2013, pp. 869–874, iSSN: 2153-0017.) used the Doppler velocity information of the radar point cloud to calculate the vehicle's speed, thereby realizing the estimation of the vehicle's motion. Gao et al. proposed a new automotive radar-based localization algorithm, DC-Loc[3](Gao P, Zhang S, Wang W, et al.Dc-loc:Accurate automotive radar based metric localization with explicit doppler compensation[C] / / 2022 International Conference on Robotics and Automation (ICRA).IEEE,2022:4128-4134.). Their research focuses on motion compensation under high-speed radar movement and the chain effect of Doppler effect on localization algorithm. DC-Loc can explicitly compensate for Doppler distortion of radar scanning to obtain more accurate point cloud position estimation. Then, a measurement uncertainty model of Doppler-compensated point cloud is established to further optimize the localization effect. However, their algorithm does not consider the influence of radar noise. The localization accuracy is low due to noise, and it is heavily dependent on prior maps. Inaccurate maps will lead to localization failure. Summary of the Invention

[0008] To address the shortcomings of existing technologies, this invention provides a map-based 4D millimeter-wave radar positioning method and system.

[0009] According to the present invention, a map-based 4D millimeter-wave radar positioning method and system are provided, the scheme of which is as follows:

[0010] Firstly, a map-based 4D millimeter-wave radar positioning method is provided, the method comprising:

[0011] Radar point cloud preprocessing steps: Extract ground point cloud, compare two consecutive point cloud frames, and remove interference points below the ground.

[0012] The vehicle speed estimation steps are as follows: using the Doppler velocity information of the 4D millimeter-wave radar point cloud, static points are extracted and the vehicle speed is estimated.

[0013] The fusion localization steps are as follows: relative pose transformation is estimated by registration of point cloud and local sub-map and pose graph is constructed. Prior localization is estimated by registration of 4D millimeter-wave radar point cloud and prior map. Finally, the optimal pose is estimated by graph optimization.

[0014] Map update steps: Unknown areas are detected based on the overlap between the 4D millimeter-wave radar point cloud and the prior map. If no unknown areas are detected, the pose estimation results of the fusion positioning step are output. If it is determined that the device has moved into an unknown area, the estimated vehicle speed is used for pre-integration to construct a local keyframe map. After leaving the unknown area, the pose map is optimized to update the global map.

[0015] Preferably, the radar point cloud preprocessing step includes: removing interference points below the ground, wherein the interference points include ghost points and random points;

[0016] Ground point clouds are extracted from the original radar point clouds, and ghost points below the ground are filtered out; random points are identified and filtered out by comparing point clouds from two consecutive frames.

[0017] Preferably, the vehicle speed estimation step includes:

[0018] Step S2.1: Static point extraction; The 4D millimeter-wave radar can obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows:

[0019] v r,i =-d i ·v s

[0020] Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s That's the speed of the radar.

[0021] Since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows:

[0022]

[0023] Among them, t s and R sThese represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These are the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0024]

[0025] Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s The antisymmetric matrix, where R 3×3 This represents the set of 3×3 real matrices. Since the relationship between the Doppler velocity of dynamic points and the vehicle velocity does not conform to the above equation, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0026] Step S2.2: Least squares estimation. After extracting the static points, the vehicle speed is estimated using the least squares method by utilizing the relationship between the Doppler velocity and the vehicle speed of all static points.

[0027] Preferably, the fusion positioning step includes:

[0028] Step S3.1: Radar odometry, based on normal distribution transformation, estimates the relative transformation pose between keyframes by matching the current 4D millimeter-wave radar point cloud and keyframe sub-map;

[0029] A radar sub-map is built using a sliding window with multiple keyframe point clouds. If the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe and added to the radar sub-map. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded.

[0030] After establishing the radar submap, it is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. When calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is considered. The current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud distribution in the submap is calculated. The error of this pose transformation estimation is e. O ;

[0031] Step S3.2: Prior localization. Based on normal distribution transformation, the global localization pose is estimated by matching the current 4D millimeter-wave radar point cloud with the known prior map, and the cumulative error of the radar odometry is corrected.

[0032] The prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. The current 4D millimeter-wave radar point cloud is registered with the prior map. The pose that maximizes the probability of the current point cloud distribution in the prior map is calculated. The error of this pose estimation is e. W ;

[0033] To balance the accuracy and speed of the algorithm, prior localization is calculated every 5 keyframes;

[0034] Step S3.3: Pose fusion. Establish a pose graph, considering the error terms in pose estimation from radar odometry and prior localization at all times. Estimate the optimal pose of the device at all times through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. Record all poses as... Constructing a maximum a posteriori problem is equivalent to solving a nonlinear least squares problem to estimate the optimal pose.

[0035]

[0036] in, Includes all moments, e O ,e w The error of pose estimation obtained at different steps.

[0037] Preferably, the map update step includes:

[0038] Step S4.1: Unknown area detection. Determine whether the device has moved into an unknown area of ​​the map. When the device is moving in a known area of ​​the map, each point in the current point cloud is surrounded by many map points. Therefore, the anomaly score is represented by the average distance between each point in the current point cloud and the surrounding map points. When the anomaly score is greater than the threshold δ, the anomaly score is determined. s When the device is considered to have entered an area unknown on the map, the known map state v=0 is set, and local mapping begins.

[0039] Similarly, anomaly scores are used to determine whether the device should return to an area where the map exists, thus ending local mapping; when the anomaly score is less than a threshold δ... s ′ Furthermore, if a mapping pose exists near the current positioning pose, set the known map state v=1, end the local mapping, and complete and update the established local map onto the known prior map.

[0040] Step S4.2: Velocity pre-integration. Using the estimated equipment velocity, calculate the relative pose transformation, introducing additional reliable relative pose estimation. The estimated linear and angular velocities of the equipment at time t are denoted as... and The estimated value is the true value superimposed with zero-mean Gaussian white noise, i.e.:

[0041]

[0042]

[0043] Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It is the orientation of the device in the world coordinate system. That is, R t The transpose of the equation; the relative rotation transformation ΔR between time i and time j is obtained by integration. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e. V ;

[0044] Step S4.3: Local graph construction. Establish a pose graph, considering the velocity pre-integration at all times and the error term in the relative pose estimation from the radar odometer. Estimate the optimal pose of the device at all times through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. Record all poses as... The corresponding point cloud is The known prior map is The updated map is The local map created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map is as follows:

[0045]

[0046]

[0047]

[0048] in, Includes all moments, e O ,e V ,e w The error of pose estimation obtained in different steps corresponds to v∈{0,1}, which is the known map state in step S4.1. This indicates the use of optimized pose estimation Point cloud Perform the transformation.

[0049] Secondly, a map-based 4D millimeter-wave radar positioning system is provided, the system comprising:

[0050] Radar point cloud preprocessing module: Extracts ground point cloud, compares two consecutive frames of point cloud, and removes interference points below the ground;

[0051] Vehicle speed estimation module: Utilizes Doppler velocity information from 4D millimeter-wave radar point clouds to extract static points and estimate vehicle speed;

[0052] Fusion localization module: It obtains relative pose transformation estimation and constructs pose graph by registering point cloud and local sub-map, and estimates prior localization by registering 4D millimeter wave radar point cloud and prior map. Finally, it estimates the optimal pose by graph optimization.

[0053] Map update module: It detects unknown areas based on the overlap between the 4D millimeter-wave radar point cloud and the prior map. If no unknown area is detected, it outputs the pose estimation result of the fusion positioning module. If it is determined that the device has moved into an unknown area, it uses the estimated vehicle speed for pre-integration to build a local keyframe map. After leaving the unknown area, it optimizes the pose map to update the global map.

[0054] Preferably, the radar point cloud preprocessing module includes: removing interference points below the ground, wherein the interference points include ghost points and random points;

[0055] Ground point clouds are extracted from the original radar point clouds, and ghost points below the ground are filtered out; random points are identified and filtered out by comparing point clouds from two consecutive frames.

[0056] Preferably, the vehicle speed estimation module includes:

[0057] Module M2.1: Static Point Extraction; 4D millimeter-wave radar can obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows:

[0058] v r,i =-d i ·v s

[0059] Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s That's the speed of the radar.

[0060] Since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows:

[0061]

[0062] Among them, t s and R s These represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These are the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0063]

[0064] Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s The antisymmetric matrix, where R 3×3 This represents the set of 3×3 real matrices. Since the relationship between the Doppler velocity of dynamic points and the vehicle velocity does not conform to the above equation, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0065] Module M2.2: Least Squares Estimation. After extracting the static points, the vehicle speed is estimated using the least squares method by utilizing the relationship between the Doppler velocity of all static points and the vehicle speed.

[0066] Preferably, the fusion positioning module includes:

[0067] Module M3.1: Radar odometry, based on normal distribution transformation, estimates the relative transformation pose between keyframes by matching the current 4D millimeter-wave radar point cloud and keyframe sub-map;

[0068] A radar sub-map is built using a sliding window with multiple keyframe point clouds. If the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe and added to the radar sub-map. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded.

[0069] After establishing the radar submap, it is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. When calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is considered. The current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud distribution in the submap is calculated. The error of this pose transformation estimation is e. O ;

[0070] Module M3.2: Prior localization, based on normal distribution transformation, estimates global positioning pose by matching the current 4D millimeter-wave radar point cloud with the known prior map, and corrects the cumulative error of radar odometry;

[0071] The prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. The current 4D millimeter-wave radar point cloud is registered with the prior map. The pose that maximizes the probability of the current point cloud distribution in the prior map is calculated. The error of this pose estimation is e. W ;

[0072] To balance the accuracy and speed of the algorithm, prior localization is calculated every 5 keyframes;

[0073] Module M3.3: Pose Fusion. A pose graph is established, considering the error terms in pose estimation from radar odometry and prior localization at all times. The optimal pose of the device at all times is estimated through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. All poses are denoted as... Constructing a maximum a posteriori problem is equivalent to solving a nonlinear least squares problem to estimate the optimal pose.

[0074]

[0075] in, Includes all moments, e O ,e w The error of pose estimation obtained from different modules.

[0076] Preferably, the map update module includes:

[0077] Module M4.1: Unknown Area Detection. This module determines whether the device has moved into an unknown area of ​​the map. When the device is traveling in a known area of ​​the map, each point in the current point cloud is surrounded by numerous map points. Therefore, an anomaly score is represented by the average distance between each point in the current point cloud and its surrounding map points. When the anomaly score exceeds a threshold δ... s When the device is considered to have entered an area unknown on the map, the known map state v=0 is set, and local mapping begins.

[0078] Similarly, anomaly scores are used to determine whether the device should return to an area where the map exists, thus ending local mapping; when the anomaly score is less than a threshold δ... s ′ Furthermore, if a mapping pose exists near the current positioning pose, set the known map state ν=1, end the local mapping, and complete and update the established local map onto the known prior map.

[0079] Module M4.2: Velocity pre-integration. Using the estimated equipment velocity, the relative pose transformation is calculated, introducing additional reliable relative pose estimation. The estimated linear and angular velocities of the equipment at time t are denoted as... and The estimated value is the true value superimposed with zero-mean Gaussian white noise, i.e.:

[0080]

[0081]

[0082] Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It is the orientation of the device in the world coordinate system. That is, R t The transpose of the equation; the relative rotation transformation ΔR between time i and time j is obtained by integration. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e. V ;

[0083] Module M4.3: Local graph construction, establishing a pose graph, considering the velocity pre-integration at all times and the error term of relative pose estimation in radar odometer, and estimating the optimal pose of the device at all times through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times; all poses are denoted as... The corresponding point cloud is The known prior map is The updated map is The local map created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map is as follows:

[0084]

[0085]

[0086]

[0087] in, Includes all moments, e O ,e V ,e w The pose estimation errors obtained from different modules are represented by ν∈{0,1}, which represents the known map state in module M4.1. This indicates the use of optimized pose estimation Point cloud Perform the transformation.

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

[0089] 1. In order to achieve better accuracy in the case of sparse point clouds, the present invention pre-integrates the estimated vehicle speed to obtain additional relative pose estimation.

[0090] 2. This invention introduces map updates. The scope of a map is usually limited. When the device reaches an area where the map is unknown, it needs to maintain the accuracy of the positioning and complete and update the map when it returns to a known area.

[0091] 3. This invention can utilize 4D millimeter-wave radar to perform accurate and robust positioning and map updates based on prior maps.

[0092] Other beneficial effects of the present invention will be explained in detail through the introduction of specific technical features and technical solutions in specific embodiments. Those skilled in the art should be able to understand the beneficial technical effects brought about by these technical features and technical solutions through the introduction of these technical features and technical solutions. Attached Figure Description

[0093] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0094] Figure 1 This is a schematic diagram of the overall process of the present invention. Detailed Implementation

[0095] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0096] This invention provides a map-based 4D millimeter-wave radar localization method, applicable to the localization and map updating problems of vehicles or mobile robots when the prior map of the environment is known. The algorithm comprises four parts: radar point cloud preprocessing, vehicle speed estimation, fusion localization, and map updating. To address the issue of high noise in 4D millimeter-wave radar point clouds, a preprocessing step was designed to reduce ghosting points and random noise in the original 4D millimeter-wave radar point cloud. Next, the vehicle speed was estimated from the Doppler velocity of the filtered point cloud, which played a crucial role in subsequent map updates. In fusion localization, radar odometry was achieved through point cloud registration based on normal distribution transformation, estimating relative pose transformation, and the cumulative drift of the radar odometry was corrected using the global pose estimation from prior localization. In map updates, unknown region detection was used to determine if the device had moved into an unknown area of ​​the map. To achieve better accuracy with sparse point clouds, the estimated vehicle speed was pre-integrated to obtain additional relative pose estimation. This was then combined with the radar odometry results for local map construction. Upon returning to the prior map area, pose map optimization was performed, and the local map was completed and updated onto the prior map. The optimized local pose estimation was then output, achieving accurate and robust localization and map updates.

[0097] Reference Figure 1 As shown, the method specifically includes:

[0098] Radar point cloud preprocessing steps: extract ground point clouds, compare two consecutive frames of point clouds, and remove ghost points and random points below the ground.

[0099] This step specifically includes:

[0100] Step 1.1: Ghost point removal. Ghost points are spurious measurements below the ground in the original 4D millimeter-wave radar point cloud due to the multipath effect of millimeter waves, and are one type of noise in 4D millimeter-wave radar measurements. This is achieved by extracting the ground point cloud from the original radar point cloud and then filtering out the ghost points below the ground.

[0101] The raw radar point cloud obtained by 4D millimeter-wave radar n is the number of points in the point cloud, where the position of each point in the radar coordinate system can be represented as... Where r i ,θ i , These are the spatial information of the point obtained from 4D millimeter-wave radar measurements: range, azimuth, and elevation. For the original radar point cloud, points with ranges less than a threshold δ are first retained. r And the height is around a certain threshold δ near the radar installation height H. h Points within, that is, for all reserve And z i ∈[-H-Δ h ,-H+Δ h The points are selected because they are most likely to belong to the ground. Then, principal component analysis is used to estimate the upward normal vector n of each point. i Next, the angle between the normal vector and the positive z-axis unit vector is kept to be less than δ. n The point, i.e., n i ·(0,0,1) T ≥cosδ n Finally, the random sample consensus algorithm is used to extract the plane Ax + By + Cz + D = 0, which is considered the ground. Ghost points below the ground, i.e., Ax, are then filtered out. i +By i +Cz i +D<0 points;

[0102] Step 1.2: Random point removal. Random points are unstable, flickering spurious measurements contained in the original 4D millimeter-wave radar point cloud, and are one type of measurement noise in 4D millimeter-wave radar. Since random points do not appear consecutively in multiple frames of radar point cloud, they are identified and filtered out by comparing two consecutive frames of point cloud.

[0103] First, calculate the current point cloud. Point cloud in the next adjacent frame Rotational transformation between R k-1 Translation and shift transformation t k-1 :

[0104]

[0105]

[0106] in, and These are the estimated linear velocity and angular velocity of the vehicle in the previous frame, respectively. Δt is the time difference between the two frames, and Exp(·)∶R 3 →SO(3) is an exponential mapping of three-dimensional rotation (R 3 It is a three-dimensional real vector space, and SO(3) is a three-dimensional special orthogonal group. Then, using R... k-1 and t k-1 Transform the point cloud from the previous frame to the coordinate system of the current frame to obtain... If there are no points near a certain point in the current frame If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0107] The vehicle speed estimation steps are as follows: using the Doppler velocity information of the 4D millimeter-wave radar point cloud, static points are extracted and the vehicle speed is estimated.

[0108] This step specifically includes:

[0109] Step 2.1: Static Point Extraction. In addition to spatial information, the 4D millimeter-wave radar can also obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, if it is a stationary point with an absolute velocity of 0 in the real world, then its relative velocity with the radar and the radar's own velocity are equal in magnitude and opposite in direction. Furthermore, the Doppler velocity is the radial component of the relative velocity between the measured point and the 4D millimeter-wave radar. Therefore, the relationship between the Doppler velocity of this point and the radar's own velocity is as follows:

[0110] v r,i =-d i ·v s ,

[0111] Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s That's the speed of the radar.

[0112] Furthermore, since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows:

[0113]

[0114] Among them, t s and R s These represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These represent the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0115]

[0116] Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s antisymmetric matrix (R) 3×3 (Represents a set of 3×3 real matrices). Based on the relationship between the Doppler velocities of all static points in the radar point cloud and the vehicle's velocity, a random sample consensus algorithm is used to remove dynamic outliers and extract static points.

[0117] Step 2.2: Least squares estimation. After extracting the static points, the vehicle speed is estimated again using the relationship between the Doppler velocity and the vehicle speed of all static points and the least squares method.

[0118] The fusion localization steps are as follows: relative pose transformation estimation is obtained by registering point cloud and local sub-map and a pose graph is constructed. Prior localization is estimated by registering 4D millimeter-wave radar point cloud and prior map. Finally, the optimal pose is estimated by graph optimization.

[0119] The fusion localization process in this step mainly includes the following steps:

[0120] Step 3.1: Radar odometry, based on normal distribution transformation, estimates the relative transformation by direct matching of the current 4D millimeter-wave radar point cloud and keyframe sub-map.

[0121] Because 4D millimeter-wave radar point clouds are too sparse to accurately estimate attitude using only a single frame of point cloud matching, a sliding window method is used to build a denser radar sub-map using multiple keyframe point clouds. Specifically, if the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe. The new keyframe is then added to the radar sub-map, and a node associated with this keyframe is added to the pose graph. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded. The radar sub-map built using this method reflects local environmental features more clearly than a single-frame radar point cloud, improving the accuracy and robustness of point cloud registration.

[0122] After establishing the radar submap, it is uniformly divided into grids of equal size, and the point cloud in each grid is modeled as a local normal distribution. Because the 4D millimeter-wave radar point cloud is very sparse, after grid segmentation, it's easy for a grid to have fewer than three points or collinear (plane) points, making it difficult to invert the covariance of the normal distribution. Therefore, when calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is comprehensively considered. This reduces the degradation effect of the covariance and fully utilizes the point cloud information of a grid even when there are few points. Then, the current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud distribution in the submap is estimated and added to the pose map as a radar odometry factor. The error of this pose transformation estimation is e. O .

[0123] Step 3.2: Prior localization. Based on normal distribution transformation, the global localization pose is estimated by matching the current 4D millimeter-wave radar point cloud with the known prior map, and the cumulative error of the radar odometry is corrected.

[0124] The prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. The current 4D millimeter-wave radar point cloud is registered with the prior map. The pose that maximizes the probability of the current point cloud distribution in the prior map is calculated. The error of this pose estimation is e. W ;

[0125] Because the pose drift estimated solely by radar odometry is not significant over short distances, it is not necessary to perform prior localization pose calculation for every keyframe to reduce the computational load of pose fusion and point cloud registration in subsequent steps of prior localization. To balance the algorithm's accuracy and speed, prior localization is calculated every 5 keyframes.

[0126] Step 3.3: Pose fusion. A pose graph is established, considering the error terms in pose estimation from radar odometry and prior localization at all times. The optimal pose of the device at all times is estimated through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. All poses are denoted as... Constructing a maximum a posteriori problem is equivalent to solving a nonlinear least squares problem to estimate the optimal pose.

[0127]

[0128] in, Includes all moments, e O ,e w The error of pose estimation obtained at different steps.

[0129] Map update steps: Unknown areas are detected based on the overlap between the 4D millimeter-wave radar point cloud and the prior map. If no unknown areas are detected, the pose estimation results of the fusion positioning step are output. If it is determined that the device has moved into an unknown area, the estimated vehicle speed is used for pre-integration to construct a local keyframe map. After leaving the unknown area, the pose map is optimized to update the global map.

[0130] The map update process in this step mainly includes the following steps:

[0131] Step 4.1: Unknown Area Detection. This step determines whether the device has moved into an unknown area of ​​the map. When the device is moving within a known area of ​​the map, each point in the current point cloud is surrounded by numerous map points. Therefore, an anomaly score can be represented by the average distance between each point in the current point cloud and its surrounding map points. When the anomaly score exceeds a threshold δ... s When the device is considered to have entered an area unknown on the map, the known map state v=0 is set, and local mapping begins.

[0132] Similarly, anomaly scores are used to determine whether the device should return to an area where the map exists, thus ending local mapping. When the anomaly score is less than a threshold δ... s ′, and if a mapping pose exists near the current positioning pose, set the known map state v=1, end the local mapping, and complete and update the established local map onto the known prior map.

[0133] Step 4.2: Velocity pre-integration. Using the estimated vehicle velocity, the relative pose transformation is calculated, thus introducing additional reliable relative pose estimation to improve the accuracy and robustness of positioning. The estimated linear velocity and angular velocity of the vehicle at time t are denoted as... and Furthermore, the estimated value can be viewed as the true value superimposed with zero-mean Gaussian white noise, that is:

[0134]

[0135]

[0136] Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It refers to the car's orientation in a global coordinate system. That is, R t The transpose of . From this, the relative rotation transformation ΔR between time i and time j can be obtained. ij and relative translation transformation Δp ij :

[0137]

[0138]

[0139] in, J r,k yes The right Jacobian matrix, It is a vector antisymmetric matrix, and The relative rotation and translation obtained through pre-integration of the vehicle's velocity are respectively added to the pose graph as velocity pre-integration factors between nodes corresponding to times i and j. ij and δp ij These represent the corresponding noise levels. The error of this pose transformation estimation is e. V .

[0140] Step 4.3: Local Mapping. When the device enters an unknown area on the map, the pose data from radar odometry and velocity pre-integration are fused to construct a local map of the unknown area. After the device leaves the unknown area, the local map is updated and supplemented onto the global map based on prior positioning information. By establishing a pose graph, considering the error terms of relative pose estimation in velocity pre-integration and radar odometry at all times, graph optimization is used to estimate the optimal pose of the device at all times. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. All poses are denoted as... The corresponding point cloud is The known prior map is The updated map is The local map created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map is as follows:

[0141]

[0142]

[0143]

[0144] in, Includes all moments, e O ,e V ,e w The error of pose estimation obtained in different steps corresponds to v∈{0,1}, which is the known map state in step 4.1. This indicates the use of optimized pose estimation Point cloud Perform the transformation.

[0145] This invention also provides a map-based 4D millimeter-wave radar positioning system. This system can be implemented by executing the steps of the map-based 4D millimeter-wave radar positioning method. Therefore, those skilled in the art can understand the map-based 4D millimeter-wave radar positioning method as a preferred embodiment of the map-based 4D millimeter-wave radar positioning system. The system specifically includes the following:

[0146] Radar point cloud preprocessing module: Extracts ground point cloud, compares two consecutive frames of point cloud, and removes ghost points and random points under the ground.

[0147] This module specifically includes:

[0148] Module 1.1: Ghost Point Removal. Ghost points are spurious measurements below the ground in the original 4D millimeter-wave radar point cloud due to the multipath effect of millimeter waves, and are one type of noise in 4D millimeter-wave radar measurements. This is addressed by extracting the ground point cloud from the original radar point cloud and then filtering out the ghost points below the ground.

[0149] The raw radar point cloud obtained by 4D millimeter-wave radar n is the number of points in the point cloud, where the position of each point in the radar coordinate system can be represented as... Where r i ,θ i , These are the spatial information of the point obtained from 4D millimeter-wave radar measurements: range, azimuth, and elevation. For the original radar point cloud, points with ranges less than a threshold δ are first retained. r And the height is around a certain threshold δ near the radar installation height H. h Points within, that is, for all reserve And z i ∈[-H-Δ h ,-H+Δ h The points are selected because they are most likely to belong to the ground. Then, principal component analysis is used to estimate the upward normal vector n of each point. i Next, the angle between the normal vector and the positive z-axis unit vector is kept to be less than δ. n The point, i.e., n i ·(0,0,1) T ≥cosδ n Finally, the random sample consensus algorithm is used to extract the plane Ax + By + Cz + D = 0, which is considered the ground. Ghost points below the ground, i.e., Ax, are then filtered out. i +By i +Cz i +D<0 points;

[0150] Module 1.2: Random Point Removal. Random points are unstable, flickering, and spurious measurements contained in the original 4D millimeter-wave radar point cloud, and are considered part of the measurement noise in 4D millimeter-wave radar. Since random points do not appear consecutively in multiple frames of radar point cloud, they are identified and filtered out by comparing two consecutive frames of point cloud.

[0151] First, calculate the current point cloud. Point cloud in the next adjacent frame Rotational transformation between R k-1 Translation and shift transformation t k-1 :

[0152]

[0153]

[0154] in, and These are the estimated linear velocity and angular velocity of the vehicle in the previous frame, respectively. Δt is the time difference between the two frames, and Exp(·)∶R 3 →SO(3) is an exponential mapping of three-dimensional rotation (R 3 It is a three-dimensional real vector space, and SO(3) is a three-dimensional special orthogonal group. Then, using R... k-1 and t k-1 Transform the point cloud from the previous frame to the coordinate system of the current frame to obtain... If there are no points near a certain point in the current frame If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0155] Vehicle speed estimation module: Utilizes Doppler velocity information from 4D millimeter-wave radar point clouds to extract static points and estimate vehicle speed.

[0156] This module specifically includes:

[0157] Module 2.1: Static Point Extraction. In addition to spatial information, 4D millimeter-wave radar can also obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, if it is a stationary point with an absolute velocity of 0 in the real world, then its relative velocity with the radar and the radar's own velocity are equal in magnitude and opposite in direction. Furthermore, the Doppler velocity is the radial component of the relative velocity between the measured point and the 4D millimeter-wave radar. Therefore, the relationship between the Doppler velocity of this point and the radar's own velocity is as follows:

[0158] v r,i =-d i ·v s ,

[0159] Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s That's the speed of the radar.

[0160] Furthermore, since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows:

[0161]

[0162] Among them, t s and R s These represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These represent the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0163]

[0164] Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s antisymmetric matrix (R) 3×3 (Represents a set of 3×3 real matrices). Based on the relationship between the Doppler velocities of all static points in the radar point cloud and the vehicle's velocity, a random sample consensus algorithm is used to remove dynamic outliers and extract static points.

[0165] Module 2.2: Least Squares Estimation. After extracting the static points, the relationship between the Doppler velocity and the vehicle velocity of all static points is used again to estimate the vehicle velocity using the least squares method.

[0166] The fusion localization module obtains relative pose transformation estimates and constructs a pose graph by registering point clouds and local sub-maps. It also uses the registration of 4D millimeter-wave radar point clouds and prior maps to estimate prior localization. Finally, it uses graph optimization to estimate the optimal pose.

[0167] The fusion localization process in this module mainly includes the following modules:

[0168] Module 3.1: Radar Odometry, based on normal distribution transformation, estimates relative transformation by direct matching of the current 4D millimeter-wave radar point cloud and keyframe sub-map.

[0169] Because 4D millimeter-wave radar point clouds are too sparse to accurately estimate attitude using only a single frame of point cloud matching, a sliding window method is used to build a denser radar sub-map using multiple keyframe point clouds. Specifically, if the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe. The new keyframe is then added to the radar sub-map, and a node associated with this keyframe is added to the pose graph. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded. The radar sub-map built using this method reflects local environmental features more clearly than a single-frame radar point cloud, improving the accuracy and robustness of point cloud registration.

[0170] After establishing the radar submap, it is uniformly divided into grids of equal size, and the point cloud in each grid is modeled as a local normal distribution. Because the 4D millimeter-wave radar point cloud is very sparse, after grid segmentation, it's easy for a grid to have fewer than three points or collinear (plane) points, making it difficult to invert the covariance of the normal distribution. Therefore, when calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is comprehensively considered. This reduces the degradation effect of the covariance and fully utilizes the point cloud information of a grid even when there are few points. Then, the current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud distribution in the submap is estimated and added to the pose map as a radar odometry factor. The error of this pose transformation estimation is e. O .

[0171] Module 3.2: Prior localization, based on normal distribution transformation, estimates global positioning pose by matching the current 4D millimeter-wave radar point cloud with the known prior map, and corrects the cumulative error of radar odometry;

[0172] The prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. The current 4D millimeter-wave radar point cloud is registered with the prior map. The pose that maximizes the probability of the current point cloud distribution in the prior map is calculated. The error of this pose estimation is e. W ;

[0173] Because the pose drift estimated solely by radar odometry is not significant over short distances, it is not necessary to perform prior localization pose calculation for every keyframe to reduce the computational load of pose fusion and point cloud registration in subsequent modules. To balance the algorithm's accuracy and speed, prior localization is calculated every five keyframes.

[0174] Module 3.3: Pose Fusion. A pose graph is established, considering the error terms in pose estimation from radar odometry and prior localization at all times. The optimal pose of the device at all times is estimated through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. All poses are denoted as... Constructing a maximum a posteriori problem is equivalent to solving a nonlinear least squares problem to estimate the optimal pose.

[0175]

[0176] in, Includes all moments, e O ,e w The error of pose estimation obtained from different modules.

[0177] Map update module: It detects unknown areas based on the overlap between the 4D millimeter-wave radar point cloud and the prior map. If no unknown area is detected, it outputs the pose estimation result of the fusion positioning module. If it is determined that the device has moved into an unknown area, it uses the estimated vehicle speed for pre-integration to build a local keyframe map. After leaving the unknown area, it optimizes the pose map to update the global map.

[0178] The map update process in this module mainly includes the following modules:

[0179] Module 4.1: Unknown Area Detection. This module determines whether the device has moved into an unknown area on the map. When the device is moving within a known area on the map, each point in the current point cloud is surrounded by numerous map points. Therefore, an anomaly score can be represented by the average distance between each point in the current point cloud and its surrounding map points. When the anomaly score exceeds a threshold δ... s When the device is considered to have entered an area unknown on the map, the known map state ν = 0 is set, and local mapping begins.

[0180] Similarly, anomaly scores are used to determine whether the device should return to an area where the map exists, thus ending local mapping. When the anomaly score is less than a threshold δ... s ′ Furthermore, if a mapping pose exists near the current positioning pose, set the known map state ν=1, end the local mapping, and complete and update the established local map onto the known prior map.

[0181] Module 4.2: Velocity pre-integration. Using the estimated vehicle velocity, the relative pose transformation is calculated, thus introducing additional reliable relative pose estimation to improve positioning accuracy and robustness. The estimated linear velocity and angular velocity of the vehicle at time t are denoted as... and Furthermore, the estimated value can be viewed as the true value superimposed with zero-mean Gaussian white noise, that is:

[0182]

[0183]

[0184] Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It refers to the car's orientation in a global coordinate system. That is, R t The transpose of . From this, the relative rotation transformation ΔR between time i and time j can be obtained. ij and relative translation transformation Δpij :

[0185]

[0186]

[0187] in, J r,k yes The right Jacobian matrix, It is a vector antisymmetric matrix, and The relative rotation and translation obtained through pre-integration of the vehicle's velocity are respectively added to the pose graph as velocity pre-integration factors between nodes corresponding to times i and j. ij and δp ij These represent the corresponding noise levels. The error of this pose transformation estimation is e. V .

[0188] Module 4.3: Local Mapping. When the device enters an unknown map area, it integrates the pose data from radar odometry and velocity pre-integration to construct a local map of the unknown area. After the device leaves the unknown map area, the local map is updated and supplemented onto the global map based on prior positioning information. By establishing a pose graph, considering the error terms of relative pose estimation in velocity pre-integration and radar odometry at all times, graph optimization is used to estimate the optimal pose of the device at all times. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. All poses are denoted as... The corresponding point cloud is The known prior map is The updated map is The local map created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map is as follows:

[0189]

[0190]

[0191]

[0192] in, Includes all moments, e O ,e V ,e w The error of pose estimation obtained from different modules. For the known state of the map in module 4.1, This indicates the use of optimized pose estimation Point cloud Perform the transformation.

[0193] The present invention will now be described in more detail.

[0194] This invention provides a map-based 4D millimeter-wave radar positioning method, referring to... Figure 1 As shown, the method specifically includes the following:

[0195] Step 1: Radar point cloud preprocessing. The original 4D millimeter-wave radar point cloud mainly contains two types of noise: ghost points and random points. By extracting the ground point cloud and comparing two consecutive frames of point cloud, ghost points and unstable random points under the ground are removed, thereby reducing the noise in the 4D millimeter-wave radar point cloud.

[0196] Step 1.1: Ghost point removal. Ghost points are spurious measurements below the ground in the original 4D millimeter-wave radar point cloud due to the multipath effect of millimeter waves, and are one type of noise in 4D millimeter-wave radar measurements. This is achieved by extracting the ground point cloud from the original radar point cloud and then filtering out the ghost points below the ground.

[0197] The raw radar point cloud obtained by 4D millimeter-wave radar n is the number of points in the point cloud, where the position of each point in the radar coordinate system can be represented as... Where r i ,θ i , These are the spatial information of the point obtained from 4D millimeter-wave radar measurements: range, azimuth, and elevation. For the original radar point cloud, points with ranges less than a threshold δ are first retained. r And the height is around a certain threshold δ near the radar installation height H. h Points within, that is, for all reserve And z i ∈[-H-δ h ,-H+δ h The points are selected because they are most likely to belong to the ground. Then, principal component analysis is used to estimate the upward normal vector n of each point. i Next, the angle between the normal vector and the positive z-axis unit vector is kept to be less than δ. n Point, i.e., n i ·(0,0,1) T ≥cosδ n Finally, the random sample consensus algorithm is used to extract the plane Ax + By + Cz + D = 0, which is considered the ground. Ghost points below the ground, i.e., Ax, are then filtered out. i +By i+Cz i Points where +D<0.

[0198] Step 1.2: Random point removal. Random points are unstable, flickering spurious measurements contained in the original 4D millimeter-wave radar point cloud, and are one type of measurement noise in 4D millimeter-wave radar. Since random points do not appear consecutively in multiple frames of radar point cloud, they are identified and filtered out by comparing two consecutive frames of point cloud.

[0199] First, calculate the current point cloud. Point cloud in the next adjacent frame Rotational transformation between R k-1 Translation and shift transformation t k-1 :

[0200]

[0201]

[0202] in, and These are the estimated linear and angular velocities of the vehicle in the previous frame, respectively. Δt is the time difference between the two frames, and Exp(·):R 3 →SO(3) is an exponential mapping of three-dimensional rotation (R 3 It is a three-dimensional real vector space, and SO(3) is a three-dimensional special orthogonal group. Then, using R... k-1 and t k-1 Transform the point cloud from the previous frame to the coordinate system of the current frame to obtain the transformed point cloud from the previous frame. Next, using Construct a KD-tree to perform nearest neighbor search. If the current point cloud... It does not exist within a certain range at a certain point. If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0203] Step 2: Vehicle speed estimation. The 4D millimeter-wave radar can obtain the Doppler velocity of each point. The Doppler velocity is the radial component of the relative velocity between the radar and the target point. Based on this property of Doppler velocity, the linear velocity and angular velocity of the vehicle are estimated using the Doppler velocity information from the 4D millimeter-wave radar point cloud.

[0204] Step 2.1: Static Point Extraction. In addition to spatial information, the 4D millimeter-wave radar can also obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, if it is a stationary point with an absolute velocity of 0 in the real world, then its relative velocity with the radar and the radar's own velocity are equal in magnitude and opposite in direction. Furthermore, the Doppler velocity is the radial component of the relative velocity between the measured point and the 4D millimeter-wave radar. Therefore, the relationship between the Doppler velocity of this point and the radar's own velocity is as follows:

[0205] v r,i =-d i ·v s ,

[0206] Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s That's the speed of the radar.

[0207] Furthermore, since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows:

[0208]

[0209] Among them, t s and R s These represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These represent the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0210]

[0211] Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s antisymmetric matrix (R) 3×3 (Represents the set of 3×3 real matrices). Since the relationship between the Doppler velocity of dynamic points and the vehicle velocity does not conform to the above equation, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0212] Step 2.2: Least squares estimation. After extracting the static points, the vehicle speed is estimated again using the relationship between the Doppler velocity and the vehicle speed of all static points and the least squares method.

[0213] Step 3: Fusion localization. Based on normal distribution transformation, the current radar point cloud and local radar keyframe sub-maps are registered to obtain millimeter-wave radar odometry factors; based on normal distribution transformation, the current radar point cloud and known prior maps are registered to obtain prior factors; finally, the pose graph is constructed using the two factors, and the optimal pose is estimated based on graph optimization.

[0214] Step 3.1: Radar odometry, based on normal distribution transformation, estimates the relative transformation by direct matching of the current 4D millimeter-wave radar point cloud and keyframe sub-map.

[0215] Because 4D millimeter-wave radar point clouds are too sparse to accurately estimate attitude using only a single frame of point cloud matching, a sliding window method is used to build a denser radar sub-map using multiple keyframe point clouds. Specifically, if the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe. The new keyframe is then added to the radar sub-map, and a node associated with this keyframe is added to the pose graph. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded. The radar sub-map built using this method reflects local environmental features more clearly than a single-frame radar point cloud, improving the accuracy and robustness of point cloud registration.

[0216] After establishing the radar submap, it is uniformly divided into grids of equal size, and the point cloud in each grid is modeled as a local normal distribution. Because 4D millimeter-wave radar point clouds are very sparse, after grid segmentation, it's easy for a single grid to contain fewer than three points or collinear (plane) points, making it difficult to invert the covariance of the normal distribution. Therefore, when calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is comprehensively considered. This reduces the degradation effect of the covariance and fully utilizes the point cloud information of a grid even when there are few points. Then, the current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud's distribution in the submap is estimated. This is then added to the pose graph as the radar odometry factor between the nodes corresponding to times i and j. The error of this pose transformation estimation is e. O The format is as follows:

[0217]

[0218] in, The corresponding covariance matrix is ​​Log(·)∶SE(3)→R 6 It is a logarithmic mapping of a three-dimensional transformation (SE(3) is a three-dimensional special Euclidean group, R). 6 It is a six-dimensional real vector space.

[0219] Step 3.2: Prior localization. Based on normal distribution transformation, global localization is estimated by matching the current 4D millimeter-wave radar point cloud with the known prior map, and the cumulative drift of the radar odometry is corrected.

[0220] In this invention, the method for obtaining the initial pose of the device in the first frame is not considered; it is assumed that the initial pose of the device in the first frame is known. In practical applications, the device can usually obtain the initial pose of the first frame through GPS or the location of a fixed charging station.

[0221] Given the initial pose of the first frame, estimating the current global pose simplifies to point cloud registration, assuming good initial values. Specifically, the prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a locally normal distribution. The 4D millimeter-wave radar point cloud of the i-th keyframe is registered with the prior map, and the pose that maximizes the probability of the current point cloud's distribution in the prior map is calculated. This is then added as a priori factor to the pose graph. The error of this pose estimation is e. W The format is as follows:

[0222]

[0223] in, It includes all keyframes obtained from prior poses through global map registration. The corresponding covariance matrix is ​​Log(·)∶SE(3)→R 6 It is a logarithmic mapping of a three-dimensional transformation (SE(3) is a three-dimensional special Euclidean group, R). 6 It is a six-dimensional real vector space.

[0224] Because pose drift estimated solely by radar odometry is not significant over short distances, it's not necessary to perform prior localization pose calculations for every keyframe to reduce computational overhead in subsequent pose fusion and point cloud registration for prior localization. To balance accuracy and speed, prior localization is calculated every five keyframes, and then the prior factors are added to the pose map.

[0225] Step 3.3: Pose fusion. Since the prior map may not be consistent with the actual scene, in addition to using the prior map to calculate the prior pose, it is also necessary to fuse the pose estimation results of the online radar odometry to achieve more accurate estimation. By establishing a pose graph, the global pose estimated by the prior localization module and the odometry pose output by the radar odometry module are fused, and the optimal pose of the vehicle at all times is estimated using graph optimization to achieve more accurate and robust localization. All nodes in the pose correspond to the pose at different times, and the edges between nodes correspond to the relative pose transformation between different times. The pose is denoted as x=(R,p)∈SE(3), where R and p are rotation and translation, respectively. The obtained device trajectory prior is represented as Measurement of relative pose transformation between poses Record the pose at all times as The pose estimation problem can then be represented as a pose graph optimization problem with prior information, which has the following maximum a posteriori estimation form:

[0226]

[0227] Assuming that the relative pose measurement and the prior pose have Gaussian noise, the above equation can be calculated by optimizing a least squares problem of the following form.

[0228]

[0229] in, Includes all moments, e o ,e w The error of pose estimation obtained at different steps.

[0230] Step 4: Map Update. Based on the overlap between the 4D millimeter-wave radar point cloud and the prior map, unknown areas are detected to determine if the device has moved into an area unknown to the map. If no unknown area is detected, the result of Step 3 can be directly output as the final pose estimate. If an unknown area is detected, it is considered that the device has entered a region with missing maps. The global pose estimate from the prior localization is no longer reliable and is therefore no longer used to correct pose drift. Instead, odometry factors and velocity pre-integration factors are used to construct a pose map and establish a local map. After exiting the region with missing maps, prior localization needs to be performed again based on the known map. Then, the prior factors are reintegrated into the pose map, thus completing and updating the established local map into the original global map.

[0231] Step 4.1: Unknown Area Detection – Determine if the device has moved into an unknown area of ​​the map. When the device is moving within a known area of ​​the map, the overlap between the current point cloud and the map point cloud is high, meaning that each point in the current point cloud is surrounded by numerous map points. However, when the device moves outside the map's boundaries, the overlap between the current point cloud and the map point cloud decreases significantly. Therefore, anomaly scores can be represented based on the distance between each point in the current point cloud and its surrounding map points.

[0232] Specifically, first, record the map point cloud as... Construct a KD tree using the map point cloud, and then for the current point cloud... Each point P in i Search for the n nearest map points and calculate the average distance d between the current point and the n nearest map points. i The final calculated anomaly score, socre, is as follows:

[0233]

[0234] Where N is the current number of point clouds, and D = {di}, i∈{1,2,...,N}, therefore ||D<δ d ||0 represents all average distances less than δ d The number of points. Therefore, the smaller the average distance between each point and its surrounding map points, and the average distance is less than δ... d The more points there are, the lower the anomaly score. When the anomaly score exceeds a certain threshold δ... s When the map is set to known state v=0, it is considered that the device has entered an unknown area of ​​the map and local mapping begins.

[0235] Furthermore, when the device returns to the area where the map exists, local mapping needs to end and the map needs to be completed and updated. Therefore, it is also necessary to determine when to end local mapping. Similarly, anomaly scores are used for this determination, but to prevent premature termination of local mapping, a pose offset check is added. Specifically, firstly, all poses of the known map mapping trajectory are put into a KD-tree. Then, during local mapping, the distance d between the current pose and the nearest pose node is searched in the KD-tree. x If the abnormal score is less than a certain threshold δ s ′, and when the distance d x Less than a certain threshold δ x At this time, the known map state v=1 is set, indicating that the device has returned to the area where the map exists. To improve the robustness of the abnormal termination judgment, δ needs to be set. s <δ s ′, to prevent premature termination.

[0236] Step 4.2: Velocity pre-integration. Using the estimated vehicle velocity, the relative pose transformation is calculated, thus introducing additional reliable relative pose estimation to improve the accuracy and robustness of positioning. The estimated linear velocity and angular velocity of the vehicle at time t are denoted as... and Furthermore, the estimated value can be viewed as the true value superimposed with zero-mean Gaussian white noise, that is:

[0237]

[0238]

[0239] Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It refers to the car's orientation in a global coordinate system. That is, R t The transpose of . Assume that within a very short time interval [t, t+Δt], ω t and v tIf it remains unchanged, then the position p at time t+Δt t+Δt and rotation R t+Δt The position p at time t can be used t and rotation R t The calculation yielded:

[0240]

[0241]

[0242] For the point cloud of each frame between two consecutive keyframes at times i and j, a vehicle speed estimate can be obtained. Therefore, by integrating over all time intervals Δt between times i and j, the vehicle speed estimate at position p at time j can be obtained. j and rotation R j The position p at time i can be used i and rotation R i The calculation yielded:

[0243]

[0244]

[0245] Therefore, the relative rotation transformation ΔR between time i and time j can be obtained. ij and relative translation transformation Δp ij :

[0246]

[0247]

[0248] in, J r,k yes The right Jacobian matrix, It is a vector antisymmetric matrix, and The relative rotation and translation, obtained through pre-integration of the vehicle's velocity, are added to the pose graph as velocity pre-integration factors between nodes corresponding to times i and j. δφ ij and δp ij These are the corresponding noises, in the form of:

[0249]

[0250]

[0251] Due to δφ ij It is zero-mean Gaussian noise A linear combination, therefore δφ ijIt is zero-mean Gaussian noise. Similarly, δp ij It is zero-mean Gaussian noise φ ik and A linear combination of these, therefore also zero-mean Gaussian noise. Let Gaussian noise be... The error of this pose transformation estimation is:

[0252]

[0253]

[0254] Wherein, Log(·)∶SO(3)→R 3 It is a logarithmic mapping of three-dimensional rotation.

[0255] Step 4.3: Local Mapping. When the robot enters an unknown area of ​​the map, the poses obtained from odometry and velocity pre-integration are fused to construct a local map, thus establishing a local map of the unknown area in the original global map. After the robot leaves the unknown area, the local map is updated and supplemented onto the global map based on prior localization information. By establishing a pose graph, considering the error terms of velocity pre-integration and relative pose estimation in radar odometry at all times, graph optimization is used to estimate the optimal pose of the device at all times. All nodes in the pose graph correspond to the poses at different times, and the edges between nodes correspond to the relative pose transformations between different times. All poses are denoted as... The corresponding point cloud is The known prior map is The updated map is The local map created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map has the following format:

[0256]

[0257]

[0258]

[0259] in, Includes all moments, e O ,e V ,e w The error of pose estimation obtained in different steps corresponds to v∈{0,1}, which represents the known map state in step 4.1, indicating whether prior factors should be used in pose graph optimization. This indicates the use of optimized pose estimation Point cloud A transformation is performed. When the device is detected to have entered an unknown area of ​​the map, v = 0 is set to avoid the influence of incorrect prior positioning results in the unknown area on the local mapping. After the local mapping is completed, v = 1 is set to complete the local map onto the global map using the prior pose, so as to obtain an updated global map with global consistency.

[0260] This invention provides a map-based 4D millimeter-wave radar positioning method and system. To achieve better accuracy in sparse point cloud conditions, the estimated vehicle speed is pre-integrated to obtain additional relative pose estimation. This invention introduces map updating. Since the scope of a map is usually limited, when the device reaches an area unknown on the map, it is necessary to maintain positioning accuracy and to complete and update the map when returning to a known area. This invention can utilize 4D millimeter-wave radar to perform accurate and robust positioning and map updating based on a priori maps.

[0261] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0262] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A map-based 4D millimeter-wave radar positioning method, characterized in that, include: Radar point cloud preprocessing steps: Extract ground point cloud, compare two consecutive point cloud frames, and remove interference points below the ground. The vehicle speed estimation steps are as follows: using the Doppler velocity information of the 4D millimeter-wave radar point cloud, extract static points and estimate the vehicle speed; The fusion localization steps are as follows: relative pose transformation is estimated by registration of point cloud and local sub-map and pose graph is constructed. Prior localization is estimated by registration of 4D millimeter-wave radar point cloud and prior map. Finally, the optimal pose is estimated by graph optimization. Map update steps: Unknown areas are detected based on the overlap between the 4D millimeter-wave radar point cloud and the prior map. If no unknown areas are detected, the pose estimation results of the fusion positioning step are output. If it is determined that the device has moved into an unknown area, the estimated vehicle speed is used for pre-integration to construct a local keyframe map. After leaving the unknown area, the pose map is optimized to update the global map.

2. The map-based 4D millimeter-wave radar positioning method according to claim 1, characterized in that, The radar point cloud preprocessing step includes: removing interference points under the ground, wherein the interference points include ghost points and random points; Ground point clouds are extracted from the original radar point clouds, and ghost points below the ground are filtered out; random points are identified and filtered out by comparing point clouds from two consecutive frames.

3. The map-based 4D millimeter-wave radar positioning method according to claim 1, characterized in that, The vehicle speed estimation step includes: Step S2.1: Static point extraction; The 4D millimeter-wave radar can obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows: v r,i =-d i ·v s Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s It is the speed of the radar; Since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows: Among them, t s and R s These represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These are the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation: Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s The antisymmetric matrix, where R 3×3 This represents the set of 3×3 real matrices. Since the relationship between the Doppler velocity of dynamic points and the vehicle velocity does not conform to the above equation, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points. Step S2.2: Least squares estimation. After extracting the static points, the vehicle speed is estimated using the least squares method by utilizing the relationship between the Doppler velocity and the vehicle speed of all static points.

4. The map-based 4D millimeter-wave radar positioning method according to claim 1, characterized in that, The fusion positioning step includes: Step S3.1: Radar odometry, based on normal distribution transformation, estimates the relative transformation pose between keyframes by matching the current 4D millimeter-wave radar point cloud and keyframe sub-map; A radar sub-map is built using a sliding window with multiple keyframe point clouds. If the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe and added to the radar sub-map. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded. After establishing the radar submap, it is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. When calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is considered. The current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud distribution in the submap is calculated. The error of this pose transformation estimation is e. O ; Step S3.2: Prior localization. Based on normal distribution transformation, the global localization pose is estimated by matching the current 4D millimeter-wave radar point cloud with the known prior map, and the cumulative error of the radar odometry is corrected. The prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. The current 4D millimeter-wave radar point cloud is registered with the prior map. The pose that maximizes the probability of the current point cloud distribution in the prior map is calculated. The error of this pose estimation is e. W ; To balance the accuracy and speed of the algorithm, prior localization is calculated every 5 keyframes; Step S3.3: Pose fusion. Establish a pose graph, considering the error terms in pose estimation from radar odometry and prior localization at all times. Estimate the optimal pose of the device at all times through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. Record all poses as... Constructing a maximum a posteriori problem is equivalent to solving a nonlinear least squares problem to estimate the optimal pose. in, Includes all moments, e O ,e w The error of pose estimation obtained at different steps.

5. The map-based 4D millimeter-wave radar positioning method according to claim 1, characterized in that, The map update steps include: Step S4.1: Unknown area detection. Determine whether the device has moved into an unknown area of ​​the map. When the device is moving in a known area of ​​the map, each point in the current point cloud is surrounded by many map points. Therefore, the anomaly score is represented by the average distance between each point in the current point cloud and the surrounding map points. When the anomaly score is greater than the threshold δ, the anomaly score is determined. s When the device is considered to have entered an area unknown on the map, the known map state ν = 0 is set, and local mapping begins. Similarly, anomaly scores are used to determine whether the device should return to an area where the map exists, thus ending local mapping; when the anomaly score is less than the threshold δ′ s Furthermore, if a mapping pose exists near the current positioning pose, set the known map state v=1, end the local mapping, and complete and update the established local map onto the known prior map. Step S4.2: Velocity pre-integration. Using the estimated equipment velocity, calculate the relative pose transformation, introducing additional reliable relative pose estimation. The estimated linear and angular velocities of the equipment at time t are denoted as... and The estimated value is the true value superimposed with zero-mean Gaussian white noise, i.e.: Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It is the orientation of the device in the world coordinate system. That is, R t The transpose of the equation; the relative rotation transformation ΔR between time i and time j is obtained by integration. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e V ; Step S4.3: Local graph construction. Establish a pose graph, considering the velocity pre-integration at all times and the error term in the relative pose estimation from the radar odometer. Estimate the optimal pose of the device at all times through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. Record all poses as... The corresponding point cloud is The known prior map is The updated map is The local map that was created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map is as follows: in, Includes all moments, e O ,e v ,e w The error of pose estimation obtained in different steps corresponds to v∈{0,1}, which is the known map state in step S4.

1. This indicates the use of optimized pose estimation Point cloud Perform the transformation.

6. A map-based 4D millimeter-wave radar positioning system, characterized in that, include: Radar point cloud preprocessing module: Extracts ground point cloud, compares two consecutive frames of point cloud, and removes interference points below the ground; Vehicle speed estimation module: Utilizes Doppler velocity information from 4D millimeter-wave radar point clouds to extract static points and estimate vehicle speed; Fusion localization module: It obtains relative pose transformation estimation and constructs pose graph by registering point cloud and local sub-map, and estimates prior localization by registering 4D millimeter wave radar point cloud and prior map. Finally, it estimates the optimal pose by graph optimization. Map update module: It detects unknown areas based on the overlap between the 4D millimeter-wave radar point cloud and the prior map. If no unknown area is detected, it outputs the pose estimation result of the fusion positioning module. If it is determined that the device has moved into an unknown area, it uses the estimated vehicle speed for pre-integration to build a local keyframe map. After leaving the unknown area, it optimizes the pose map to update the global map.

7. The map-based 4D millimeter-wave radar positioning system according to claim 6, characterized in that, The radar point cloud preprocessing module includes: removing interference points below the ground, wherein the interference points include ghost points and random points; Ground point clouds are extracted from the original radar point clouds, and ghost points below the ground are filtered out; random points are identified and filtered out by comparing point clouds from two consecutive frames.

8. The map-based 4D millimeter-wave radar positioning system according to claim 6, characterized in that, The vehicle speed estimation module includes: Module M2.1: Static Point Extraction; 4D millimeter-wave radar can obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D millimeter-wave radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows: v r,i =-d i ·v s Among them, v r,i It is the Doppler velocity at the i-th point. It is the unit direction vector of that point relative to the radar, v s It is the speed of the radar; Since the 4D millimeter-wave radar is installed on the car, the relationship between the radar speed and the car's own speed is as follows: Among them, t s and R s These represent the installation position and orientation of the 4D millimeter-wave radar in the vehicle coordinate system. and These are the vehicle's linear velocity and angular velocity, respectively. In the real world, most points in a 4D millimeter-wave radar point cloud are static points. Therefore, for all static points in a 4D millimeter-wave radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation: Where m is the number of all static points in the point cloud, I 3×3 It is a 3×3 identity matrix. It is t s The antisymmetric matrix, where R 3×3 This represents the set of 3×3 real matrices. Since the relationship between the Doppler velocity of dynamic points and the vehicle velocity does not conform to the above equation, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points. Module M2.2: Least Squares Estimation. After extracting the static points, the vehicle speed is estimated using the least squares method by utilizing the relationship between the Doppler velocity of all static points and the vehicle speed.

9. The map-based 4D millimeter-wave radar positioning system according to claim 6, characterized in that, The fusion positioning module includes: Module M3.1: Radar odometry, based on normal distribution transformation, estimates the relative transformation pose between keyframes by matching the current 4D millimeter-wave radar point cloud and keyframe sub-map; A radar sub-map is built using a sliding window with multiple keyframe point clouds. If the translation or rotation from the latest keyframe to the current point cloud exceeds a threshold, the current frame is selected as a new keyframe and added to the radar sub-map. When the number of keyframes in the sub-map exceeds the window size, the oldest keyframe is discarded. After establishing the radar submap, it is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. When calculating the mean and covariance of the normal distribution, the measurement uncertainty of each point is considered. The current 4D millimeter-wave radar point cloud is registered with the radar submap, and the relative pose transformation that maximizes the probability of the current point cloud distribution in the submap is calculated. The error of this pose transformation estimation is e. O ; Module M3.2: Prior localization, based on normal distribution transformation, estimates global positioning pose by matching the current 4D millimeter-wave radar point cloud with the known prior map, and corrects the cumulative error of radar odometry; The prior map is uniformly divided into grids of equal size. The point cloud in each grid is modeled as a local normal distribution. The current 4D millimeter-wave radar point cloud is registered with the prior map. The pose that maximizes the probability of the current point cloud distribution in the prior map is calculated. The error of this pose estimation is e. W ; To balance the accuracy and speed of the algorithm, prior localization is calculated every 5 keyframes; Module M3.3: Pose Fusion. A pose graph is established, considering the error terms in pose estimation from radar odometry and prior localization at all times. The optimal pose of the device at all times is estimated through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times. All poses are denoted as... Constructing a maximum a posteriori problem is equivalent to solving a nonlinear least squares problem to estimate the optimal pose. in, Includes all moments, e O ,e w The error of pose estimation obtained from different modules.

10. The map-based 4D millimeter-wave radar positioning system according to claim 6, characterized in that, The map update module includes: Module M4.1: Unknown Area Detection. This module determines whether the device has moved into an unknown area on the map. When the device is traveling in a known area on the map, each point in the current point cloud is surrounded by numerous map points. Therefore, an anomaly score is represented by the average distance between each point in the current point cloud and its surrounding map points. When the anomaly score exceeds a threshold δ... s When the device is considered to have entered an area unknown on the map, the known map state v=0 is set, and local mapping begins. Similarly, anomaly scores are used to determine whether the device should return to an area where the map exists, thus ending local mapping; when the anomaly score is less than a threshold δ... s ′ Furthermore, if a mapping pose exists near the current positioning pose, set the known map state v=1, end the local mapping, and complete and update the established local map onto the known prior map. Module M4.2: Velocity pre-integration. Using the estimated equipment velocity, the relative pose transformation is calculated, introducing additional reliable relative pose estimation. The estimated linear and angular velocities of the equipment at time t are denoted as... and The estimated value is the true value superimposed with zero-mean Gaussian white noise, i.e.: Where, ω t and v t These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term, R t It is the orientation of the device in the world coordinate system. That is, R t The transpose of the equation; the relative rotation transformation ΔR between time i and time j is obtained by integration. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e V ; Module M4.3: Local graph construction, establishing a pose graph, considering the velocity pre-integration at all times and the error term of relative pose estimation in radar odometer, and estimating the optimal pose of the device at all times through graph optimization. All nodes in the pose graph correspond to poses at different times, and the edges between nodes correspond to relative pose transformations between different times; all poses are denoted as... The corresponding point cloud is The known prior map is The updated map is The local map that was created is The optimal pose is estimated by solving a nonlinear least squares problem. The updated map is as follows: in, Includes all moments, e O ,e V ,e w The pose estimation errors obtained from different modules are represented by v∈{0,1}, which represents the known map state in module M4.

1. This indicates the use of optimized pose estimation Point cloud Perform the transformation.

Images