A 4D millimeter-wave radar mapping optimization method and system based on beam adjustment

Abstract

This invention discloses a 4D millimeter-wave radar mapping optimization method based on beam adjustment, belonging to the field of surveying and robot navigation. The method includes: obtaining an initial pose trajectory and corresponding millimeter-wave radar point cloud frames of a static scene based on acquired millimeter-wave radar frames and IMU data using millimeter-wave radar inertial odometry; selecting keyframes based on the initial pose trajectory, performing scene recognition and geometric verification to obtain closed-loop frame pairs, constructing closed-loop constraints, and correcting drift through pose graph optimization; constructing a global mesh graph based on the optimized pose using all keyframes, and constructing point-to-point distance constraints based on the global mesh graph; the point-to-point distance constraints, the self-velocity constraints of the millimeter-wave radar frames, and the IMU pre-integration constraints constitute an optimization objective function, which is solved using least-squares optimization to obtain the optimized keyframe pose; and smoothing the pose graph to obtain the poses of all millimeter-wave radar frames and constructing a millimeter-wave radar point cloud graph.

Description

Technical Field

[0001] This invention belongs to the field of surveying and robot navigation, specifically relating to a 4D millimeter-wave radar mapping optimization method and system based on bundle adjustment. Background Technology

[0002] 4D millimeter-wave radar, as an advanced sensing technology, has attracted widespread attention due to its adaptability and robustness under harsh conditions. Compared to traditional 3D millimeter-wave radar, which only senses planar coordinates and relative Doppler velocity, 4D millimeter-wave radar significantly improves environmental perception performance by adding altitude sensing capabilities. Extensive research has been conducted in related technical fields for millimeter-wave radar odometry and mapping. These systems (such as 4D iRIOM) can provide the vehicle's initial pose and millimeter-wave radar frames, enabling the construction of preliminary maps of the scene.

[0003] However, the existing technology has the following shortcomings:

[0004] Odometry drift leads to accuracy issues: Odometry tracks inevitably drift, especially at revisited locations, potentially causing incorrect feature associations. This results in insufficient global accuracy and consistency for maps built solely based on odometry.

[0005] There is a lack of research on global optimization of millimeter-wave radar data: existing work mainly focuses on real-time millimeter-wave radar odometry, and there are few studies on consistency optimization for multiple frames of millimeter-wave radar after the initial pose of the millimeter-wave radar odometry. Summary of the Invention

[0006] This invention addresses the problems of accumulated drift and errors in the initial pose provided by existing millimeter-wave radar odometry, which lead to insufficient global consistency and accuracy of the generated global millimeter-wave radar point cloud map. It provides a 4D millimeter-wave radar mapping optimization method based on bundle adjustment. By fusing constraints from 4D millimeter-wave radar and IMU, it optimizes the millimeter-wave radar frame pose and the global millimeter-wave radar point cloud map. By integrating millimeter-wave radar observation and IMU constraints within the bundle adjustment framework to optimize the millimeter-wave radar frame pose and the overall millimeter-wave radar point cloud, it can significantly improve the global accuracy and consistency of the 4D millimeter-wave radar map.

[0007] According to one aspect of the present invention, a 4D millimeter-wave radar mapping optimization method based on beam adjustment is provided, comprising:

[0008] Based on the acquired millimeter-wave radar frames and IMU data, the initial pose trajectory and the corresponding millimeter-wave radar point cloud frames of the static scene are obtained through millimeter-wave radar inertial odometry.

[0009] Key frames are selected based on the initial pose trajectory and millimeter-wave radar point cloud frames. Scene recognition and geometric verification are performed on all key frames to obtain closed-loop frame pairs. The optimized key frame pose trajectory is obtained by combining the pose graph optimization algorithm.

[0010] A global mesh graph is constructed based on the pose trajectory of the optimized keyframes. For point pairs in each voxel in the global mesh graph that satisfy the preset time and space constraints, point-to-point distance constraints are constructed.

