Pose graph slam calculation method and system based on 4d millimeter wave radar

Abstract

The application provides a 4D millimeter wave radar-based pose graph SLAM calculation method and system, comprising the following steps: S1, ground point cloud is extracted, two continuous frames of point clouds are compared, ghost points and random points under the ground are removed; S2, the linear velocity and angular velocity of the device are estimated by using the Doppler velocity information of the 4D radar point cloud; S3, the pose graph is estimated and constructed by using the relative pose transformation, the point cloud registration based on the normal distribution transformation, the pre-integration is performed by using the estimated device velocity, the loop detection is performed, and the optimal pose is estimated by using the graph optimization. In order to achieve better accuracy in the case of sparse point cloud, the pre-integration is performed on the estimated self-vehicle velocity, additional relative pose estimation is obtained, the loop detection is introduced, the places that are reached again are identified, the historical poses are optimized from the global perspective, the cumulative drift is reduced, and accurate and robust mapping and positioning are realized.

Description

Technical Field

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

[0002] Millimeter-wave radar has been widely used in Simultaneous Localization and Mapping (SLAM) technology, especially in the field of autonomous driving. Compared with optical sensors such as cameras or LiDAR, millimeter-wave radar is less sensitive to weather and lighting conditions and is much cheaper. However, despite these advantages, millimeter-wave radar point clouds are generally sparser and noisier than LiDAR, which poses challenges to accurate localization and mapping. In recent years, the latest millimeter-wave radar sensors, namely 4D imaging radar (4D radar), have received increasing attention in autonomous driving due to their unique advantages over traditional radar.

[0003] Traditional millimeter-wave radar can be divided into two types: scanning radar and automotive radar. Scanning radar performs a 360-degree scan of the surrounding environment, providing a two-dimensional radar energy image without velocity information. On the other hand, automotive radar typically has a smaller field of view and provides a sparse 2D point cloud with Doppler velocity. Compared to traditional millimeter-wave radar sensors, 4D radar has a longer detection range and a larger field of view. The 4D point cloud provided by 4D radar has information in four dimensions: range, azimuth, elevation, and Doppler velocity. In addition, 4D radar also provides some low-level features, such as energy intensity or radar cross section. Because of the additional information and higher resolution, 4D radar offers new opportunities for SLAM applications.

[0004] Researchers both domestically and internationally have conducted extensive studies on the issue of SLAM using millimeter-wave radar. Based on the type of millimeter-wave radar used, these studies can be broadly categorized into two types:

[0005] One type is the scanning radar-based SLAM algorithm, which dominates millimeter-wave radar SLAM. This type of algorithm uses 2D images obtained by scanning radar as input, and usually uses some feature extraction algorithms to extract feature points from the image, and then performs scan registration based on feature points. The most famous one is the RadarSLAM algorithm proposed by Hong et al. [1] (Z.Hong, Y.Petillot, and S.Wang, “RadarSLAM: Radar based Large-Scale SLAM in All Weathers,” in 2020 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2020, pp. 5164–5170, iSSN: 2153-086). They were inspired by visual SLAM related methods, extracting SURF feature points from radar images and performing registration. In addition, there are some methods that use deep learning to implement scanning radar SLAM. However, these methods are not suitable for 4D radar point clouds because they use 2D scanning radar images as input.

[0006] Second, SLAM algorithms based on automotive radar. These algorithms use sparse radar point clouds obtained by traditional automotive radar as input and estimate the vehicle motion by using the Doppler velocity information of the point cloud or by using 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 speed, thereby realizing the vehicle motion estimation. Kung et al. proposed a radar odometer method suitable for scanning radar and automotive radar [3] (P.-C.Kung, C.-C.Wang, and W.-C.Lin, “A Normal Distribution Transform-Based Radar Odometry Designed For Scanning and Automotive Radars,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). Xi'an, China: IEEE, May 2021, pp.14 417–14 423.). They utilized the vehicle speed estimation method proposed by Kellner et al. to merge multiple radar point clouds into a single radar sub-map. Then, they applied scan matching based on normal distribution transformation to match the two radar sub-maps, thereby estimating the vehicle's motion. However, the radar sub-map is simply a superposition of continuous radar point clouds using the vehicle speed estimation, so its accuracy largely depends on the accuracy of the vehicle speed estimation.

[0007] Patent document CN111522043A discloses a method for rapid re-matching and localization of LiDAR for unmanned vehicles, belonging to the field of autonomous driving. This invention consists of three modules: multi-sensor calibration, pose fusion, and localization fusion. Through joint multi-sensor calibration, a consistent description of the target is obtained; the pose information calculated by the GPS and LiDAR sensors for the unmanned vehicle is fused to obtain the vehicle pose information fused from the GPS and LiDAR sensors; when the LiDAR SLAM localization module fails to match the point cloud, the fused pose is used to replace the localization prediction matrix of the point cloud matching algorithm, achieving rapid re-matching of the LiDAR SLAM algorithm and continuous localization of the unmanned vehicle using the SLAM algorithm. However, this invention does not introduce loop closure detection to identify revisited locations and cannot optimize historical poses from a global perspective. Summary of the Invention

[0008] In view of the deficiencies in the prior art, the purpose of this invention is to provide a pose graph SLAM calculation method and system based on 4D millimeter-wave radar.

[0009] A pose graph SLAM calculation method based on 4D millimeter-wave radar according to the present invention includes:

[0010] Step S1: Extract the ground point cloud, compare two consecutive frames of point clouds, and remove ghost points and random points under the ground;

[0011] Step S2: Using the Doppler velocity information of the 4D radar point cloud, estimate the linear velocity and angular velocity of the device;

[0012] Step S3: Estimate and construct a pose graph using relative pose transformation, register the point cloud based on normal distribution transformation, perform pre-integration using the estimated device velocity, perform loop closure detection, and estimate the optimal pose using graph optimization.

[0013] Preferably, in step S1:

