Positioning and mapping method and system based on fusion of 4d millimeter wave radar and imu

Abstract

This invention relates to a localization and mapping method and system based on the fusion of 4D millimeter-wave radar and IMU. The method first uses a point cloud preprocessing algorithm to denoise the 4D point cloud data obtained from the 4D millimeter-wave radar to obtain stable 4D point cloud data. Then, it calculates the three-dimensional volume velocity of the self-platform based on the stable 4D point cloud data. Next, it fuses the stable 4D point cloud data with the pose data from the inertial navigation system to optimize and calculate the odometry information of the self-platform. Finally, it fuses the odometry information and the three-dimensional volume velocity to optimize and calculate the optimal odometry information. Simultaneously, it draws an environmental point cloud map based on the stable 4D point cloud data and the optimal odometry information. The localization and mapping method and system based on the fusion of 4D millimeter-wave radar and IMU of this invention have the characteristics of wide environmental perception, low cost, and good real-time performance.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and environmental perception technology for unmanned systems, and in particular to a positioning and mapping method and system based on the fusion of 4D millimeter-wave radar and IMU. Background Technology

[0002] SLAM, or Simultaneous Localization and Mapping, is a technology that enables robots or autonomous vehicles to autonomously locate themselves in unknown environments and build maps of their surroundings. An IMU (Inertial Measurement Unit) is a sensor device that integrates an accelerometer and a gyroscope. It measures dynamic information such as an object's acceleration, angular velocity, and attitude. IMUs can provide high-frequency attitude updates, which are crucial for the localization of fast-moving robots or vehicles.

[0003] Traditional SLAM systems primarily rely on lidar for environmental perception and distance measurement. LiDAR can provide high-precision distance data, which, combined with attitude and motion information from the IMU, can achieve relatively accurate SLAM localization and map building.

[0004] However, lidar is not only expensive, but it also cannot provide accurate data for certain environments, such as transparent or reflective surfaces. The amount of data drops sharply in occlusion environments like rain and snow, leading to incomplete or inaccurate maps obtained using SLAM. Furthermore, existing SLAM methods suffer from computational and processing delays when dealing with large-scale environments, resulting in poor real-time performance. Summary of the Invention

[0005] Therefore, it is necessary to provide a localization and mapping method and system based on the fusion of 4D millimeter-wave radar and IMU that has wide environmental perception, low cost and good real-time performance to address the above-mentioned technical problems.

[0006] In a first aspect, the present invention provides a localization and mapping method based on the fusion of 4D millimeter-wave radar and IMU, comprising the following steps:

[0007] The 4D point cloud data obtained by 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data.

[0008] The three-dimensional velocity of the self-platform is calculated based on stable 4D point cloud data;

[0009] By integrating stable 4D point cloud data and pose data from inertial navigation devices, the odometry information of the self-platform is optimized and calculated.

[0010] By integrating odometry information and 3D volume velocity from its own platform, the optimal odometry information is calculated and optimized. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometry information.

[0011] In one embodiment, the three-dimensional volume velocity of the self-platform is calculated based on stable 4D point cloud data, including:

[0012] A least squares solution model is constructed using multidimensional information from stable 4D point cloud data.

[0013] The three-dimensional volume velocity of the self-platform is obtained by solving the least squares model using the linear least squares solution method.

[0014] In one embodiment, the fusion of stable 4D point cloud data, pose data from inertial navigation devices, odometry information from the fusion self-platform, and 3D volume velocity is performed using a graph optimization model in a sliding window manner.

[0015] In one embodiment, the least squares solution model is:

[0016]

[0017] In the formula, This is the measured Doppler velocity, where N is the total number of measurements, r represents the radar coordinate system, and v... r Indicates radar speed.

[0018] In one embodiment, the factor modeling results of the graph optimization model include IMU pre-integration factors, odometry factors, and velocity prior factors.

[0019] In one embodiment, the IMU pre-integration factor is:

[0020]