[0011] The point-to-point distance constraint, the self-velocity constraint of the millimeter-wave radar frame, and the IMU pre-integration constraint are combined into an objective function. The objective function is then solved and optimized using least squares to obtain the optimized keyframe state variables.

[0012] Based on the optimized keyframe state variables, the poses of all optimized millimeter-wave radar frames are obtained through pose graph smoothing, and a millimeter-wave radar point cloud map is constructed based on the poses of all optimized millimeter-wave radar frames.

[0013] As a further technical solution, key frames are selected based on the initial pose trajectory and millimeter-wave radar point cloud frames, including:

[0014] Construct a keyframe sliding window represented by a voxel mesh;

[0015] Based on the initial pose trajectory, calculate the ratio of the number of voxels in the current millimeter-wave radar point cloud frame falling into the keyframe sliding window to the total number of voxels in the current millimeter-wave radar point cloud frame.

[0016] If the ratio is lower than the set ratio threshold, the current frame is used as a keyframe and added to the keyframe sliding window;

[0017] When the number of keyframe windows exceeds the set window limit, the first frame of the keyframe window will be removed.

[0018] As a further technical solution, scene recognition and geometric verification are performed on all keyframes to obtain closed-loop frame pairs, including:

[0019] Descriptors are calculated based on keyframes, and closed-loop frame pairs in the revisited scenario are identified based on the Euclidean distance corresponding to the descriptors.

[0020] The relative poses of the two frames in the closed-loop frame pair are iteratively optimized. When the average distance between the matching points of the two frames is less than the set distance, a closed-loop constraint is formed.

[0021] As a further technical solution, the preset spatial constraint is that the voxel overlap of the two point clouds is greater than a set value for each pair of points within a voxel; the preset temporal constraint is that the time interval between the two point clouds is less than a set interval value for each pair of points within a voxel, or the time interval between the two point clouds and the two point clouds in a certain closed-loop frame pair is less than a set interval value.

[0022] As a further technical solution, the point-to-point distance constraint, the self-velocity constraint of the millimeter-wave radar frame, and the IMU pre-integration constraint are combined into an objective function, including:

[0023] Based on the point-to-point distance constraints in each voxel, the geometric cost of point cloud matching in each voxel is obtained;

[0024] The self-velocity residual of millimeter-wave radar is obtained based on the self-velocity constraint of millimeter-wave radar frames.

[0025] The IMU pre-integration residual is obtained based on the IMU pre-integration constraint;

[0026] The objective function is constructed based on the geometric cost of point cloud matching in each voxel, the millimeter-wave radar self-velocity residual, the IMU pre-integration residual, and the prior of the first keyframe pose and zero bias.

[0027] As a further technical solution, the expression for the objective function is:

[0028]

[0029] in, Describe the objective function. The geometric cost is given by M, where M is the number of keyframes. for arrive IMU predicted factorization for The covariance matrix; For keyframes The self-velocity residual, For keyframes The covariance matrix of the self-velocity residuals; The pose prior constraints for the first keyframe. The covariance matrix of the pose prior constraints for the first keyframe; For IMU zero-biased prior, is the covariance matrix of the IMU zero-bias prior; the superscript T indicates matrix transpose, and the superscript -1 indicates matrix inversion.

[0030] As a further technical solution, the expression for the geometric cost is:

[0031]

[0032] in, The geometric cost for a voxel. Let j be the state variable for keyframe j, and k be another keyframe. For the set of valid frame pairs, and They are from keyframes point and from keyframes point The covariance.

[0033] As a further technical solution, based on the optimized keyframe state variables, the poses of all optimized millimeter-wave radar frames are obtained through pose graph smoothing, including:

[0034] Based on the absolute pose constraints of keyframes and the relative pose constraints between adjacent frames obtained from millimeter-wave radar inertial odometry, a pose graph optimization problem is constructed.

[0035] The pose of all millimeter-wave radar frames is obtained by solving the pose graph optimization problem through least squares iteration.