[0014] By extracting ground point clouds and comparing two consecutive frames of point clouds, ghost points and unstable random points under the ground are removed, reducing noise in 4D radar point clouds.

[0015] Step S1.1: Ghost point removal. Extract the ground point cloud from the original radar point cloud and filter out ghost points below the ground.

[0016] For the raw radar point cloud of 4D radar measurement n is the number of points in the point cloud;

[0017] The position of each point in the radar coordinate system is represented as follows: Where r i θ is the distance to this point measured by 4D radar. iThe azimuth angle of this point is obtained from 4D radar measurement. The elevation angle of this point is obtained from 4D radar measurement;

[0018] For the original radar point cloud, the retention distance is less than the threshold δ. r And the height is near the radar installation height threshold δ h For points within the z-axis, principal component analysis is used to calculate the upward normal vector for each point, retaining those points where the angle between the normal vector and the positive z-axis unit vector is less than δ. n The points are extracted using a random sampling consensus algorithm, and ghost points below the ground are filtered out.

[0019] Step S1.2: Random point removal. Random points are identified and filtered out by comparing the point clouds of the previous and next frames. For the current point cloud and the immediately preceding point cloud, the pose transformation of the point clouds of the previous and next frames is calculated based on the device speed. The point cloud of the previous frame is transformed into the coordinate system of the current frame. If there is no transformed point cloud of the previous frame within a preset range near a certain point in the current frame, then the point is classified as a random point and filtered out.

[0020] 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 :

[0021]

[0022]

[0023] in, and These are the device linear velocity and angular velocity estimates for the previous frame, respectively, where Δ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, SO(3) is a three-dimensional special orthogonal group, 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 a point in the current frame does not exist within the preset range If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0024] Preferably, in step S2:

[0025] Step S2.1: Static point extraction. For the i-th point in the 4D radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows:

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

[0027] 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;

[0028] The relationship between radar speed and equipment speed is as follows:

[0029]

[0030] Among them, t s and R s These represent the installation position and attitude of the 4D radar in the equipment coordinate system. and Let be the linear velocity and angular velocity of the device, respectively. For all static points in the 4D radar point cloud, the Doppler velocity and the device velocity conform to the following equation:

[0031]

[0032] 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, R 3×3 Represents a set of 3×3 real matrices. Based on the relationship between the Doppler velocity of all static points in the radar point cloud and the equipment velocity, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0033] Step S2.2: Least squares estimation. After extracting the static points, the equipment speed is calculated using the least squares method based on the relationship between the Doppler velocity of all static points and the equipment speed.

[0034] Preferably, in step S3:

[0035] Pose graph optimization utilizes point cloud registration, velocity pre-integration, and loop closure detection to obtain relative pose transformation estimates and construct a pose graph. Finally, graph optimization is used to estimate the optimal pose.

[0036] Step S3.1: Point cloud registration, based on normal distribution transformation, the relative transformation is estimated by matching the current 4D radar point cloud and the key frame sub-map;

[0037] A sliding window is used to build a radar sub-map using 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 earliest keyframe is discarded.

[0038] 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 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 ;

[0039] Step S3.2: Velocity pre-integration. Using the estimated equipment velocity, calculate the relative pose transformation, introducing additional reliable relative pose estimation. The estimated linear velocity and angular velocity 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.:

[0040]

[0041]

[0042] Where, ω t and W 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 The orientation of the device in the world coordinate system is given by integration, which yields the relative rotation transformation ΔR between time i and time j. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e. V .

[0043] Preferably, step S3.3: Loop closure detection, identifying previously visited locations, dividing the 4D radar point cloud into a grid on polar coordinates, and mapping the 3D point cloud into a 2D matrix, where each element of the matrix represents the maximum energy intensity of the radar point in the corresponding grid. As the device moves, the cosine distance between the 2D matrix of the current frame and the 2D matrices generated from all previous keyframes is continuously searched and calculated. If the distance is less than a set threshold, a loop closure is considered detected. When a loop closure is detected, the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes is calculated. The error of this pose transformation estimation is e. L ;

[0044] Step S3.4: Graph optimization. Establish a pose graph, considering the error term of relative pose estimation 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... The optimal pose is estimated by solving a nonlinear least squares problem.

[0045]

[0046] in, Includes all moments, e O ,e V ,e L The error of the relative pose transformation obtained at different steps.

[0047] A pose graph SLAM calculation system based on 4D millimeter-wave radar, provided by the present invention, includes:

[0048] Module M1: Extracts ground point cloud, compares two consecutive frames of point cloud, and removes ghost points and random points under the ground;

[0049] Module M2: Utilizes Doppler velocity information from 4D radar point clouds to estimate the linear velocity and angular velocity of the device;

[0050] Module M3: Estimates and constructs a pose graph using relative pose transformation, performs point cloud registration based on normal distribution transformation, pre-integrates using estimated device velocity, performs loop closure detection, and estimates the optimal pose using graph optimization.

[0051] Preferably, in module M1:

[0052] By extracting ground point clouds and comparing two consecutive frames of point clouds, ghost points and unstable random points under the ground are removed, reducing noise in 4D radar point clouds.

[0053] Module M1.1: Ghost point removal, extracts the ground point cloud from the original radar point cloud and filters out ghost points below the ground;

[0054] For the raw radar point cloud of 4D radar measurement n is the number of points in the point cloud;

[0055] The position of each point in the radar coordinate system is represented as follows: Where r i θ is the distance to this point measured by 4D radar. i The azimuth angle of this point is obtained from 4D radar measurement. The elevation angle of this point is obtained from 4D radar measurement;

[0056] For the original radar point cloud, the retention distance is less than the threshold δ. r And the height is near the radar installation height threshold δ h For points within the z-axis, principal component analysis is used to calculate the upward normal vector for each point, retaining those points where the angle between the normal vector and the positive z-axis unit vector is less than δ. n The points are extracted using a random sampling consensus algorithm, and ghost points below the ground are filtered out.