[0021] In the formula, the operator vec(·) is used to extract the vector part of the quaternion, and m represents the m-th frame. Represents a rotation matrix. b represents the system's position, velocity, and attitude states, respectively. a,m b g,m This represents the bias of the accelerometer and gyroscope. Represents the pre-integral quantity, g w Represents the gravitational parameter, Δτ m The parameters are Gaussian white noise.

[0022] In one embodiment, the odometer factor is

[0023]

[0024] In the formula, The quaternion multiplication operator is vec(·), which extracts the vector part of a quaternion. Let Δp represent the inverse pose and the predicted pose of frame m, respectively. m and These represent the change in location and the predicted change in location, respectively.

[0025] In one embodiment, the velocity prior factor is:

[0026]

[0027] In the formula, v radar It is a priori velocity estimate. It's an estimated speed.

[0028] Secondly, the present invention also provides a positioning and mapping device based on the fusion of 4D millimeter-wave radar and IMU. The device includes:

[0029] The point cloud preprocessing module is used to denoise the 4D point cloud data obtained by the 4D millimeter-wave radar through the point cloud preprocessing algorithm to obtain stable 4D point cloud data.

[0030] The radar self-motion estimation module is used to calculate the three-dimensional volume velocity of the self-platform based on stable 4D point cloud data.

[0031] The pose calculation module is used to fuse stable 4D point cloud data and pose data from inertial navigation devices to optimize and calculate the odometry information of the self-platform.

[0032] The graph-optimized multi-sensor fusion SLAM module is used to fuse odometry information and 3D volume velocity from its own platform, optimize and calculate the optimal odometry information, and draw an environmental point cloud map based on stable 4D point cloud data and the optimal odometry information.

[0033] The beneficial effects of this invention are:

[0034] (1) This invention combines 4D millimeter-wave radar with an IMU, utilizing the high-precision range information of the 4D millimeter-wave radar and the high-frequency attitude information of the IMU to achieve accurate localization and map building. The 4D millimeter-wave radar can provide accurate environmental perception and obstacle detection, while the IMU can provide continuous attitude updates. By fusing the two, the limitations of each can be overcome, the robustness and accuracy of the SLAM system can be improved, and the real-time performance of the SLAM system can be increased.

[0035] (2) 4D millimeter-wave radar is inexpensive, which can reduce the cost of the entire SLAM system. It also has good measurement capabilities for transparent and reflective surfaces and provides ample data even in obstructed environments such as rain and snow, ensuring the integrity and accuracy of the final mapping of the SLAM system. Attached Figure Description

[0036] Figure 1 This is a flowchart illustrating a localization and mapping method based on the fusion of 4D millimeter-wave radar and IMU provided in an embodiment of the present invention.

[0037] Figure 2 This is a flowchart illustrating another localization and mapping method based on the fusion of 4D millimeter-wave radar and IMU provided in an embodiment of the present invention.

[0038] Figure 3 This is a schematic diagram of the multi-sensor fusion SLAM module structure based on graph optimization provided in an embodiment of the present invention. Detailed Implementation

[0039] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0040] In one embodiment, such as Figure 1 As shown, Figure 1 This is one of the flowcharts illustrating the localization and mapping method based on 4D millimeter-wave radar and IMU fusion provided in this embodiment of the invention. The localization and mapping method based on 4D millimeter-wave radar and IMU fusion in this embodiment includes the following steps:

[0041] S101. The 4D point cloud data obtained by the 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data.

[0042] Specifically, the 4D point cloud data obtained by 4D millimeter-wave radar contains noisy point clouds, which can be removed using point cloud preprocessing algorithms. It should be noted that point cloud preprocessing algorithms are well-known to those skilled in the art and will not be elaborated upon here.

[0043] S102. Calculate the 3D volume velocity of the self-platform based on stable 4D point cloud data. The 3D volume velocity of the self-platform is based on a single radar scan, each scan consisting of 3D points in the radar frame and the corresponding velocity obtained from the Doppler frequency shift. The self-platform can be, but is not limited to, drones and unmanned vehicles.