[0036] As a further technical solution, the method also includes multiple iterations between the two steps of constructing point-to-point distance constraints and solving and optimizing the objective function.

[0037] According to one aspect of the present invention, a 4D millimeter-wave radar mapping optimization system based on beam adjustment is provided.

[0038] The first processing module is used to obtain the initial pose trajectory and the corresponding millimeter-wave radar point cloud frame of the static scene based on the acquired millimeter-wave radar frame and IMU data through millimeter-wave radar inertial odometry.

[0039] The second processing module is used to select key frames based on the initial pose trajectory and millimeter-wave radar point cloud frames, perform scene recognition and geometric verification on all key frames to obtain closed-loop frame pairs, and combine the pose graph optimization algorithm to obtain the optimized key frame pose trajectory.

[0040] The third processing module is used to construct a global mesh map based on the pose trajectory of the optimized keyframes, and to construct point-to-point distance constraints for point pairs within each voxel in the global mesh map that satisfy preset time and space constraints.

[0041] The fourth processing module is used to combine the point-to-point distance constraint, the self-velocity constraint of the millimeter-wave radar frame, and the IMU pre-integration constraint into an objective function, and to optimize the objective function by least squares to obtain the optimized key frame state variables.

[0042] The fifth processing module is used to obtain the pose of all millimeter-wave radar frames after optimization by smoothing the pose graph based on the optimized keyframe state variables, and to construct a millimeter-wave radar point cloud map based on the pose of all millimeter-wave radar frames after optimization.

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

[0044] 1. This invention optimizes the pose of key frames by introducing beam adjustment, which is to optimize point-to-point distance constraints and other constraints by weighting point variance. This expands the optimization scope from local frame-to-frame matching or frame-to-submap matching of odometry to matching between multiple frames in the global context, thus achieving consistent 4D millimeter-wave radar point cloud mapping.

[0045] 2. This invention selects effective frame pairs through spatiotemporal matching to construct point-to-point distance constraints, which helps to avoid erroneous feature associations at revisited locations due to odometer drift. It also constructs an objective function by combining the self-velocity constraints obtained from Doppler observations of millimeter-wave radar and the pre-integration constraints of IMU, thus maintaining the smoothness of the trajectory. This effectively combines the advantages of the two sensors and significantly improves the mapping quality.

[0046] 3. This invention obtains key frame sequences by filtering from the original frame sequences, which can effectively process long-term millimeter-wave radar data, ensuring computational efficiency. It is applicable to fields that require accurate and consistent environmental perception, such as engineering surveying, robot navigation, and autonomous driving. Attached Figure Description

[0047] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0048] Figure 1 The flowchart illustrates the 4D millimeter-wave radar mapping optimization method based on beam adjustment provided in this embodiment of the invention.

[0049] Figure 2 The diagram shows the structural block diagram of the 4D millimeter-wave radar mapping optimization method based on beam adjustment provided in the embodiments of the present invention.

[0050] Figure 3 This is a comparison of the mapping results on the SNAIL-Radar dataset (20231105 / data4 sequence) provided in the embodiments of the present invention.

[0051] Figure 4 This is a comparison of the mapping results on the SNAIL-Radar dataset (sequence 20231105 / data6) provided in this embodiment of the invention.

[0052] Figure 5 This is a comparison chart of the mapping results on the Coloradar dataset (edgar_classroom_run0 sequence) provided in the embodiments of the present invention. Detailed Implementation