[0057] Module M1.2: Random Point Removal. This module identifies and filters random points by comparing the point clouds of two consecutive frames. For the current point cloud and the immediately preceding point cloud, the pose transformation between the two frames is calculated based on the device speed. The previous frame point cloud is then transformed to the coordinate system of the current frame. If a point in the current frame does not exist within a preset range after the transformation of the previous frame point cloud, that point is classified as a random point and filtered out.

[0058] 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 :

[0059]

[0060]

[0061] in, and These are the device linear velocity and angular velocity estimates for the previous frame, respectively, where Δ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, SO(3) is a three-dimensional special orthogonal group, 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 a point in the current frame does not exist within the preset range If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0062] Preferably, in module M2:

[0063] Module M2.1: Static point extraction. For the i-th point in the 4D radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows:

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

[0065] 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;

[0066] The relationship between radar speed and equipment speed is as follows:

[0067]

[0068] Among them, t s and R s These represent the installation position and attitude of the 4D radar in the equipment coordinate system. and Let be the linear velocity and angular velocity of the device, respectively. For all static points in the 4D radar point cloud, the Doppler velocity and the device velocity conform to the following equation:

[0069]

[0070] 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, R 3×3 Represents a set of 3×3 real matrices. Based on the relationship between the Doppler velocity of all static points in the radar point cloud and the equipment velocity, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0071] Module M2.2: Least squares estimation. After extracting the static points, the device speed is calculated using the least squares method based on the relationship between the Doppler velocity of all static points and the device speed.

[0072] Preferably, in module M3:

[0073] Pose graph optimization utilizes point cloud registration, velocity pre-integration, and loop closure detection to obtain relative pose transformation estimates and construct a pose graph. Finally, graph optimization is used to estimate the optimal pose.

[0074] Module M3.1: Point cloud registration, based on normal distribution transformation, estimates the relative transformation by matching the current 4D radar point cloud and keyframe sub-map;

[0075] A sliding window is used to build a radar sub-map using 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 earliest keyframe is discarded.

[0076] 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 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 ;

[0077] Module M3.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.:

[0078]

[0079]

[0080] Where, ω t and W 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 The orientation of the device in the world coordinate system is given by integration, which yields the relative rotation transformation ΔR between time i and time j. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e. V .

[0081] Preferably, module M3.3: Loop closure detection identifies previously visited locations. In polar coordinates, the 4D radar point cloud is divided into a grid, and the 3D point cloud is mapped into a 2D matrix. The value of each element in the matrix is ​​the maximum energy intensity of the radar point in the corresponding grid. As the device moves, the cosine distance between the 2D matrix of the current frame and the 2D matrices generated from all previous keyframes is continuously searched and calculated. If the distance is less than a set threshold, a loop closure is considered detected. When a loop closure is detected, the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes is calculated. The error of this pose transformation estimation is e. L ;

[0082] Module M3.4: Graph Optimization. A pose graph is built, considering the error term of relative pose estimation 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... The optimal pose is estimated by solving a nonlinear least squares problem.

[0083]

[0084] in, Includes all moments, e O ,e V ,e L The error of the relative pose transformation obtained at different steps.

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

[0086] 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.

[0087] 2. This invention introduces loop closure detection to identify places that have been revisited, and optimizes historical poses from a global perspective to reduce cumulative drift and achieve accurate and robust mapping and localization;

[0088] 3. This invention can utilize 4D millimeter-wave radar to perform accurate and robust synchronous positioning and mapping in unknown environments. Attached Figure Description

[0089] 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:

[0090] Figure 1 This is a schematic diagram of the method flow of the present invention. Detailed Implementation

[0091] 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.

[0092] Example 1:

[0093] Based on previous work, this invention innovatively proposes a pose graph SLAM algorithm based on 4D millimeter-wave radar. This algorithm can be applied to manned intelligent wheelchairs, autonomous vehicles, etc., to achieve accurate and robust synchronous localization and mapping tasks.

[0094] This invention provides a pose graph SLAM algorithm based on 4D millimeter-wave radar. The algorithm comprises three modules: radar point cloud filtering, vehicle velocity estimation, and pose graph optimization. To address the issue of high noise in 4D millimeter-wave radar (4D radar) point clouds, a filtering step is designed to reduce ghosting points and random noise in the original 4D radar point cloud. Then, the vehicle velocity is estimated from the Doppler velocity of the filtered point cloud, which plays a crucial role in the subsequent pose graph optimization. In pose graph optimization, radar odometry is achieved through point cloud registration based on normal distribution transformation to estimate the relative pose transformation. To achieve better accuracy in sparse point cloud conditions, the estimated vehicle velocity is pre-integrated to obtain additional relative pose estimation. Furthermore, loop closure detection is introduced to identify revisited locations, optimizing historical poses from a global perspective to reduce cumulative drift and achieve accurate and robust mapping and localization.

[0095] According to the present invention, a pose graph SLAM calculation method based on 4D millimeter-wave radar is provided, such as... Figure 1 As shown, it includes:

[0096] Step S1: Extract the ground point cloud, compare two consecutive frames of point clouds, and remove ghost points and random points under the ground;

[0097] Specifically, in step S1:

[0098] By extracting ground point clouds and comparing two consecutive frames of point clouds, ghost points and unstable random points under the ground are removed, reducing noise in 4D radar point clouds.

[0099] Step S1.1: Ghost point removal. Extract the ground point cloud from the original radar point cloud and filter out ghost points below the ground.

[0100] For the raw radar point cloud of 4D radar measurement n is the number of points in the point cloud;

[0101] The position of each point in the radar coordinate system is represented as follows: Where r i θ is the distance to this point measured by 4D radar. i The azimuth angle of this point is obtained from 4D radar measurement. The elevation angle of this point is obtained from 4D radar measurement;