[0044] The 3D volume velocity of the self-platform can be estimated based on the least squares solution.

[0045]

[0046] Wherein, given the target position p r The direction r is obtained through normalization. r The measured Doppler velocity It is the direction r r With radar speed v r scalar product:

[0047]

[0048] S103 integrates stable 4D point cloud data and pose data from inertial navigation devices to optimize and calculate the odometry information of its own platform.

[0049] S104. By integrating the odometer information and 3D volume velocity from its own platform, the optimal odometer information is calculated and optimized. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information.

[0050] This embodiment presents a localization and mapping method based on the fusion of 4D millimeter-wave radar and IMU. By combining 4D millimeter-wave radar with IMU, it utilizes the high-precision distance information of 4D millimeter-wave radar and the high-frequency attitude information of IMU to achieve accurate localization and map construction. 4D millimeter-wave radar provides accurate environmental perception and obstacle detection, while IMU provides continuous attitude updates. By fusing the two, their respective limitations can be overcome, improving the robustness and accuracy of the SLAM system and enhancing the real-time performance of the SLAM method. Furthermore, 4D millimeter-wave radar is inexpensive, reducing the overall cost of the SLAM system, and has good measurement capabilities for transparent and reflective surfaces. It also provides ample data even in occluded environments such as rain and snow, ensuring the completeness and accuracy of the final map constructed by the SLAM system.

[0051] In one embodiment, such as Figure 2 As shown, Figure 2 This is a flowchart illustrating another localization and mapping method based on 4D millimeter-wave radar and IMU fusion provided in this embodiment of the invention. This embodiment involves how to calculate the three-dimensional volume velocity of the self-platform based on stable 4D point cloud data. Based on the above embodiment, step S102 includes:

[0052] S201. Construct a least squares solution model using multidimensional information from stable 4D point cloud data.

[0053] Specifically, the least squares solution model is as follows:

[0054]

[0055] In the formula, This is the measured Doppler velocity, where N is the total number of measurements, r represents the radar coordinate system, and v... r Indicates radar speed.

[0056] S202. The three-dimensional velocity of the self-platform is obtained by solving the least squares model using the linear least squares solution method.

[0057] Using the linear least squares (LSQ) solution, we can obtain the following solution:

[0058]

[0059] In the formula, To obtain the three-dimensional velocity of the platform, the H matrix is ​​the least squares solution model containing r. x,N r y,N r z,N The matrix, y r For the measured Doppler velocity

[0060] Solving this least-squares problem using any measurement method is prone to errors. The environment cannot be assumed to be static, so outliers caused by noise, reflections, or ghosting must be removed; the aforementioned point cloud preprocessing is to address this issue.

[0061] In one embodiment, the fusion of stable 4D point cloud data, pose data from inertial navigation devices, odometry information from the fusion platform, and 3D volume velocity is performed using a graph optimization model in a sliding window manner.

[0062] Specifically, such as Figure 3 As shown, Figure 3 The diagram below shows the structure of a graph-optimized multi-sensor fusion SLAM module provided in this embodiment of the invention. Stable 4D point cloud data, pose data of inertial navigation devices, and three-dimensional volume velocity can be fused in a sliding window manner within a multi-sensor fusion SLAM framework.

[0063] During the fusion process, the states contained in the sliding window at time t are defined as follows: in The active IMU state within a sliding window over time t. t Let t represent the set of IMU measurements at point t.

[0064] IMU status is

[0065]

[0066] In the formula, It is a unit quaternion representing the rotation from world frame {w} to IMU frame {I}. wT and v wTThese are the IMU position and velocity, b g b a These are the random walk biases of the gyroscope and accelerometer, respectively.

[0067] Using the definition of state, the goal is to minimize the cost function of residuals generated by different measurements in the above equation.

[0068]