[0053] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0054] like Figure 1-2 As shown, this invention proposes a 4D millimeter-wave radar mapping optimization method based on beam adjustment, including: obtaining an initial pose trajectory and corresponding millimeter-wave radar point cloud frames of a static scene based on acquired millimeter-wave radar frames and IMU data using millimeter-wave radar inertial odometry; selecting key frames based on the initial pose trajectory and millimeter-wave radar point cloud frames, performing scene recognition and geometric verification on all key frames to obtain closed-loop frame pairs, and combining this with a pose graph optimization algorithm to obtain the optimized key frame pose trajectory; constructing a global mesh graph based on the optimized key frame pose trajectory; and further optimizing the mapping of the key frames. Within each voxel in the global mesh graph, point pairs satisfying preset time and spatial constraints are used to construct point-to-point distance constraints weighted by point variance. These constraints, along with the self-velocity constraints of the millimeter-wave radar frames and the IMU pre-integration constraints, are combined into an objective function. This objective function is then optimized using least squares to obtain the optimized keyframe state variables. Based on these optimized keyframe state variables, the poses of all optimized millimeter-wave radar frames are obtained through pose graph smoothing. Finally, a millimeter-wave radar point cloud map is constructed based on the optimized poses of all millimeter-wave radar frames. Multiple iterations are performed between the steps of constructing the point-to-point distance constraints and optimizing the objective function.

[0055] Specifically, embodiments of the present invention provide a data acquisition and preprocessing process:

[0056] 1. Data Acquisition: Based on the acquired millimeter-wave radar frames and IMU data, the initial pose trajectory and corresponding millimeter-wave radar point cloud frames of the static scene are obtained through millimeter-wave radar inertial odometry (e.g., 4D iRIOM). Compared to the millimeter-wave radar frames acquired by 4D millimeter-wave radar, the millimeter-wave radar point cloud frames output after processing by the millimeter-wave radar inertial odometry have had the point cloud points corresponding to dynamic objects removed.

[0057] 2. Keyframe selection: Construct a keyframe sliding window represented by a voxel grid. Calculate the ratio of the number of voxels in the current millimeter-wave radar point cloud frame falling into the keyframe sliding window to the total number of voxels in the current millimeter-wave radar point cloud frame. If the ratio is lower than a set threshold (e.g., less than 0.6), the current frame is selected as a keyframe and added to the keyframe sliding window. If the size of the keyframe sliding window exceeds the set window limit (e.g., greater than 21), the first frame of the keyframe sliding window is removed.

[0058] 3. Calculate the covariance of each point in the keyframe: Find the point clouds of the k adjacent frames (e.g., k=10) to the left and right of each keyframe to obtain a 2k+1 frame window; transform all frames in the window to the coordinate system of the center keyframe to construct a voxel mesh; the center keyframe is the center frame of the 2k+1 frame window; based on the voxel corresponding to each point of the center keyframe, calculate the covariance of the voxel as the covariance of that point, which is used as the weight for subsequent point-to-point distances.

[0059] 4. Scene Recognition and Geometric Verification: (1) Scene Recognition: Descriptors are calculated for keyframes using PointNetVlad, and closed-loop frame pairs for revisiting scenes are identified based on the Euclidean distance of the smaller descriptor. (2) Geometric Verification of Candidate Loop Closure: The relative pose of the two frames in the closed-loop frame pair is iteratively optimized using GICP (generalized iterative closest point). If the average distance between the matching points of the two frames is less than a set distance, the closed-loop frame pair is accepted, forming a closed-loop constraint. In addition, using the relative pose constraints formed by these closed-loop frame pairs and the relative pose constraints of adjacent frames obtained by odometry, a pose graph closed-loop optimization problem is constructed to correct the odometry drift, that is, the optimized keyframe pose trajectory is obtained by combining the pose graph optimization algorithm.

[0060] Specifically, embodiments of the present invention provide the following: constructing a global mesh graph, selecting valid frame pairs, and constructing point-to-point distance constraints:

[0061] Based on the keyframe pose optimized from the pose graph, all millimeter-wave radar keyframes are divided into a global mesh graph (i.e., transformed to the world coordinate system to obtain the overall point cloud). This overall point cloud is then divided into a voxel grid. For short-range (<20m) millimeter-wave radar, the voxel size is smaller, such as 0.12m, while for long-range millimeter-wave radar, the voxel size is larger, such as 0.5m. To avoid inconsistencies in optimization, each voxel stores at most one point per frame. Two point clouds are two separate point clouds, each containing many points that fall within many voxels in the voxel grid. The voxel overlap is the ratio of the number of overlapping voxels between the two point clouds to the number of voxels in the frame with more voxels. A frame pair consists of two point clouds, and a closed loop is formed by these two point clouds.