[0102] For the original radar point cloud, the retention distance is less than the threshold δ. r And the height is near the radar installation height threshold δ h For points within the z-axis, principal component analysis is used to calculate the upward normal vector for each point, retaining those points where the angle between the normal vector and the positive z-axis unit vector is less than δ. nThe points are extracted using a random sampling consensus algorithm, and ghost points below the ground are filtered out.

[0103] Step S1.2: Random point removal. Random points are identified and filtered out by comparing the point clouds of the previous and next frames. For the current point cloud and the immediately preceding point cloud, the pose transformation of the point clouds of the previous and next frames is calculated based on the device speed. The point cloud of the previous frame is transformed into the coordinate system of the current frame. If there is no transformed point cloud of the previous frame within a preset range near a certain point in the current frame, then the point is classified as a random point and filtered out.

[0104] 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 :

[0105]

[0106]

[0107] in, and These are the device linear velocity and angular velocity estimates for the previous frame, respectively, where Δ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, SO(3) is a three-dimensional special orthogonal group, 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 a point in the current frame does not exist within the preset range If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0108] Step S2: Using the Doppler velocity information of the 4D radar point cloud, estimate the linear velocity and angular velocity of the device;

[0109] Specifically, in step S2:

[0110] Step S2.1: Static point extraction. For the i-th point in the 4D radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows:

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

[0112] 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 sIt is the speed of the radar;

[0113] The relationship between radar speed and equipment speed is as follows:

[0114]

[0115] Among them, t s and R s These represent the installation position and attitude of the 4D radar in the equipment coordinate system. and Let be the linear velocity and angular velocity of the device, respectively. For all static points in the 4D radar point cloud, the Doppler velocity and the device velocity conform to the following equation:

[0116]

[0117] 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, R 3×3 Represents a set of 3×3 real matrices. Based on the relationship between the Doppler velocity of all static points in the radar point cloud and the equipment velocity, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0118] Step S2.2: Least squares estimation. After extracting the static points, the equipment speed is calculated using the least squares method based on the relationship between the Doppler velocity of all static points and the equipment speed.

[0119] Step S3: Estimate and construct a pose graph using relative pose transformation, register the point cloud based on normal distribution transformation, perform pre-integration using the estimated device velocity, perform loop closure detection, and estimate the optimal pose using graph optimization.

[0120] Specifically, in step S3:

[0121] Pose graph optimization utilizes point cloud registration, velocity pre-integration, and loop closure detection to obtain relative pose transformation estimates and construct a pose graph. Finally, graph optimization is used to estimate the optimal pose.

[0122] Step S3.1: Point cloud registration, based on normal distribution transformation, the relative transformation is estimated by matching the current 4D radar point cloud and the key frame sub-map;

[0123] A sliding window is used to build a radar sub-map using 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 earliest keyframe is discarded.

[0124] 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 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 ;

[0125] Step S3.2: Velocity pre-integration. Using the estimated equipment velocity, calculate the relative pose transformation, introducing additional reliable relative pose estimation. The estimated linear velocity and angular velocity 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.:

[0126]

[0127]

[0128] Where, ω t and W 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 The orientation of the device in the world coordinate system is given by integration, which yields the relative rotation transformation ΔR between time i and time j. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e. V .

[0129] Specifically, step S3.3: Loop closure detection. This involves identifying previously visited locations and dividing the 4D radar point cloud into a grid on polar coordinates. The 3D point cloud is then mapped into a 2D matrix, where each element represents the maximum energy intensity of the radar point in the corresponding grid. As the device moves, the cosine distance between the current frame's 2D matrix and the 2D matrices generated from all previous keyframes is continuously searched and calculated. If the distance is less than a set threshold, a loop closure is considered detected. When a loop closure is detected, the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes is calculated. The error of this pose transformation estimation is e. L ;

[0130] Step S3.4: Graph optimization. Establish a pose graph, considering the error term of relative pose estimation 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... The optimal pose is estimated by solving a nonlinear least squares problem.

[0131]

[0132] in, Includes all moments, e O ,e V ,e L The error of the relative pose transformation obtained at different steps.

[0133] Example 2:

[0134] Example 2 is a preferred embodiment of Example 1, and is used to illustrate the present invention in more detail.

[0135] The present invention also provides a pose graph SLAM calculation system based on 4D millimeter-wave radar. The pose graph SLAM calculation system based on 4D millimeter-wave radar can be implemented by executing the process steps of the pose graph SLAM calculation method based on 4D millimeter-wave radar. That is, those skilled in the art can understand the pose graph SLAM calculation method based on 4D millimeter-wave radar as a preferred embodiment of the pose graph SLAM calculation system based on 4D millimeter-wave radar.

[0136] A pose graph SLAM calculation system based on 4D millimeter-wave radar, provided by the present invention, includes:

[0137] Module M1: Extracts ground point cloud, compares two consecutive frames of point cloud, and removes ghost points and random points under the ground;

[0138] Specifically, in module M1:

[0139] By extracting ground point clouds and comparing two consecutive frames of point clouds, ghost points and unstable random points under the ground are removed, reducing noise in 4D radar point clouds.

[0140] Module M1.1: Ghost point removal, extracts the ground point cloud from the original radar point cloud and filters out ghost points below the ground;

[0141] For the raw radar point cloud of 4D radar measurement n is the number of points in the point cloud;

[0142] The position of each point in the radar coordinate system is represented as follows: Where r i θ is the distance to this point measured by 4D radar. i The azimuth angle of this point is obtained from 4D radar measurement. The elevation angle of this point is obtained from 4D radar measurement;

[0143] For the original radar point cloud, the retention distance is less than the threshold δ. r And the height is near the radar installation height threshold δ h For points within the z-axis, principal component analysis is used to calculate the upward normal vector for each point, retaining those points where the angle between the normal vector and the positive z-axis unit vector is less than δ. n The points are extracted using a random sampling consensus algorithm, and ghost points below the ground are filtered out.