[0069] The first term is the residual based on the IMU, r I,m Define the measurement residual between frames m and m+1. The second term is the odometer measurement residual, and the last term is the velocity residual. t V and Σ are a set of measurements within a sliding window at time t. i and Σ j These are the covariances of the two measurements.

[0070] The optimized solution after fusion is typically solved using an iterative least squares solver with linear approximation. In this embodiment, the solution is specifically, but not limited to, based on GTSAM.

[0071] In an optional embodiment, the factor modeling results of the graph optimization model include IMU pre-integration factors, odometry factors, and velocity prior factors.

[0072] In one embodiment, the IMU pre-integration factor is:

[0073]

[0074] In the formula, the operator vec(·) is used to extract the vector part of the quaternion, and m represents the m-th frame. Represents a rotation matrix. b represents the system's position, velocity, and attitude states, respectively. a,m b g,m This represents the bias of the accelerometer and gyroscope. Represents the pre-integral quantity, g w Represents the gravitational parameter, Δτ m The parameters are Gaussian white noise.

[0075] Specifically, the IMU pre-integration factor r I,m This includes the error term containing relative motion constraints between radar keyframes. By using IMU pre-integration, based on the known IMU state variables from the previous moment, the linear acceleration and angular velocity measured by the IMU are integrated to obtain the current state variables. Ultimately, this enables the acquisition of odometry information at the IMU frequency from the IMU data, based on the radar pose obtained through point cloud matching.

[0076] In one embodiment, the odometer factor is

[0077]

[0078] In the formula, The quaternion multiplication operator is vec(·), which extracts the vector part of a quaternion. Let Δp represent the inverse pose and the predicted pose of frame m, respectively. m and These represent the change in location and the predicted change in location, respectively.

[0079] In one embodiment, the velocity prior factor is:

[0080]

[0081] In the formula, v radar It is a priori velocity estimate. It's an estimated speed.

[0082] Specifically, the velocity prior factor restricts the estimation of robot velocity to improve the robustness of SLAM.

[0083] It should be noted that the sensor of the present invention is based on 4D millimeter-wave radar and inertial measurement elements, and can be installed on vehicle / drone platforms to provide the platform with accurate positioning information and point cloud maps.

[0084] Based on the same inventive concept, this invention also provides a positioning and mapping device based on the fusion of 4D millimeter-wave radar and IMU. The device includes:

[0085] The point cloud preprocessing module is used to denoise the 4D point cloud data obtained by the 4D millimeter-wave radar using a point cloud preprocessing algorithm to obtain stable 4D point cloud data. The input of the point cloud preprocessing module is a ROS point cloud data type from the radar point cloud driver, and the output is a ROS point cloud data type, which is sent to the NDT point cloud matching module and the radar self-motion estimation module.

[0086] Specifically, the data flow of the point cloud preprocessing module is shown in Table 1.

[0087] Table 1 Data Flow Table of Point Cloud Preprocessing Module

[0088]

[0089]

[0090] The radar self-motion estimation module is used to calculate the 3D volumetric velocity of the self-platform based on stable 4D point cloud data. This module calculates the radar's own velocity using Doppler velocity information from the 4D millimeter-wave radar point cloud. The input is ROS point cloud data processed by the point cloud preprocessing module, and the output is the radar's current instantaneous velocity. The velocity result is then sent to GTSMAM.

[0091] Specifically, the data flow of the radar self-motion estimation module is shown in Table 2.

[0092] Table 2 Data Flow Table for Radar Motion Estimation Module

[0093]

[0094] The pose calculation module fuses stable 4D point cloud data and pose data from inertial navigation devices to optimize and calculate the odometry information of the self-platform. The input is ROS point cloud data processed by the point cloud preprocessing module, and the output is the radar's current six-DOF pose and a global point cloud map projected onto the world coordinate system. The pose result is then sent to GTSMAM.

[0095] Specifically, the data flow of the pose calculation module is shown in Table 3.

[0096] Table 3 Pose Calculation Module