[0062] When the voxel overlap 'o' of two point cloud frames is greater than a certain threshold, such as 'o>0.1', spatially compatible frame pairs can be selected, which constitutes spatial constraints. If the time interval between the two frames in these spatially compatible frame pairs is small (e.g., ... <30s) or the time interval between the two frames and the two frames in a certain closed loop is relatively small (e.g. <5s), these frame pairs can be selected as time-fitted frame pairs, i.e., time-constrained. These frame pairs that simultaneously satisfy both spatial and temporal constraints will be used as valid frame pairs to filter point pairs in each voxel. For point pairs in each voxel, if their source frame belongs to a valid frame pair, a point-to-point distance constraint is constructed, which uses the inverse of the sum of the covariances of each point as the weight. It should be noted that in the embodiments of the present invention, the time and spatial constraints are not limited to the above calculation process and order; valid frame pairs can be determined as long as both constraints are satisfied simultaneously.

[0063] Specifically, embodiments of the present invention provide pose optimization, including: optimizing keyframe pose using point-to-point distance constraints, self-velocity constraints derived from Doppler observations, and IMU prediction constraints, thereby improving the consistency of the global point cloud. The pose optimization process uses a least-squares solver and automatic differentiation. The pose optimization step and the above-mentioned point-to-point constraint construction step can terminate after at most K iterations (K can be 6 in this embodiment of the present invention), and finally output the optimized trajectory of the millimeter-wave radar keyframe.

[0064] Specifically, embodiments of the present invention provide an optimized global objective function. The process of constructing its constraints is as follows:

[0065] Set each keyframe for optimization Current Time The state variables are:

[0066] (1)

[0067] in, and This indicates the position and attitude of the inertial measurement unit (IMU) (B represents the IMU coordinate system) in the world coordinate system W. The velocity of the IMU in the world coordinate system. and They are respectively The zero bias of the gyroscope and the zero bias of the accelerometer at that moment.

[0068] Given the current keyframe state, transform all keyframe points to world coordinates and insert them into the voxel mesh. Let voxels... It contains points from several keyframes, denoted as... For the first Keyframe, falling voxel A point in the radar coordinate system will have its position in the world coordinate system as follows:

[0069] (2)

[0070] in, and This refers to the rotation and translation extrinsic parameters of the millimeter-wave radar relative to the IMU system.

[0071] remember To be related to voxels The associated set of valid frame pairs, voxels The corresponding geometric cost is:

[0072] (3)

[0073] in, and Voxels From keyframes point and from keyframes point The covariance. This geometric cost constrains the consistency between points falling into the same voxel but coming from different frames.

[0074] Voxel-based geometric cost The objective function is constructed by combining the IMU pre-integration residual, the millimeter-wave radar self-velocity residual, and the prior values ​​of the first keyframe pose and zero bias, as shown in Equation (4). The state variables of all keyframes are used as the variables to be optimized and solved by the least squares method. Among them, the prior value of the pose of the first keyframe is its initial value, and the prior value of the zero bias is a six-dimensional zero vector.

[0075] (4)

[0076] in, The geometric cost is given by M, where M is the number of keyframes. for arrive The IMU prediction factor is used to connect the associated keyframe states and constrain the consistency of pose, velocity and zero bias through pre-integration. for The covariance matrix is ​​used for weighted prediction factors; For keyframes The self-velocity residual; For keyframes The covariance matrix of the self-velocity residuals is used to weight the self-velocity residuals; The pose prior constraints for the first keyframe. The covariance matrix of the pose prior for the first keyframe; For IMU zero-biased priors, typically a 6-dimensional zero vector, This is the zero-biased prior covariance matrix of the IMU; the superscript T indicates matrix transpose, and the superscript -1 indicates matrix inversion.