[0144] Module M1.2: Random Point Removal. This module identifies and filters random points by comparing the point clouds of two consecutive frames. For the current point cloud and the immediately preceding point cloud, the pose transformation between the two frames is calculated based on the device speed. The previous frame point cloud is then transformed to the coordinate system of the current frame. If a point in the current frame does not exist within a preset range after the transformation of the previous frame point cloud, that point is classified as a random point and filtered out.

[0145] 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 :

[0146]

[0147]

[0148] in, and These are the device linear velocity and angular velocity estimates for the previous frame, respectively, where Δ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, SO(3) is a three-dimensional special orthogonal group, 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 a point in the current frame does not exist within the preset range If a point is selected from the list, then the current point is classified as a random point and filtered out.

[0149] Module M2: Utilizes Doppler velocity information from 4D radar point clouds to estimate the linear velocity and angular velocity of the device;

[0150] Specifically, in module M2:

[0151] Module M2.1: Static point extraction. For the i-th point in the 4D radar point cloud, the relationship between the Doppler velocity of that point and the radar's own velocity is as follows:

[0152] v r, i = -di ·v s

[0153] 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;

[0154] The relationship between radar speed and equipment speed is as follows:

[0155]

[0156] Among them, t s and R s These represent the installation position and attitude of the 4D radar in the equipment coordinate system. and Let be the linear velocity and angular velocity of the device, respectively. For all static points in the 4D radar point cloud, the Doppler velocity and the device velocity conform to the following equation:

[0157]

[0158] 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, R 3×3 Represents a set of 3×3 real matrices. Based on the relationship between the Doppler velocity of all static points in the radar point cloud and the equipment velocity, the random sampling consensus algorithm is used to remove dynamic outliers and extract static points.

[0159] Module M2.2: Least squares estimation. After extracting the static points, the device speed is calculated using the least squares method based on the relationship between the Doppler velocity of all static points and the device speed.

[0160] Module M3: Estimates and constructs a pose graph using relative pose transformation, performs point cloud registration based on normal distribution transformation, pre-integrates using estimated device velocity, performs loop closure detection, and estimates the optimal pose using graph optimization.

[0161] Specifically, in module M3:

[0162] Pose graph optimization utilizes point cloud registration, velocity pre-integration, and loop closure detection to obtain relative pose transformation estimates and construct a pose graph. Finally, graph optimization is used to estimate the optimal pose.

[0163] Module M3.1: Point cloud registration, based on normal distribution transformation, estimates the relative transformation by matching the current 4D radar point cloud and keyframe sub-map;

[0164] A sliding window is used to build a radar sub-map using 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 earliest keyframe is discarded.

[0165] 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 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 ;

[0166] Module M3.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.:

[0167]

[0168]

[0169] Where, ω t and W 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 The orientation of the device in the world coordinate system is given by integration, which yields the relative rotation transformation ΔR between time i and time j. ij and relative translation transformation Δp ij The error of this pose transformation estimation is e. V .

[0170] Specifically, module M3.3: Loop closure detection identifies previously visited locations. In polar coordinates, the 4D radar point cloud is divided into a grid, and the 3D point cloud is mapped into a 2D matrix. Each element in the matrix represents the maximum energy intensity of the radar point in the corresponding grid. As the device moves, the cosine distance between the current frame's 2D matrix and the 2D matrices generated from all previous keyframes is continuously searched and calculated. If the distance is less than a set threshold, a loop closure is detected. When a loop closure is detected, the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes is calculated. The error of this pose transformation estimation is e. L ;

[0171] Module M3.4: Graph Optimization. A pose graph is built, considering the error term of relative pose estimation 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... The optimal pose is estimated by solving a nonlinear least squares problem.

[0172]

[0173] in, Includes all moments, e O ,e V ,e L The error of the relative pose transformation obtained at different steps.

[0174] Example 3:

[0175] Example 3 is a preferred example of Example 1, and is used to illustrate the present invention in more detail.

[0176] To address the shortcomings of existing technologies, this paper innovatively proposes a pose graph SLAM algorithm based on 4D millimeter-wave radar, which is applicable to the synchronous localization and mapping problem of automobiles or mobile robots in unknown environments.

[0177] The pose graph SLAM algorithm based on 4D millimeter-wave radar proposed in this invention mainly includes the following steps:

[0178] Step 1: Radar point cloud filtering. The original 4D 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, reducing the noise in the 4D radar point cloud.

[0179] 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. Using the Doppler velocity information from the 4D radar point cloud, the linear velocity and angular velocity of the vehicle are estimated.

[0180] Step 3: Pose graph optimization. Based on normal distribution transformation, the current radar point cloud and local radar keyframe sub-maps are registered to obtain radar odometry factors; the estimated vehicle speed is pre-integrated to obtain speed pre-integration factors; loop closure detection is performed to obtain loop closure factors; finally, the pose graph is constructed using the three factors, and the optimal pose is estimated based on graph optimization.

[0181] Specifically, the radar point cloud filtering process in step 1 mainly includes the following steps:

[0182] Step 1.1: Ghost point removal. Ghost points are spurious measurements below the ground in the original 4D radar point cloud due to the multipath effect of millimeter waves, and are one type of 4D radar measurement noise. 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.

[0183] For the raw radar point cloud obtained by 4D 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 radar measurements: range, azimuth, and elevation. For the original radar point cloud, first, points with ranges less than a threshold δ are 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;

[0184] Step 1.2: Random point removal. Random points are unstable, flickering, and spurious measurements contained in the original 4D radar point cloud, and are considered part of the noise in 4D radar measurements. 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.

[0185] 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 :

[0186]

[0187]

[0188] 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.

[0189] Specifically, the vehicle speed estimation process in step 2 mainly includes the following steps:

[0190] Step 2.1: Static Point Extraction. In addition to spatial information, 4D radar can also obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D 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 radar. Therefore, the relationship between the Doppler velocity of this point and the radar's own velocity is as follows:

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

[0192] 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.

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

[0194]

[0195] Among them, t s and R s These represent the installation position and orientation of the 4D 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 radar point cloud are static points. Therefore, for all static points in a 4D radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0196]