[0097]

[0098]

[0099] The graph-optimized multi-sensor fusion SLAM module is used to fuse odometry information and 3D volume velocity from its own platform, optimize and calculate the optimal odometry information, and draw an environmental point cloud map based on stable 4D point cloud data and the optimal odometry information.

[0100] Specifically, the data flow of the graph-optimized multi-sensor fusion SLAM module is shown in Table 4.

[0101] Table 4. Data Flow Table for Graph-Optimized Millimeter-Wave Radar and IMU Fusion SLAM Algorithm

[0102]

[0103] The localization and mapping system based on the fusion of 4D millimeter-wave radar and IMU in this embodiment overcomes the limitations of each other by integrating the two, improving the robustness and accuracy of the SLAM system and increasing its real-time performance. Furthermore, the low cost of 4D millimeter-wave radar reduces the overall cost of the SLAM system.

[0104] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A localization and mapping method based on the fusion of 4D millimeter-wave radar and IMU, characterized in that, Includes the following steps: The 4D point cloud data obtained by 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data. The three-dimensional velocity of the self-platform is calculated based on the stable 4D point cloud data. By integrating the stable 4D point cloud data and the pose data of the inertial navigation device, the odometry information of the self-platform is optimized and calculated. By integrating the odometer information and 3D volume velocity of the self-platform, the optimal odometer information is calculated and optimized. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information. The fusion of the stable 4D point cloud data, the pose data of the inertial navigation device, the odometry information and 3D volume velocity of the self-platform are all performed using a graph optimization model in a sliding window manner. The factor modeling results of the graph optimization model include IMU pre-integration factor, odometry factor, and velocity prior factor. The IMU pre-integration factor is 　　　（2） In the formula, the operator Used to extract the vector portion of a quaternion, where m represents the m-th frame. Represents a rotation matrix. , , These represent the system's position, velocity, and attitude states, respectively. , This represents the bias of the accelerometer and gyroscope. , , Represents the pre-integral quantity, Represents gravity parameters, The parameters are Gaussian white noise; The odometer factor is 　　　（3） In the formula, This represents the quaternion multiplication operator. Used to extract the vector part of a quaternion. , Let represent the inverse pose and the predicted pose of frame m, respectively. and These represent the change in location and the predicted change in location, respectively. The velocity prior factor is 　　　（4） In the formula, It is a priori velocity estimate. It's an estimated speed.

2. The positioning and mapping method based on 4D millimeter-wave radar and IMU fusion according to claim 1, characterized in that, The three-dimensional volume velocity of the self-platform is calculated based on the stable 4D point cloud data, including: A least squares solution model is constructed using multidimensional information from stable 4D point cloud data. The three-dimensional volume velocity of the self-platform is obtained by solving the least squares model using the linear least squares solution method.

3. The positioning and mapping method based on 4D millimeter-wave radar and IMU fusion according to claim 2, characterized in that, The least squares solution model is as follows: 　　　（1） In the formula, This is the measured Doppler velocity, where N is the total number of measurements. Indicates the radar coordinate system. Indicates radar speed.

4. A positioning and mapping system based on 4D millimeter-wave radar and IMU fusion, used to execute the method as described in any one of claims 1 to 3, characterized in that, The system includes: The point cloud preprocessing module is used to denoise the 4D point cloud data obtained by the 4D millimeter-wave radar through the point cloud preprocessing algorithm to obtain stable 4D point cloud data. The radar self-motion estimation module is used to calculate the three-dimensional volume velocity of the self-platform based on the stable 4D point cloud data. The pose calculation module is used to fuse the stable 4D point cloud data and the pose data of the inertial navigation device to optimize and calculate the odometry information of the self-platform. A graph-optimized multi-sensor fusion SLAM module is used to fuse odometry information and 3D volume velocity of the self-platform, optimize and calculate the optimal odometry information, and draw an environmental point cloud map based on stable 4D point cloud data and the optimal odometry information.

Images