[0077] The self-velocity residual of keyframe j is defined as:

[0078] (5)

[0079] in, and It is the extrinsic parameter of the radar relative to the IMU coordinate system. for transpose, The velocity of the IMU in the world coordinate system. It is the angular velocity of the IMU system. It is the calculated self-velocity of the millimeter-wave radar. covariance and It can be obtained by using GNC (graduated non convexity) based in 4D iRIOM based on Doppler observations from millimeter-wave radar.

[0080] at last, Six degrees of pose freedom are fixed by constraining the pose of the first keyframe, while Then, a priori constraints are applied to the zero-bias state of the first keyframe. Objective function Minimization can be achieved using a least-squares solver, yielding the optimized state variables for all keyframes. The point-to-point distance constraint construction and optimization of all keyframe state variables employ a method similar to bundle adjustment to construct and solve the mapping problem. To ensure effectiveness, these two steps can be iterated multiple times to improve the accuracy of the keyframe state variables and the global millimeter-wave radar point cloud map.

[0081] Specifically, to recover the pose of non-keyframes, this embodiment of the invention constructs a pose graph optimization problem. The constraints of this problem include absolute pose constraints of keyframes and relative pose constraints between adjacent frames output from the odometry trajectory. The state variable optimized by this problem is the pose of all millimeter-wave radar frames. The optimized poses of all millimeter-wave radar frames can be obtained through least-squares iterative solving, and the final millimeter-wave radar point cloud map can be constructed accordingly.

[0082] Specifically, to verify the effectiveness of the method proposed in this embodiment, tests were conducted on two outdoor long-range 4D millimeter-wave radar sequences from SNAIL radar and one indoor short-range 4D millimeter-wave radar sequence from Coloradar. Data preprocessing included obtaining the pose of the initial 4D millimeter-wave radar frames using the 4D iRIOM odometry method, detecting loop closures using PointNetVLAD, and then performing pose graph optimization. Subsequently, these millimeter-wave radar point cloud frames underwent six iterative optimizations using the proposed method for point cloud constraint construction and beam adjustment to solve for the pose. Figure 3-5 The cumulative point cloud maps of the three selected sequences before iterative optimization (after pose map optimization), after iterative optimization, and the reference pose are displayed. The reference point cloud synthesized based on the reference pose is shown in green, the map after pose map optimization is shown in red, and the map after 6 iterations of optimization using the proposed method is shown in blue. Figure 3 The quality of the point cloud image of the SNAIL radar 20231105 / data4 sequence was significantly improved in the black-framed area, making it closer to the shape of the reference point cloud. Figure 4 The black box on the left shows that the wall point cloud optimized by this invention is thinner (closer to a plane) than the wall point cloud before optimization of the SNAIL radar 20231105 / data6 sequence. Figure 5 The optimized map is closer to the geometry of the green reference point cloud of the Coloradar sequence edgar_classroom_run0, especially in the left area of ​​the point cloud map.

[0083] The implementation of the various embodiments of the present invention is based on programmed processing by a device with processor functionality. Therefore, in practical engineering, the technical solutions and functions of the various embodiments of the present invention are encapsulated into various modules. Based on this reality, and building upon the above embodiments, the embodiments of the present invention provide a 4D millimeter-wave radar mapping optimization system based on beam adjustment, which is used to execute the 4D millimeter-wave radar mapping optimization method based on beam adjustment in the above method embodiments.