[0197] 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.

[0198] 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.

[0199] Specifically, the pose graph optimization process in step 3 mainly includes the following steps:

[0200] Step 3.1: Point cloud registration. Based on normal distribution transformation, the relative transformation is estimated by direct matching of the current 4D radar point cloud and key frame sub-map.

[0201] Because 4D radar point clouds are too sparse to accurately estimate pose 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. This 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.

[0202] 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 radar point clouds are 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 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 .

[0203] Step 3.2: Velocity pre-integration. Using the estimated vehicle velocity, the relative pose transformation is calculated, thus introducing additional reliable relative pose estimation and improving the accuracy and robustness of SLAM. 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, i.e.:

[0204]

[0205]

[0206] Where, ω t and W 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 This represents the car's orientation in the world coordinate system. From this, the relative rotation transformation ΔR between time i and time j can be obtained. ij and relative translation transformation Δp ij :

[0207]

[0208]

[0209]

[0210] 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 .

[0211] Step 3.3: Loop closure detection, identifying previously visited locations to reduce cumulative drift. In polar coordinates, the 4D radar point cloud is divided into a grid, mapping the 3D point cloud into a 2D matrix. Each element in the matrix represents the maximum energy intensity of the radar point in the corresponding grid. As the vehicle moves, the cosine distance between the current frame's 2D matrix and the 2D matrices generated from all previous keyframes is continuously searched and calculated. A loop closure is considered detected when the distance is less than a set threshold. Once a loop closure is detected, the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes is calculated using the registration method described in the point cloud registration step. The error of this pose transformation estimation is e. L .

[0212] Step 3.4: Graph optimization. A pose graph is established, and considering the error terms of relative pose estimation at all time points, the optimal pose of the vehicle at all time points is estimated through graph optimization. All nodes in the pose graph correspond to poses at different time points, and the edges between nodes correspond to relative pose transformations between different time points. All poses are denoted as... The optimal pose is estimated by solving a nonlinear least squares problem.

[0213]

[0214] in, Includes all moments, e O ,e V ,e L The error of the relative pose transformation obtained at different steps.

[0215] In the specific implementation process, such as Figure 1 As shown, this invention is divided into three parts: (1) radar point cloud filtering, (2) vehicle speed estimation, and (3) pose map optimization. The main steps are as follows:

[0216] Step 1: Radar point cloud filtering. The original 4D 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 radar point cloud.

[0217] Step 1.1: Ghost point removal. Ghost points are spurious measurements below the ground in the original 4D radar point cloud due to the multipath effect of millimeter waves, and are one type of 4D radar measurement noise. 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.

[0218] For the raw radar point cloud obtained by 4D 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 ri ,θ i , These are the spatial information of the point obtained from 4D radar measurements: range, azimuth, and elevation. For the original radar point cloud, first, points with ranges less than a threshold δ are 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 +Bt i +Cz i +D<0 points;

[0219] Step 1.2: Random point removal. Random points are unstable, flickering, and spurious measurements contained in the original 4D radar point cloud, and are considered part of the noise in 4D radar measurements. 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.

[0220] 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 :

[0221]

[0222]

[0223] 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-1Transform 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.

[0224] 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 radar point cloud.

[0225] Step 2.1: Static Point Extraction. In addition to spatial information, 4D radar can also obtain the Doppler velocity of each point in the point cloud. For the i-th point in the 4D 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 radar. Therefore, the relationship between the Doppler velocity of this point and the radar's own velocity is as follows:

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

[0227] 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.

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

[0229]

[0230] Among them, t s and R s These represent the installation position and orientation of the 4D 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 radar point cloud are static points. Therefore, for all static points in a 4D radar point cloud, the Doppler velocity and the vehicle's velocity conform to the following equation:

[0231]

[0232] Where m is the number of all static points in the point cloud, I3×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.

[0233] 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.

[0234] Step 3: Pose graph optimization. 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; the estimated vehicle speed is pre-integrated to obtain speed pre-integration factors; loop closure detection is performed to obtain loop closure factors; finally, the pose graph is constructed using the three factors, and the optimal pose is estimated based on graph optimization.

[0235] Step 3.1: Point cloud registration. Based on normal distribution transformation, the relative transformation is estimated by direct matching of the current 4D radar point cloud and key frame sub-map.

[0236] Because 4D radar point clouds are too sparse to accurately estimate pose 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. This 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.

[0237] 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 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 approach 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 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:

[0238]

[0239] 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.

[0240] Step 3.2: Velocity pre-integration. Using the estimated vehicle velocity, the relative pose transformation is calculated, thus introducing additional reliable relative pose estimation and improving the accuracy and robustness of SLAM. 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, i.e.:

[0241]

[0242]

[0243] Where, ω t and W 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 This refers to the car's orientation in the world coordinate system. Assume that within a very short time interval [t, t+Δt], ω... t and W v t If 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:

[0244]

[0245]

[0246] 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 usedi and rotation R i The calculation yielded:

[0247]

[0248]

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

[0250]

[0251]

[0252] in, J r,k yes The right Jacobian matrix, It is a vector antisymmetric matrix, and The relative rotation and translation are obtained through pre-integration of the vehicle's velocity, and 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:

[0253]

[0254]

[0255] Due to δφ ij It is zero-mean Gaussian noise A linear combination, therefore δφ ij It 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:

[0256]

[0257]

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

[0259] Step 3.3: Loop closure detection identifies previously visited locations to reduce cumulative drift. In polar coordinates, the 4D radar point cloud is divided into a grid, mapping the 3D point cloud into a 2D matrix. Each element in the matrix represents the maximum energy intensity of the radar point in the corresponding grid. As the vehicle moves, the cosine distance between the current frame's 2D matrix and the 2D matrices generated from all previous keyframes is continuously searched and calculated. A loop closure is considered detected when the distance is less than a set threshold. Once a loop closure is detected, the relative pose transformation between the current frame j and the sub-map composed of the loop closure frame k and nearby keyframes is calculated using the registration method described in the point cloud registration step. Then The closure factor between the nodes corresponding to times j and k is added to the pose graph. The error of this pose transformation estimation is...