[0084] The system includes: a first processing module, used to obtain an initial pose trajectory and corresponding millimeter-wave radar point cloud frames of a static scene based on acquired millimeter-wave radar frames and IMU data using millimeter-wave radar inertial odometry; a second processing module, used to select key frames based on the initial pose trajectory and millimeter-wave radar point cloud frames, perform scene recognition and geometric verification on all key frames to obtain closed-loop frame pairs, and combine a pose graph optimization algorithm to obtain an optimized key frame pose trajectory; a third processing module, used to construct a global mesh graph based on the optimized key frame pose trajectory, and construct point-to-point distance constraints for point pairs within each voxel in the global mesh graph that satisfy preset time and spatial constraints; a fourth processing module, used to combine the point-to-point distance constraints, the self-velocity constraints of the millimeter-wave radar frames, and the IMU pre-integration constraints into an objective function, and optimize the objective function by least squares to obtain the optimized key frame state variables; and a fifth processing module, used to obtain the optimized pose of all millimeter-wave radar frames by smoothing the pose graph based on the optimized key frame state variables, and construct a millimeter-wave radar point cloud graph based on the optimized pose of all millimeter-wave radar frames.

[0085] The 4D millimeter-wave radar mapping optimization system based on bundle adjustment provided in this invention addresses the problems of insufficient global consistency and accuracy of the generated global millimeter-wave radar point cloud map caused by the cumulative drift and errors in the initial pose provided by existing millimeter-wave radar odometry. It employs several modules to optimize the millimeter-wave radar frame pose and the global millimeter-wave radar point cloud map by fusing constraints from the 4D millimeter-wave radar and the IMU. Within the bundle adjustment framework, it integrates constraints from millimeter-wave radar observation and the inertial measurement unit to optimize the millimeter-wave radar frame pose and the overall point cloud. Optimizing the initial millimeter-wave radar pose significantly improves the accuracy and global consistency of the 4D millimeter-wave radar map.

[0086] Finally, it should be noted that the above specific embodiments are merely representative examples of the present invention. Obviously, the present invention is not limited to the above specific embodiments and many variations are possible. Any simple modifications, equivalent changes, and alterations made to the above specific embodiments based on the technical essence of the present invention should be considered within the protection scope of the present invention.

Claims

1. A 4D millimeter-wave radar mapping optimization method based on beam adjustment, characterized in that, include: Based on the acquired millimeter-wave radar frames and IMU data, the initial pose trajectory and the corresponding millimeter-wave radar point cloud frames of the static scene are obtained through millimeter-wave radar inertial odometry. Key frames are selected based on the initial pose trajectory and millimeter-wave radar point cloud frames. Scene recognition and geometric verification are performed on all key frames to obtain closed-loop frame pairs. The optimized key frame pose trajectory is obtained by combining the pose graph optimization algorithm. A global mesh graph is constructed based on the pose trajectory of the optimized keyframes. For point pairs in each voxel in the global mesh graph that satisfy the preset time and space constraints, point-to-point distance constraints are constructed. The point-to-point distance constraint, the self-velocity constraint of the millimeter-wave radar frame, and the IMU pre-integration constraint are combined into an objective function. The objective function is then solved and optimized using least squares to obtain the optimized keyframe state variables. Based on the optimized keyframe state variables, the poses of all millimeter-wave radar frames are obtained by smoothing the pose graph, and a millimeter-wave radar point cloud map is constructed based on the poses of all millimeter-wave radar frames.

2. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 1, characterized in that, Key frames are selected based on the initial pose trajectory and millimeter-wave radar point cloud frames, including: Construct a keyframe sliding window represented by a voxel mesh; Based on the initial pose trajectory, calculate the ratio of the number of voxels in the current millimeter-wave radar point cloud frame falling into the keyframe sliding window to the total number of voxels in the current millimeter-wave radar point cloud frame. If the ratio is lower than the set ratio threshold, the current frame is used as a keyframe and added to the keyframe sliding window; When the number of keyframe windows exceeds the set window limit, the first frame of the keyframe window will be removed.

3. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 1, characterized in that, Scene recognition and geometric verification are performed on all keyframes to obtain closed-loop frame pairs, including: Descriptors are calculated based on keyframes, and closed-loop frame pairs in the revisited scenario are identified based on the Euclidean distance corresponding to the descriptors. The relative poses of the two frames in the closed-loop frame pair are iteratively optimized. When the average distance between the matching points of the two frames is less than the set distance, a closed-loop constraint is formed.

4. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 1, characterized in that, The preset spatial constraint is that the voxel overlap of the two point clouds is greater than a set value for each voxel pair; the preset temporal constraint is that the time interval between the two point clouds is less than a set interval value for each voxel pair, or the time interval between the two point clouds and the two point clouds in a certain closed-loop frame pair is less than a set interval value.

5. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 1, characterized in that, The point-to-point distance constraint, the self-velocity constraint of the millimeter-wave radar frame, and the IMU pre-integration constraint are combined into an objective function, including: Based on the point-to-point distance constraints in each voxel, the geometric cost of point cloud matching in each voxel is obtained; The self-velocity residual of millimeter-wave radar is obtained based on the self-velocity constraint of millimeter-wave radar frames. The IMU pre-integration residual is obtained based on the IMU pre-integration constraint; The objective function is constructed based on the geometric cost of point cloud matching in each voxel, the millimeter-wave radar self-velocity residual, the IMU pre-integration residual, and the prior of the first keyframe pose and zero bias.

6. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 5, characterized in that, The expression for the objective function is: , in, Describe the objective function. The geometric cost is given by M, where M is the number of keyframes. for arrive IMU predicted factorization for The covariance matrix; For keyframes The self-velocity residual, For keyframes The covariance matrix of the self-velocity residuals; The pose prior constraints for the first keyframe. The covariance matrix of the pose prior constraints for the first keyframe; For IMU zero-biased prior, is the covariance matrix of the IMU zero-bias prior; the superscript T indicates matrix transpose, and the superscript -1 indicates matrix inversion.

7. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 5, characterized in that, The expression for the geometric cost is: , in, The geometric cost for a voxel. Let j be the state variable for keyframe j, and k be another keyframe. For the set of valid frame pairs, and They are from keyframes point and from keyframes point The covariance.

8. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 1, characterized in that, Based on the optimized keyframe state variables, the poses of all optimized millimeter-wave radar frames are obtained through pose graph smoothing, including: Based on the absolute pose constraints of keyframes and the relative pose constraints between adjacent frames obtained from millimeter-wave radar inertial odometry, a pose graph optimization problem is constructed. The pose of all millimeter-wave radar frames is obtained by solving the pose graph optimization problem through least squares iteration.

9. The 4D millimeter-wave radar mapping optimization method based on beam adjustment according to claim 1, characterized in that, The method further includes multiple iterations between the two steps of constructing point-to-point distance constraints and solving and optimizing the objective function.

10. A 4D millimeter-wave radar mapping optimization system based on beam adjustment, characterized in that, include: The first processing module is used to obtain the initial pose trajectory and the corresponding millimeter-wave radar point cloud frame of the static scene based on the acquired millimeter-wave radar frame and IMU data through millimeter-wave radar inertial odometry. The second processing module is used to select key frames based on the initial pose trajectory and millimeter-wave radar point cloud frames, perform scene recognition and geometric verification on all key frames to obtain closed-loop frame pairs, and combine the pose graph optimization algorithm to obtain the optimized key frame pose trajectory. The third processing module is used to construct a global mesh map based on the pose trajectory of the optimized keyframes, and to construct point-to-point distance constraints for point pairs within each voxel in the global mesh map that satisfy preset time and space constraints. The fourth processing module is used to combine the point-to-point distance constraint, the self-velocity constraint of the millimeter-wave radar frame, and the IMU pre-integration constraint into an objective function, and to optimize the objective function by least squares to obtain the optimized key frame state variables. The fifth processing module is used to obtain the pose of all millimeter-wave radar frames after optimization by smoothing the pose graph based on the optimized keyframe state variables, and to construct a millimeter-wave radar point cloud map based on the pose of all millimeter-wave radar frames after optimization.

Images