[0260]

[0261] in, Includes all loopback frames. The corresponding covariance matrix is ​​Log(·)∶SE(3)→R 6 It is a logarithmic mapping of three-dimensional transformation.

[0262] Step 3.4: Graph optimization. By establishing a pose graph, considering all error terms of relative pose estimation, and using graph optimization to estimate the optimal pose of the vehicle at all times, more accurate and robust localization and mapping are achieved. 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. The pose is denoted as x = (R, p) ∈ SE (3), where R and p are rotation and translation, respectively. The poses at all times are denoted as... The optimal pose is estimated by solving a nonlinear least squares problem.

[0263]

[0264] Where K contains all the times, e O ,e V ,e L The error of the relative pose transformation obtained at different steps.

[0265] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0266] 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 pose graph SLAM calculation method based on 4D millimeter-wave radar, characterized in that, include: Step S1: Extract the ground point cloud, compare two consecutive frames of point clouds, and remove ghost points and random points under the ground; Step S2: Using the Doppler velocity information of the 4D radar point cloud, estimate the linear velocity and angular velocity of the device; Step S3: Estimate and construct a pose graph using relative pose transformation, register the point cloud based on normal distribution transformation, perform pre-integration using the estimated device velocity, perform loop closure detection, and estimate the optimal pose using graph optimization. In step S3: Pose graph optimization utilizes point cloud registration, velocity pre-integration, and loop closure detection to obtain relative pose transformation estimates and construct a pose graph. Finally, graph optimization is used to estimate the optimal pose. Step S3.1: Point cloud registration, based on normal distribution transformation, the relative transformation is estimated by matching the current 4D radar point cloud and the key frame sub-map; A sliding window is used to build a radar sub-map using 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 earliest 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 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... ; Step S3.2: Velocity pre-integration. Using the estimated device velocity, calculate the relative pose transformation, introducing additional reliable relative pose estimation. The estimated linear velocity and angular velocity of the equipment at each moment are denoted as . and The estimated value is the true value superimposed with zero-mean Gaussian white noise, that is: in, and These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term. It is the orientation of the device in the world coordinate system, obtained through integration. Time and Relative rotation transformation between moments and relative translation transformation The error of this pose transformation estimation is .

2. The pose graph SLAM calculation method based on 4D millimeter-wave radar according to claim 1, characterized in that, In step S1: By extracting ground point clouds and comparing two consecutive frames of point clouds, ghost points and unstable random points under the ground are removed, reducing noise in 4D radar point clouds. Step S1.1: Ghost point removal. Extract the ground point cloud from the original radar point cloud and filter out ghost points below the ground. For the raw radar point cloud of 4D radar measurement , This represents the number of points in the point cloud. The position of each point in the radar coordinate system is represented as follows: ,in The distance to this point is measured by 4D radar. The azimuth angle of this point is obtained from 4D radar measurement. The elevation angle of this point is obtained from 4D radar measurement. For the original radar point cloud, the retention distance is less than the threshold. And the height is near the threshold of the radar installation height. For points within the z-axis, principal component analysis is used to calculate the upward normal vector for each point, retaining those points where the angle between the normal vector and the positive z-axis unit vector is less than a certain value. The points are extracted using a random sampling consensus algorithm, and ghost points below the ground are filtered out. Step S1.2: Random point removal. Random points are identified and filtered out by comparing the point clouds of the previous and next frames. For the current point cloud and the immediately preceding point cloud, the pose transformation of the point clouds of the previous and next frames is calculated based on the device speed. The point cloud of the previous frame is transformed into the coordinate system of the current frame. If there is no transformed point cloud of the previous frame within a preset range near a certain point in the current frame, then the point is classified as a random point and filtered out. Calculate the current point cloud Point cloud in the next adjacent frame Rotational transformations between Translation : in, and These are the device linear velocity and angular velocity estimates from the previous frame, respectively. It is the time difference between two frames. It is an exponential mapping of three-dimensional rotation. It is a three-dimensional real vector space. It is a three-dimensional special orthogonal group, utilizing and Transform the point cloud from the previous frame to the coordinate system of the current frame to obtain... If a point in the current frame does not exist within the preset range... If a point is selected from the list, then the current point is classified as a random point and filtered out.

3. The pose graph SLAM calculation method based on 4D millimeter-wave radar according to claim 1, characterized in that, In step S2: Step S2.1: Static point extraction, for the first point in the 4D radar point cloud... At a given point, the relationship between the Doppler velocity at that point and the radar's own velocity is as follows: in, It is the first Doppler velocity at each point, It is the unit direction vector of that point relative to the radar. It is the speed of the radar; The relationship between radar speed and equipment speed is as follows: in, and These represent the installation position and attitude of the 4D radar in the equipment coordinate system. and Let be the linear velocity and angular velocity of the device, respectively. For all static points in the 4D radar point cloud, the Doppler velocity and the device velocity conform to the following equation: in, This represents the number of all static points in the point cloud. It is a 3×3 identity matrix. yes antisymmetric matrix, Represents a set of 3×3 real matrices. Based on the relationship between the Doppler velocity of all static points in the radar point cloud and the equipment velocity, 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 equipment speed is calculated using the least squares method based on the relationship between the Doppler velocity of all static points and the equipment speed.

4. The pose graph SLAM calculation method based on 4D millimeter-wave radar according to claim 1, characterized in that: Step S3.3: Loop closure detection. Identify previously visited locations. Divide the 4D radar point cloud into a grid on polar coordinates, and map the 3D point cloud into a 2D matrix. The value of each element in the matrix is ​​the maximum energy intensity of the radar point in the corresponding grid. As the device moves, continuously search and calculate the cosine distance between the 2D matrix of the current frame and the 2D matrices generated from all previous keyframes. If the distance is less than a set threshold, a loop closure is considered detected. When a loop closure is detected, calculate the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes. The error of this pose transformation estimation is... ; Step S3.4: Graph optimization. Establish a pose graph, considering the error term of relative pose estimation 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... The optimal pose is estimated by solving a nonlinear least squares problem. : in, Includes all moments, The error of the relative pose transformation obtained at different steps.

5. A pose graph SLAM calculation system based on 4D millimeter-wave radar, characterized in that, include: Module M1: Extracts ground point cloud, compares two consecutive frames of point cloud, and removes ghost points and random points under the ground; Module M2: Utilizes Doppler velocity information from 4D radar point clouds to estimate the linear velocity and angular velocity of the device; Module M3: Estimates and constructs a pose graph using relative pose transformation, performs point cloud registration based on normal distribution transformation, pre-integrates using estimated device velocity, performs loop closure detection, and estimates the optimal pose using graph optimization. In module M3: Pose graph optimization utilizes point cloud registration, velocity pre-integration, and loop closure detection to obtain relative pose transformation estimates and construct a pose graph. Finally, graph optimization is used to estimate the optimal pose. Module M3.1: Point cloud registration, based on normal distribution transformation, estimates the relative transformation by matching the current 4D radar point cloud and keyframe sub-map; A sliding window is used to build a radar sub-map using 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 earliest 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 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... ; Module M3.2: Velocity pre-integration. Utilizing the estimated device velocity, it calculates the relative pose transformation, introducing additional reliable relative pose estimation. The estimated linear velocity and angular velocity of the equipment at each moment are denoted as . and The estimated value is the true value superimposed with zero-mean Gaussian white noise, that is: in, and These are the true values ​​of angular velocity and linear velocity in the world coordinate system. and This is the corresponding noise term. It is the orientation of the device in the world coordinate system, obtained through integration. Time and Relative rotation transformation between moments and relative translation transformation The error of this pose transformation estimation is .

6. The pose graph SLAM calculation system based on 4D millimeter-wave radar according to claim 5, characterized in that, In module M1: By extracting ground point clouds and comparing two consecutive frames of point clouds, ghost points and unstable random points under the ground are removed, reducing noise in 4D radar point clouds. Module M1.1: Ghost point removal, extracts the ground point cloud from the original radar point cloud and filters out ghost points below the ground; For the raw radar point cloud of 4D radar measurement , This represents the number of points in the point cloud. The position of each point in the radar coordinate system is represented as follows: ,in The distance to this point is measured by 4D radar. The azimuth angle of this point is obtained from 4D radar measurement. The elevation angle of this point is obtained from 4D radar measurement. For the original radar point cloud, the retention distance is less than the threshold. And the height is near the threshold of the radar installation height. For points within the z-axis, principal component analysis is used to calculate the upward normal vector for each point, retaining those points where the angle between the normal vector and the positive z-axis unit vector is less than a certain value. The points are extracted using a random sampling consensus algorithm, and ghost points below the ground are filtered out. Module M1.2: Random Point Removal. This module identifies and filters random points by comparing the point clouds of two consecutive frames. For the current point cloud and the immediately preceding point cloud, the pose transformation between the two frames is calculated based on the device speed. The previous frame point cloud is then transformed to the coordinate system of the current frame. If a point in the current frame does not exist within a preset range after the transformation of the previous frame point cloud, that point is classified as a random point and filtered out. Calculate the current point cloud Point cloud in the next adjacent frame Rotational transformations between Translation : in, and These are the device linear velocity and angular velocity estimates from the previous frame, respectively. It is the time difference between two frames. It is an exponential mapping of three-dimensional rotation. It is a three-dimensional real vector space. It is a three-dimensional special orthogonal group, utilizing and Transform the point cloud from the previous frame to the coordinate system of the current frame to obtain... If a point in the current frame does not exist within the preset range... If a point is selected from the list, then the current point is classified as a random point and filtered out.

7. The pose graph SLAM calculation system based on 4D millimeter-wave radar according to claim 5, characterized in that, In module M2: Module M2.1: Static point extraction, for the first point in a 4D radar point cloud. At a given point, the relationship between the Doppler velocity at that point and the radar's own velocity is as follows: in, It is the first Doppler velocity at each point, It is the unit direction vector of that point relative to the radar. It is the speed of the radar; The relationship between radar speed and equipment speed is as follows: in, and These represent the installation position and attitude of the 4D radar in the equipment coordinate system. and Let be the linear velocity and angular velocity of the device, respectively. For all static points in the 4D radar point cloud, the Doppler velocity and the device velocity conform to the following equation: in, This represents the number of all static points in the point cloud. It is a 3×3 identity matrix. yes antisymmetric matrix, Represents a set of 3×3 real matrices. Based on the relationship between the Doppler velocity of all static points in the radar point cloud and the equipment velocity, 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 device speed is calculated using the least squares method based on the relationship between the Doppler velocity of all static points and the device speed.

8. The pose graph SLAM calculation system based on 4D millimeter-wave radar according to claim 5, characterized in that: Module M3.3: Loop closure detection identifies previously visited locations. In polar coordinates, the 4D radar point cloud is divided into a grid, and the 3D point cloud is mapped to a 2D matrix. Each element in the matrix represents the maximum energy intensity of the radar point in the corresponding grid. As the device moves, the cosine distance between the current frame's 2D matrix and the 2D matrices generated from all previous keyframes is continuously searched and calculated. If the distance is less than a set threshold, a loop closure is detected. When a loop closure is detected, the relative pose transformation between the current frame and the sub-map composed of the loop closure frame and nearby keyframes is calculated. The error of this pose transformation estimation is... ; Module M3.4: Graph Optimization. A pose graph is built, considering the error term of relative pose estimation 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... The optimal pose is estimated by solving a nonlinear least squares problem. : in, Includes all moments, The error of the relative pose transformation obtained at different steps.

Images